Messages in this thread | | | From | Roger Larsson <> | Subject | dagens retur | Date | Tue, 9 Oct 2001 23:24:33 +0200 |
| |
-- Roger Larsson Skellefteå Sweden # TODO: window positioning from thread import * import random import struct from crc8 import crc8 as checksum import time import string
# used in debug only def y_test((_d1,y1,_d2), (_d3,y2,_d4)): if y1 < y2: return -1 elif y1 == y2: return 0 else: return 1
class VMSRestarted(IOError): pass
class VMSError(IOError):
error_code = 0
def __init__(self, error_code): self.error_code = error_code
class VMS: cs = allocate_lock()
state = 0 state_text = ('ready', 'measuring', 'transfer calibration', 'laser operations')
serial_no = (2*10)*'00' test = {} # self.test results
fd = 0 _image_id_tag = int(time.time()) image_files = ['Foggy1.jpg']
def get_image_data(self, types): self._state(1) image_list = self._get_image_data(types) self._state(0) return image_list
def get_image_files(self, types): self._state(1) image_list = self._get_image_data(types) files = [] for (id, file_type, data) in image_list: filename = tempfile.mktemp("." + file_type) wfd = os.open(filename, "wb") wfd.write(data) wfd.close() files.append(filename)
self.image_files = files self._state(0)
return files
def measure(self, laser): self._state(1) self.laser_on(laser) self._request_measure() (used_laser, measured) = self._read_measure() assert used_laser == laser, "VMS: Measure data arriving from wrong laser (%d, expected %d)" % (used_laser, laser) self.laser_off(laser) self._state(0) return measured
def raw_measure(self, laser): self._state(1) self.laser_on(laser) self._request_object_list() (used_laser, measured) = self._read_object_list() assert used_laser == laser, "VMS: Measure data arriving from wrong laser (%d, expected %d)" % (used_laser, laser) self._read_stop_lists() # always follows a object list self.laser_off(laser) self._state(0) return measured
def ready(self): return state == 0
def set_calibration(self, laser, x_calib_params, y_calib_params): self._state(2) # TODO self._state(0) return
def get_calibration(self, laser): self._state(2) # TODO self._state(0) return 11*(laser,)
def modes(self): return [1, 2] # Lasers
def laser_on(self, laser): save_state = self._state(3) assert laser in self.modes(), "Error: trying to lit laser %d" % laser self._set_laser(laser) self._state(save_state) print "Laser ", laser, " on"
def laser_off(self, laser): save_state = self._state(3) self._set_laser(0) self._state(save_state) print "Laser ", laser, " off"
################################################################ # Protocol functions #
# # 1: Upload image # _formats = ['', 'raw', 'gif', 'jpg'] _types = ['', 'Binary laser image', 'Binary background image', 'Grayscale laser image', 'Grayscale background image'] _types_formats = (0, 1, 1, 3, 3)
def _get_images(self, request_list, format_list=[], expose=1, count=1): """ Example: _request_images([(52, 'Grayscale laser image')]) returns: [(52, 'JPEG', ...)] """ data = struct.pack("BB", expose, count)
# make format_list equal lengt of request_list if len(format_list) < len(request_list): format_list += (len(request_list) - len(format_list)) * ['']
### BUG in Python? #for (id, t), f in request_list, format_list: request_list = map((lambda (id, t), f: (id, t, f)), request_list, format_list) binary_request_list = [] for (id, t, f) in request_list: # convert to ints t = self._types.index(t)
if f == '': f = self._types_formats[t] else: f = self._formats.index(f)
binary_request_list.append( (id, t, f) )
# pack data += struct.pack("BBB", t, id, f)
self._write_msg(1, data)
images = [] for (id, _, f) in binary_request_list: images.append( (id, self._formats[f], self._read_image(id)) )
return images
def _read_image(self, id): no = 0 packet = self._read_packet(id, no) while packet.len() > 0: # each image ends with an empty packet data += packet no += 1 packet = self._read_packet(id, no) return data
def _read_packet(self, id, no): packet = self._read_msg(1) (read_id, read_no) = struct.unpack("BB", packet[0:2]) assert read_id == id, "VMS: Identity mismatch, expected %d got %d" % (id, read_id) assert read_no == no, "VMS: Number mismatch, expected %d got %d" % (no, read_no)
return packet[2:]
# # 4: Set laser line on/off # def _set_laser(self): data = struct.pack("B", self._laser) self._write_msg(4, data) data = self._read_msg(4)
# # 10: Lists (X,Y,width) # def _request_measure(self, no_of_lists=1): """ no_of_lists == 0: means continously """ data = struct.pack("B", no_of_lists) self._write_msg(10, data)
def _read_measure(self): """ read object list """ (type, len, data) = self._read_msg(10) laser = ord(data[0]) measured = [] for ix in range(1, len, 5): object = struct.unpack("<hhB", data[ix:ix+5]) measured.append(object)
return (laser, measured)
# # 11: Stop lists # def _request_stop_lists(self): self._write_msg(11)
def _read_stop_lists(self): self._read_msg(11)
# # 16: Self test # def _perform_self_test(self): self._write_msg(16) (type, len, data) = self._read_msg(16) _decode_self_test(data)
def _decode_self_test(self, data): # Self test data might come on watchdog restart (self.test['status']) = struct.unpack("<H", data[0:2]) self.test['protocol'] = struct.unpack("BB", data[2:4]) self.test['FPGA'] = struct.unpack("BB", data[4:6]) self.test['DSP'] = struct.unpack("BB", data[6:8]) self.serial_no = reduce((lambda s, c: s + "%02x" % ord(c)), struct.unpack("10B", data[8:8+10]))
# # 64: Blob offset # def _request_object_list(self, repeats=1, offset=0): data = struct.pack("BB", repeats, offset) self._write_msg(64, data) # Note: always followed by Stop list for some reason.
def _read_object_list(self): (type, len, data) = self._read_msg(64)
# ignore blob offset and total blob count laser = ord(data[2])
measured = [] for ix in range(3, len, 5): object = struct.unpack("<hhB", data[ix:ix+5]) measured.append(object)
return (laser, measured)
################################################################# # Communication helper functions #
def _write_msg(self, type, data=""): build = struct.pack("BBB", 255, type, len(data)) build += data build += chr(checksum(build)) print "VMS write", type self.fd.write(build) self.fd.flush()
def _read_msg(self, expected_type=0): sync = struct.unpack("B", self.fd.read(1)) if sync != 255: print "Warning: Sync lost! resyncing (got", sync, ")" while sync != 255: sync = struct.unpack("B", self.fd.read(1))
(type, len) = struct.unpack("BB", self.fd.read(2))
data = self.fd.read(len)
csum = struct.unpack("B", self.fd.read(1)) if csum != checksum(struct.pack("BBB", 255, type, len) + data): raise IOError
if type != expected_type: if type == 16: # self test data, spontaneous restart! print "Warning: VMS restarted spontaneously!" self._decode_self_test(data) raise VMSRestarted if type == 25: error_code = struct.unpack("<H", data) print "VMS Error message:", error_code raise VMSError(error_code)
else: print "Read unexpected message (type=%d, len=%d)" % (type, len) raise IOError
print "VMS read", type, len return (type, len, data)
# # Webb interface # def html_action(self, action, query): if action == "OK": pass # for assignment in query.split('&'): # (var, value) = assignment.split('=') # if var != "action": # exec "self." + var + "= int(value)" elif action == "REFRESH": pass elif action == "GRAB": self.get_image_files(['Binary laser image', 'Binary background image', 'Grayscale laser image', 'Grayscale background image']) # ignore returned values elif action == "": pass # try: # self.settings_action("GOTO", query) # except AttributeError: # self.settings_action("OK", query) else: raise unknown_action
def html_page(self, f): f.write('<form>') f.write('<H2><br>Currently ' + self.state_text[self.state] + '<br></H2>') f.write('</form>')
f.write('<hr>') f.write('<form>') f.write('<H2>Actions</H2>') f.write('<INPUT TYPE=submit NAME=action VALUE="REFRESH">') f.write('<INPUT TYPE=submit NAME=action VALUE="GRAB">') f.write('</form>')
f.write('<br><hr><form>') f.write('<H2>Constants</H2>') f.write('Serian no: 0x' + self.serial_no) f.write('<br>Self test: ' + str(self.test)) f.write('</form>')
image_files = self.image_files if image_files: f.write('<hr>') for file in image_files: f.write('<img src="static?%s" width="105" height="140">' % file)
# # Other helper functions # def __init__(self, fd): self.fd = fd
def _state(self, state): """ Set new state, return old (internal use) """ self.cs.acquire() save_state = self.state self.state = state self.cs.release() return save_state
def _next_id_tag(self): self.cs.acquire() tag = self._image_id_tag = (self._image_id_tag + 1) % 256 self.cs.release() return tag
def _get_image_data(self, types): request_list = [] for t in types: tag = self._next_id_tag() request_list.append( (tag, t) )
image_list = self._get_images(request_list) return image_list
class SimulatedVMS(VMS):
def get_image_files(self, types): self.image_files = ['Foggy1.jpg', 'Marble01.jpg'] return self.image_files
def measure(self, laser): self.laser_on(laser) measured = [] for item in range(10): x_mm = 20 * item y_mm = 52 + random.randrange(-5, 5) if item == 4: w_mm = random.randrange(9,11) else: w_mm = random.randrange(6,8)
measured.append((x_mm,y_mm,w_mm))
self.laser_off(laser) return measured
def raw_measure(self, laser): self.laser_on(laser) measured = [] for item in range(10): xpix = 10 * item + random.randrange(-1,1) ypix = 52 + random.randrange(-5, 5) if item == 4: wpix = random.randrange(30, 50) else: wpix = random.randrange(15, 30)
measured.append((xpix,ypix,wpix))
self.laser_off(laser) measured.sort(y_test)
return measured
def _set_laser(self, laser): pass
def _read_msg(self, expected_type=0):
if expected_type == 1: data = struct.pack("BB", 0, 0) elif expected_type == 4: data = "" elif expected_type == 10: data = struct.pack("B", 1) # list elif expected_type == 16: data = 18*'\0' # why not... :-) elif expected_type == 64: data = "" # TODO: implement else: print 'Warning: unexpected type', expected_type return self._read_msg(16)
data = struct.pack("BBB", 255, expected_type, len(data)) + data
return data from thread import * import os import random import re import string
class Calibrate: cs = allocate_lock() state = 0 state_heading = ("calibrating", "ready", "calibrated", "verifying", "verified") auto = 1
# pipes to octave process octave_in = 0 octave_out = 0
def ready(self): return self.state not in (1, 3)
# Protected by lock, keyed by mode raw_data = {} verify_mm = [] verify_data = {}
# Not protected by lock motor = None sensor = None
def __init__(self, motor, sensor): self.cs.acquire() self.motor = motor self.sensor = sensor if os.name == "posix": (self.octave_in, self.octave_out)=os.popen2('octave --silent --no-history --path :../Matlab:../Matlab/Octave') elif os.name == "nt": (self.octave_in, self.octave_out)=os.popen2(r'H:\PROGRAM\GNU\OCTAVE\\bin\bash.exe" --login -c "rxvt -e octave --silent --no-history --path :/cygdrive/c/vms/Matlab:/cygdrive/c/vms/Matlab/Octave') self.octave_in.write("gnuplot_has_frames=1;\n") self.octave_in.flush() self.cs.release()
def calibrate_range_mm(self): return range(self.motor.home_mm, self.motor.alt_mm, 10)
number_re = re.compile(r"([\-]?[\d]*[\.[\d]+]?)")
def calibrate_thread(self): print "calibrate_thread" self.cs.acquire()
assert(self.state != 1) self.state = 1
sensor = self.sensor modes = sensor.modes()
self.raw_data.clear() self.verify_data.clear()
self.statistics = {} for mode in modes: self.raw_data[mode] = [] self.statistics[mode] = {} self.cs.release()
# Always start measurements from home self.motor.home() self.motor.wait()
for x_mm in self.calibrate_range_mm(): self.motor.goto_mm(x_mm) self.motor.wait()
for mode in modes: self.cs.acquire() raw_data = sensor.raw_measure(mode) self.statistics[mode]['Y pix min'] = reduce((lambda m, (_d1, y, _d2): min(m, y)), raw_data, 9999) self.statistics[mode]['Y pix max'] = reduce((lambda m, (_d1, y, _d2): max(m, y)), raw_data, -1) raw_data.sort() # in X order, not aproximately Y... self.raw_data[mode].append( raw_data ) self.cs.release()
for mode in modes: self.cs.acquire() raw_data = self.raw_data[mode] self.cs.release()
write_octave_data(self.octave_in, self.calibrate_range_mm(), raw_data) self.octave_in.write("kalibrering\n") self.octave_in.flush() calib_params_x = read_octave_data(self.octave_out, "XCoeff:", self.number_re, 11) print "calib_params_x", calib_params_x calib_params_y = read_octave_data(self.octave_out, "YCoeff:", self.number_re, 11) print "calib_params_y", calib_params_y # Two empty lines self.octave_out.readline() self.octave_out.readline() # Four lines with statistics
self.statistics[mode]['X err mean'] = float(self.number_re.search(self.octave_out.readline()).group(0)) self.statistics[mode]['X err max'] = float(self.number_re.search(self.octave_out.readline()).group(0)) self.statistics[mode]['Y err mean'] = float(self.number_re.search(self.octave_out.readline()).group(0)) self.statistics[mode]['Y err max'] = float(self.number_re.search(self.octave_out.readline()).group(0)) print self.statistics
sensor.set_calibration(mode, calib_params_x, calib_params_y)
if not self.auto: self.cs.acquire() self.state = 2 self.cs.release() else: # auto continue, no not pass state 2 ! self.verify_thread()
def verify_thread(self): print "verify_thread" self.cs.acquire() self.state = 3
sensor = self.sensor modes = sensor.modes()
self.verify_data.clear() for mode in modes: self.verify_data[mode] = [] self.cs.release()
self.cs.acquire() self.verify_mm = [0] # aways verify at 0 mm for pos in range(3): self.verify_mm.append(random.randrange(self.motor.home_mm, self.motor.alt_mm))
self.verify_mm.sort() self.cs.release()
# Always start measurements from home self.motor.home() self.motor.wait()
for x_mm in self.verify_mm: self.motor.goto_mm(x_mm) self.motor.wait()
for mode in modes: self.cs.acquire() self.verify_data[mode].append(sensor.measure(mode)) self.cs.release()
self.cs.acquire() self.state = 4 self.cs.release()
def html_action(self, action, query): self.cs.acquire()
for assignment in query.split('&'): (var, value) = assignment.split('=') if var != "action": exec "self.set_" + var + "(value)"
if action == "CALIBRATE": if self.state in (0, 2, 4): start_new_thread(self.calibrate_thread,()) elif action == "VERIFY": if self.state in (0, 2, 4): start_new_thread(self.verify_thread,()) elif action == "REFRESH": pass self.cs.release()
def html_page(self, f): # self.cs.acquire()
if self.state in (1,2,3): f.write('<meta HTTP-EQUIV="REFRESH" CONTENT="1;">')
f.write('</HEAD>\n') # from HEAD to BODY f.write('<BODY>\n') f.write('<H1>Calibrate</H1><br>') f.write("<H2>Currently " + self.state_heading[self.state] + "</H2>")
# Actions f.write("<form action=Calibrate>") # TODO: make a form receptor page
if self.state not in (1,2,3): checked_text = ("", "CHECKED") f.write('<INPUT TYPE=radio NAME=auto VALUE="cvr" ' + checked_text[self.auto] + '>Auto calibrate and verify<BR>') f.write('<INPUT TYPE=radio NAME=auto VALUE="manual" ' + checked_text[1 - self.auto] + '>Manually select action and refresh<BR>') f.write('<INPUT TYPE=submit NAME=action VALUE="REFRESH">') if self.state in (0, 2, 4): f.write("<INPUT TYPE=submit NAME=action VALUE=\"CALIBRATE\">")
if self.state in (0, 2, 4): f.write("<INPUT TYPE=submit NAME=action VALUE=\"VERIFY\">") f.write("</form><hr>")
sensor = self.sensor modes = sensor.modes()
# Results if self.state in (0, 2, 4): f.write('<H3>Calib params</H3>')
for mode in modes: f.write(str(mode) + ': ' + str(sensor.get_calibration(mode)) + '<br>') f.write('<hr>')
if self.state >= 2: f.write('<TABLE border="1"<CAPTION><EM>Statistics</EM></CAPTION>') f.write('<TR><TH>') labels = self.statistics[modes[0]].keys() labels.sort() self.write_list(f, '<TH>', labels) for mode in modes: f.write('<TR><TH>' + str(mode)) dict = self.statistics[mode] values = map((lambda label, D=dict: D[label]), labels) self.write_list(f, '<TD>', values) f.write('</TABLE>')
if self.state >= 1: f.write('<TABLE border="1"<CAPTION><EM>Raw data</EM></CAPTION>') f.write('<TR><TH>mm') self.write_list(f, '<TH>', self.calibrate_range_mm()) for mode in modes: f.write('<TR><TH>' + str(mode)) self.write_list(f, '<TD>', self.raw_data[mode], (lambda sub: string.join(map(str, sub),',<br>'))) f.write('</TABLE>')
if self.state >= 3: f.write('<TABLE border="1"<CAPTION><EM>Verify data</EM></CAPTION>') f.write('<TR><TH>mm') self.write_list(f, '<TH>', self.verify_mm) for mode in modes: f.write('<TR><TH>' + str(mode)) self.write_list(f, '<TD>', self.verify_data[mode], (lambda sub: string.join(map(str, sub),',<br>')))
# self.cs.release()
def write_list(self, f, delimiter, item_list, item_formater=str): for item in item_list: f.write(delimiter) f.write(item_formater(item))
# eval methods def set_auto(self, mode): if mode=="cvr": self.auto = 1 elif mode == "manual": self.auto = 0 else: print "Warning: Unexpected automode:", mode self.auto = 0
import sys # used in debug code def write_octave_data(to, xs_mm, raw_data): # TODO remove if random.randrange(2): # 0 or 1 to.write("xycalib_pa_prototyp3\n") else: to.write("xycalib_pa_prototyp4\n") to.flush() to = sys.stdout
to.write("cal = [\n") for ix in range(len(xs_mm)): x_mm = xs_mm[ix] measured = raw_data[ix] y_pins = y_range( map((lambda (x,y,w): w), measured) ) # object at y_mm = 0 is wider. ys_mm = map((lambda y: 20*y), y_pins) for iy in range(len(ys_mm)): m = measured[iy] y_mm = ys_mm[iy] to.write("%d %d %d %d %d;\n" % (m[0], m[1], m[2], x_mm, y_mm)) to.write("];\n") to.flush() return
def read_octave_data(rd, label, rexp, lines): ret = [] read = rd.readline() while read.find(label) == -1: read = rd.readline() for ix in range(lines): read = rd.readline() print '<', read, '>' match = rexp.search(read) assert match, "Expected number not found in '" + str(read) +"'" ret.append(float(match.group(0)))
return ret
def y_range(ls): (rd, rl) = y_range_(ls[0], 1, ls[1:]) return [rd] + rl
def y_range_(max, maxd, ls): if ls == []: return (maxd-1, []) else: if ls[0] > max: (rd, rl) = y_range_(ls[0], 1, ls[1:]) else: (rd, rl) = y_range_(max, maxd+1, ls[1:]) return (rd-1, [rd] + rl)
| |