295 lines
9.1 KiB
Python
295 lines
9.1 KiB
Python
###################################################################################################
|
|
# Deployment specific global definitions - executed after startup.py
|
|
###################################################################################################
|
|
|
|
|
|
from ch.psi.pshell.serial import TcpDevice
|
|
from ch.psi.pshell.modbus import ModbusTCP
|
|
|
|
|
|
class RobotTCP(TcpDevice):
|
|
def __init__(self, name, server, timeout = 500, retries = 1):
|
|
TcpDevice.__init__(self, name, server)
|
|
self.timeout = timeout
|
|
self.retries = retries
|
|
self.header = None
|
|
self.trailer = "\n"
|
|
self.array_separator = '|'
|
|
self.cmd_separator = ' '
|
|
self.msg_id = 0
|
|
|
|
def _sendReceive(self, msg_id, msg = ""):
|
|
tx = self.header if (self.header != None) else ""
|
|
tx = tx + msg_id + " " + msg
|
|
if (len(tx)>127):
|
|
raise Exception("Exceeded maximum message size")
|
|
self.getLogger().finer("TX = '" + str(tx)+ "'")
|
|
if (self.trailer != None): tx = tx + self.trailer
|
|
rx = self.sendReceive(tx, None, self.trailer , 0, self.timeout, self.retries)
|
|
rx=rx[:-1] #Remove 0A
|
|
self.getLogger().finer("RX = '" + str(rx) + "'")
|
|
if rx[:3] != msg_id:
|
|
print rx
|
|
raise Exception("Received invalid message id: " + str(rx[:3]) + " - expecting:" + msg_id )
|
|
if len(rx)<4:
|
|
raise Exception("Invalid message size: " + str(len(rx)) )
|
|
if rx[3] == "*":
|
|
raise Exception(rx[4:])
|
|
return rx[4:]
|
|
|
|
def call(self, msg):
|
|
id = "%03d" % self.msg_id
|
|
self.msg_id = (self.msg_id+1)%1000
|
|
return self._sendReceive(id, msg)
|
|
|
|
def execute(self, command, *argv):
|
|
msg = str(command)
|
|
if len(argv)>10:
|
|
raise Exception("Exceeded maximum number of parameters")
|
|
for i in range(len(argv)):
|
|
msg += (self.cmd_separator if (i==0) else self.array_separator) + str(argv[i])
|
|
rx = self.call(msg)
|
|
if rx.count(self.array_separator)>0:
|
|
return rx.split(self.array_separator)
|
|
return rx
|
|
|
|
def read_event(self):
|
|
ev = self._sendReceive("EVT")
|
|
if ev.strip() == "": return None
|
|
return ev
|
|
|
|
def evaluate(self, cmd):
|
|
ret = self.execute('eval', cmd)
|
|
if ret.strip() != "": raise Exception(ret)
|
|
|
|
def get_var(self, name):
|
|
return self.execute('get_var', name)
|
|
|
|
def get_varb(self, name):
|
|
return True if (self.execute('get_bool', name).strip() == '1') else False
|
|
|
|
def get_arr(self, name, size):
|
|
return self.execute('get_arr', name, size)
|
|
|
|
|
|
def get_int(self):
|
|
return int(self.get_var("n"))
|
|
|
|
def get_float(self):
|
|
return float(self.get_var("n"))
|
|
|
|
def get_bool(self, name):
|
|
return self.get_varb("b")
|
|
|
|
def get_int_arr(self, size):
|
|
# not working. A Jython bug in PyUnicaode?
|
|
#return [int(x) for x in self.get_arr("arr", size)]
|
|
ret = []
|
|
a=self.get_arr("arr", size)
|
|
for i in range(size):
|
|
ret.append(int(a[i]))
|
|
return ret
|
|
|
|
def get_float_arr(self, size):
|
|
#return [float(x) for x in self.get_arr("arr", size)]
|
|
ret = []
|
|
a=self.get_arr("arr", size)
|
|
for i in range(size):
|
|
ret.append(int(a[i]))
|
|
return ret
|
|
|
|
|
|
def eval_int(self, cmd):
|
|
self.evaluate("n=" + cmd)
|
|
return self.get_int()
|
|
|
|
def eval_float(self, cmd):
|
|
self.evaluate("n=" + cmd)
|
|
return self.get_float()
|
|
|
|
def eval_bool(self, cmd):
|
|
self.evaluate("b=" + cmd)
|
|
return self.get_bool(self)
|
|
|
|
|
|
def get_move_id(self):
|
|
return self.eval_int("getMoveId()")
|
|
|
|
def get_joint_forces(self):
|
|
robot_tcp.evaluate("getJointForce(arr)")
|
|
return robot_tcp.get_float_arr(6)
|
|
|
|
def mount(self, puck, sample):
|
|
return self.execute('mount', puck, sample)
|
|
|
|
|
|
def is_powered(self):
|
|
return self.eval_bool("isPowered()")
|
|
|
|
def enable(self):
|
|
return self.evaluate("enablePower()")
|
|
|
|
def disable(self):
|
|
return self.evaluate("disablePower()")
|
|
|
|
def doUpdate(self):
|
|
ev = None
|
|
try:
|
|
ev = self.read_event()
|
|
self.setState(State.Ready)
|
|
except:
|
|
self.setState(State.Offline)
|
|
if ev is not None:
|
|
self.getLogger().info(ev)
|
|
|
|
add_device(RobotTCP("robot_tcp", "129.129.126.100:1000"), force = True)
|
|
robot_tcp.setPolling(500)
|
|
|
|
|
|
|
|
class RobotModbus(DeviceBase):
|
|
def __init__(self, name):
|
|
DeviceBase.__init__(self, name)
|
|
robot_req.write(0)
|
|
|
|
def execute(self, command, *argv):
|
|
if robot_req.read() != 0:
|
|
raise Exception("Ongoing command")
|
|
if robot_ack.read() != 0:
|
|
raise Exception("Robot is not ready")
|
|
robot_cmd.write(command)
|
|
args = [0] * robot_args.size
|
|
index = 0
|
|
for arg in argv:
|
|
args[index] = arg
|
|
index = index + 1
|
|
if index == robot_args.size:
|
|
raise Exception("Invalid number of arguments")
|
|
robot_args.write(to_array(args, 'i'))
|
|
try:
|
|
self.request()
|
|
err = robot_ack.take()
|
|
if err == 1:
|
|
ret = robot_ret.read()
|
|
return ret
|
|
if err == 2:
|
|
raise Exception("Invalid command: " + str(command))
|
|
raise Exception("Unknown error: " + str(err))
|
|
finally:
|
|
self.cancel_request()
|
|
|
|
def request(self):
|
|
robot_req.write(1)
|
|
while robot_ack.read() == 0:
|
|
time.sleep(0.001)
|
|
|
|
def cancel_request(self):
|
|
robot_req.write(0)
|
|
while robot_ack.read() != 0:
|
|
time.sleep(0.001)
|
|
|
|
def mount(self, puck, sample):
|
|
return self.execute('1', '1', puck, sample)
|
|
|
|
add_device(RobotModbus("robot"), force = True)
|
|
|
|
|
|
|
|
|
|
|
|
add_device(img.getContrast(), force = True)
|
|
|
|
|
|
|
|
def detect_pucks(ip):
|
|
"""
|
|
"""
|
|
aux = grayscale(ip, in_place=False)
|
|
threshold(aux,0,50)
|
|
binary_fill_holes(aux)
|
|
return analyse_particles(aux, 10000,50000,
|
|
fill_holes = False, exclude_edges = True,print_table=True,
|
|
output_image = "outlines", minCirc = 0.4, maxCirc = 1.0)
|
|
|
|
def detect_samples(ip):
|
|
"""
|
|
"""
|
|
aux = grayscale(ip, in_place=False)
|
|
invert(aux)
|
|
subtract_background(aux)
|
|
auto_threshold(aux)
|
|
binary_open(aux)
|
|
return analyse_particles(aux, 250,1000,
|
|
fill_holes = False, exclude_edges = True,print_table=True,
|
|
output_image = "outlines", minCirc = 0.7, maxCirc = 1.0)
|
|
|
|
|
|
r,g,b = [0]*256,[0]*256,[0]*256
|
|
b[0]=0xFF
|
|
b[1]=0xFF ; g[1] = 0x80; r[1] = 0x80
|
|
outline_lut1 = (r,g,b)
|
|
|
|
r,g,b = [0]*256,[0]*256,[0]*256
|
|
g[0]=0x80;r[0]=0x80;
|
|
g[1]=0xFF ; r[1] = 0x80; b[1] = 0x80
|
|
outline_lut2 = (r,g,b)
|
|
|
|
|
|
|
|
from mathutils import estimate_peak_indexes, fit_gaussians, create_fit_point_list, Gaussian
|
|
import java.awt.Color as Color
|
|
|
|
import mathutils
|
|
mathutils.MAX_ITERATIONS = 100000
|
|
|
|
def fit(ydata, xdata = None, draw_plot = True):
|
|
if xdata is None:
|
|
xdata = frange(0, len(ydata), 1)
|
|
max_y= max(ydata)
|
|
index_max = ydata.index(max_y)
|
|
max_x= xdata[index_max]
|
|
print "Max index:" + str(index_max),
|
|
print " x:" + str(max_x),
|
|
print " y:" + str(max_y)
|
|
|
|
if draw_plot:
|
|
plots = plot([ydata],["data"],[xdata], title="Fit" )
|
|
p = None if plots is None else plots[0]
|
|
|
|
gaussians = fit_gaussians(ydata, xdata, [index_max,])
|
|
if gaussians[0] is None:
|
|
if draw_plot and (p is not None):
|
|
p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)
|
|
print "Fitting error"
|
|
return (None, None, None)
|
|
|
|
(norm, mean, sigma) = gaussians[0]
|
|
if draw_plot:
|
|
fitted_gaussian_function = Gaussian(norm, mean, sigma)
|
|
scale_x = [float(min(xdata)), float(max(xdata)) ]
|
|
points = max((len(xdata)+1), 100)
|
|
resolution = (scale_x[1]-scale_x[0]) / points
|
|
fit_y = []
|
|
fit_x = frange(scale_x[0],scale_x[1],resolution, True)
|
|
for x in fit_x:
|
|
fit_y.append(fitted_gaussian_function.value(x))
|
|
#Server
|
|
if p is None:
|
|
plot([ydata,fit_y],["data","fit"],[xdata,fit_x], title="Fit")
|
|
draw_plot = False
|
|
else:
|
|
p.addSeries(LinePlotSeries("fit"))
|
|
p.getSeries(1).setData(fit_x, fit_y)
|
|
|
|
if abs(mean - xdata[index_max]) < abs((scale_x[0] + scale_x[1])/2):
|
|
if draw_plot:
|
|
p.addMarker(mean, None, "Mean="+str(round(mean,4)), Color.MAGENTA.darker())
|
|
print "Mean -> " + str(mean)
|
|
return (norm, mean, sigma)
|
|
else:
|
|
if draw_plot:
|
|
p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)
|
|
print "Invalid gaussian fit: " + str(mean)
|
|
return (None, None, None)
|
|
|
|
context = get_context() |