################################################################################################### # 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): return self.execute('eval', cmd) def get_var(self, name): return self.execute('get_var', name) def get_arr(self, name, size): return self.execute('get_arr', name, size) def get_int(self): return int(self.get_var("n")) def eval_int(self, cmd): ret = self.execute('eval', "n=" + cmd) if ret.strip() != "": raise Exception(ret) return self.get_int() def get_move_id(): self.eval_int("getMoveId()") def mount(self, puck, sample): return self.execute('mount', puck, sample) 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()