lkml.org 
[lkml]   [2001]   [Oct]   [9]   [last100]   RSS Feed
Views: [wrap][no wrap]   [headers]  [forward] 
 
Messages in this thread
/
From
Subjectdagens retur
Date

--
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)
\
 
 \ /
  Last update: 2005-03-22 13:05    [W:0.037 / U:1.260 seconds]
©2003-2020 Jasper Spaans|hosted at Digital Ocean and TransIP|Read the blog|Advertise on this site