Startup

script/LN2_Monitoring.scd

@@ -0,0 +1,15 @@
+[ 
+  [ [ true, "phase_separator_level", "Device", 1, 1, "102,204,255" ],
+    [ true, "filling_phase_separator", "Device", 1, 1, "51,255,255" ],
+    [ true, "filling_dewar", "Device", 1, 1, "0,0,102" ],
+    [ true, "dewar_level", "Device", 1, 1, "0,51,153" ],
+    [ true, "rim_heater_temp", "Device", 1, 1, "255,0,51" ] ],
+  [ [ "1", 0.0, 100.0, null, null, 1000000.0, false, null ],
+    [ "2", null, null, null, null, null, null, null ],
+    [ "3", null, null, null, null, null, null, null ],
+    [ "4", null, null, null, null, null, null, null ],
+    [ "5", null, null, null, null, null, null, null ] ],
+  [ [ ] ],
+  [ [ "", 20000, 100 ],
+    [ "", "" ] ]  
+]

script/LevelMonitoring.scd

@@ -0,0 +1,12 @@
+[ 
+  [ [ true, "phase_separator_level", "Device", 1, 1, "255,0,0" ],
+    [ true, "dewar_level", "Device", 1, 1, "0,0,255" ] ],
+  [ [ "1", 0.0, 100.0, null, null, 100000.0, false, null ],
+    [ "2", null, null, null, null, null, null, null ],
+    [ "3", null, null, null, null, null, null, null ],
+    [ "4", null, null, null, null, null, null, null ],
+    [ "5", null, null, null, null, null, null, null ] ],
+  [ [ ] ],
+  [ [ "", 20000, 100 ],
+    [ "", "" ] ]  
+]

script/calibration/BinarySearchYZ.py

@@ -0,0 +1,29 @@
+import plotutils
+import math
+
+
+#STRATEGY = "Normal" 
+#STRATEGY = "Boundary" 
+#STRATEGY = "FullNeighborhood" 
+RANGE = [-5.0, 5.0]
+STEP_SIZE = 0.1
+LATENCY = 0.05
+
+
+robot.enable()
+move_to_laser()
+
+
+robot.set_motors_enabled(True)
+current_y = robot_y.getPosition()
+current_z = robot_z.getPosition()
+
+
+r = bsearch([robot_y, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search YZ")
+
+
+print r.print()
+opt_y, opt_z= r.getOptimalPosition()
+offset_y, offset_z = opt_y - current_y, opt_z - current_z
+
+print "offset_y: ", offset_y, " offset_z: ", offset_z

script/calibration/HillClimbingXZ.py

@@ -0,0 +1,45 @@
+import plotutils
+import math
+
+
+#STRATEGY = "Normal" 
+STRATEGY = "Boundary" 
+#STRATEGY = "FullNeighborhood" 
+RANGE = [-5.0, 5.0]
+INITIAL_STEP = 1.0
+STEP_SIZE = 0.05
+LATENCY = 0.05
+NOISE_FILTER = 1
+
+
+robot.enable()
+move_to_laser()
+
+
+robot.set_motors_enabled(True)
+
+current_x = robot_x.getPosition()
+current_z = robot_z.getPosition()
+
+
+
+
+class Distance(Readable):
+    def read(self):
+        ret = ue.readable.read()
+        ret = 0.0 if math.isnan(ret) else ret
+        return ret
+        
+laser_distance=Distance()   
+start = time.time()        
+r = hsearch([robot_x, robot_z],laser_distance, [RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [INITIAL_STEP,INITIAL_STEP], [STEP_SIZE,STEP_SIZE], NOISE_FILTER, relative = True, maximum=True, latency = LATENCY, title = "Hill Climbing XZ")
+
+
+
+print r.print()
+opt_x, opt_z= r.getOptimalPosition()
+offset_x, offset_z = opt_x - current_x, opt_z - current_z
+
+print "offset_x: ", offset_x, " offset_z: ", offset_z
+
+

script/calibration/ScanRZ.py

@@ -0,0 +1,31 @@
+#Imports
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+#Parameters
+RANGE = [-120.0,120.0]
+STEP = 5.0
+LATENCY = 0.005
+RELATIVE = False
+
+#Enabling and checking
+#enable_motion()
+#system_check()
+robot.enable()
+
+
+#Body
+robot.set_tool(TOOL_DEFAULT)
+move_to_laser()
+#robot.set_joint_motors_enabled(True)
+robot.set_motors_enabled(True)
+robot_rz.move(0.0)
+robot.set_motors_enabled(True)
+ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = RELATIVE, range = "auto", title = "Scan2")
+
+
+#Cleanup
+
+
+
+

script/calibration/ScanX.py

@@ -0,0 +1,101 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+FIT = True
+
+d = robot.get_distance_to_pnt("pLaser")
+if d<0:
+    raise Exception ("Error calculating distance to laser: " + str(d))
+
+if d>20:
+    raise Exception ("Should be near the laser position to perform the scan")
+
+RANGE = [-5.0, 4.0] #[-1.5, 1.5]
+STEP = 0.02
+Z_OFFSET = 0 #-1.0
+LATENCY = 0.025
+BORDER_SIZE = 0.15
+
+robot.enable()
+robot.set_motors_enabled(True)
+current_positon = robot_x.getPosition()
+robot_z.moveRel(Z_OFFSET)
+
+print "Moving to scan start position: " , RANGE[0]
+robot.set_motors_enabled(True)
+robot_x.moveRel( RANGE[0])
+robot.set_motors_enabled(True)
+print "Starting scan X"
+
+RANGE = [0, (RANGE[1] - RANGE[0] )]
+
+robot.setPolling(25)
+
+try:
+    ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
+finally:    
+    robot.setPolling(DEFAULT_ROBOT_POLLING)
+
+d = ret.getReadable(0)
+
+first_index = -1
+last_index = -1
+for i in range(len(d)):
+    if not math.isnan(d[i]):
+        if first_index<0:
+            first_index = i  
+        last_index = i  
+
+if first_index == -1 or last_index < first_index:
+    raise Exception("Invalid range")
+
+
+remove = int(max(BORDER_SIZE, STEP) / STEP)
+
+_range = [first_index+remove, last_index-remove]
+if _range[1] <= _range[0]:
+    raise Exception("Invalid range: " + str(_range))
+
+
+center_index = int((_range[0] + _range[1])/2)
+center_positon = ret.getPositions(0)[center_index]
+
+
+
+
+y =  ret.getReadable(0)[_range[0] : _range[1]]
+x =  ret.getPositions(0)[_range[0]: _range[1]]
+
+#Clear NaNs
+first_value=ret.getReadable(0)[first_index]
+y = [(first_value if math.isnan(v) else v) for v in y]
+
+
+
+if FIT:
+    x = enforce_monotonic(x)
+    offset=100
+    (normalization, mean_val, sigma) = fit_gaussian([offset-v for v in y], x)
+    closest_x = mean_val
+    closest_y = 100 -normalization 
+else:
+    closest_x = x[y.index(min(y))]
+    closest_y = y[y.index(min(y))]
+
+
+
+if closest_x is None or closest_x <= ret.getPositions(0)[first_index] or closest_x >= ret.getPositions(0)[last_index]:
+    raise Exception("Invalid Fit")
+
+
+
+center_offset = center_positon-closest_y
+#center_offset = current_positon-closest_y
+
+
+
+p=get_plots()[0]
+p.addMarker(closest_x, p.AxisId.X, str(closest_x), Color.GREEN)
+
+robot.set_motors_enabled(True)
+robot_x.move(closest_x)

script/calibration/ScanY.py

@@ -0,0 +1,99 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+FIT = True
+
+d = robot.get_distance_to_pnt("pLaser")
+if d<0:
+    raise Exception ("Error calculating distance to laser: " + str(d))
+
+if d>20:
+    raise Exception ("Should be near the laser position to perform the scan")
+
+RANGE = [-7.0, 4.0] #[-1.5, 1.5]
+STEP = 0.02
+Z_OFFSET = 0 #-1.0
+LATENCY = 0.025
+BORDER_SIZE = 0.15
+
+robot.enable()
+robot.set_motors_enabled(True)
+current_positon = robot_y.getPosition()
+robot_z.moveRel(Z_OFFSET)
+
+
+print "Moving to scan start position: " , RANGE[0]
+robot.set_motors_enabled(True)
+robot_y.moveRel( RANGE[0])
+robot.set_motors_enabled(True)
+print "Starting scan Y"
+
+RANGE = [0, (RANGE[1] - RANGE[0] )]
+
+robot.setPolling(25)
+try:
+    ret = lscan(robot_y, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
+finally:    
+    robot.setPolling(DEFAULT_ROBOT_POLLING)
+
+d = ret.getReadable(0)
+
+first_index = -1
+last_index = -1
+for i in range(len(d)):
+    if not math.isnan(d[i]):
+        if first_index<0:
+            first_index = i  
+        last_index = i  
+
+if first_index == -1 or last_index < first_index:
+    raise Exception("Invalid range")
+
+
+remove = int(max(BORDER_SIZE, STEP) / STEP)
+
+_range = [first_index+remove, last_index-remove]
+if _range[1] <= _range[0]:
+    raise Exception("Invalid range: " + str(_range))
+
+
+center_index = int((_range[0] + _range[1])/2)
+center_positon = ret.getPositions(0)[center_index]
+
+
+
+
+y =  ret.getReadable(0)[_range[0] : _range[1]]
+x =  ret.getPositions(0)[_range[0]: _range[1]]
+
+#Clear NaNs
+first_value=ret.getReadable(0)[first_index]
+y = [(first_value if math.isnan(v) else v) for v in y]
+
+
+if FIT:
+    x = enforce_monotonic(x)
+    offset=100
+    (normalization, mean_val, sigma) = fit_gaussian([offset-v for v in y], x)
+    closest_y = mean_val
+    closest_x = 100 -normalization 
+else:
+    closest_y = x[y.index(min(y))]
+    closest_x = y[y.index(min(y))]
+
+
+if closest_y is None or closest_y <= ret.getPositions(0)[first_index] or closest_y >= ret.getPositions(0)[last_index]:
+    raise Exception("Invalid Fit")
+
+
+
+center_offset = center_positon-closest_y
+#center_offset = current_positon-closest_y
+
+
+
+p=get_plots()[0]
+p.addMarker(closest_y, p.AxisId.X, str(closest_y), Color.GREEN)
+
+robot.set_motors_enabled(True)
+robot_y.move(closest_y)

script/calibration/ScanYZ.py

@@ -0,0 +1,93 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+SINGLE_PASS = False
+if SINGLE_PASS:
+    STEP_SIZE = 0.2
+else:
+    STEP_SIZE = 1.0
+STEP_Z_FINAL = 0.1
+    
+RANGE = [-5.0, 5.0]
+LATENCY = 0.05
+
+Z_FINAL_OFFSET = 0.0
+
+
+SINGLE_PASS = True
+STEP_SIZE = 0.1
+
+
+robot.enable()
+move_to_laser()
+
+step_y = STEP_SIZE
+step_z = STEP_SIZE
+range_y = [RANGE[0], RANGE[1]]
+range_z = [RANGE[0], RANGE[1]]
+
+robot.set_motors_enabled(True)
+current_y = robot_y.getPosition()
+current_z = robot_z.getPosition()
+
+print "Current pos y,z" , current_y, ",", current_z
+ret = ascan([robot_y, robot_z], ue.readable, [range_y[0], range_z[0]], [range_y[1], range_z[1]], [step_y,step_z], latency = LATENCY, relative = True , zigzag=False, title = "Scan XY")
+data = ret.getData(0)[0]
+#plot(Convert.transpose(data), title="Data")
+
+integ = []
+for x in data: integ.append(sum(   [ (0.0 if (math.isnan(y)) else y)  for y  in x]))
+
+xdata= frange(range_y[0], range_y[1], step_y , False, True)
+p = plot(integ, title = "Fit", xdata=xdata)[0]
+
+
+max_x_index = integ.index(max(integ))
+max_x = xdata[max_x_index]
+try:
+    (normalization, mean_val, sigma) = fit_gaussian(integ, xdata)
+except:
+    raise Exception("Invalid Fit")    
+gaussian = Gaussian(normalization, mean_val, sigma)
+xdata= frange(range_y[0], range_y[1], step_y/100.0 , False, True)
+plot_function(p, gaussian, "Fit", xdata, show_points = False, show_lines = True, color = Color.BLUE)
+
+#So 
+if abs(mean_val - max_x) > 1.0:
+    raise Exception("Invalid Y detection")
+y_offset = mean_val    
+center_y = current_y + y_offset
+
+print "Y offset = ", y_offset
+
+robot_y.move(center_y)
+if SINGLE_PASS:
+    z_scan_data = data[max_x_index]        
+else:    
+    step_z = STEP_Z_FINAL
+    ret2 = lscan(robot_z, ue.readable, range_z[0], range_z[1], step_z, latency = LATENCY, relative = True , zigzag=False)
+    z_scan_data = ret2.getData(0)[0]
+    
+max_z_index= z_scan_data.index(max(z_scan_data))    
+last_z_index = 0
+for i in range(len(z_scan_data)):
+    if not math.isnan(z_scan_data[i]):
+        last_z_index = i    
+#Shape is cone: z is inceraseing. For proper detection last Z must be furthest    
+if abs(max_z_index - last_z_index) * step_z > 1.0: 
+    raise Exception("Invalid Z detection")
+
+if SINGLE_PASS:
+    max_z = ret.getPositions(1)[len(data[0]) * max_x_index + last_z_index]    
+else:    
+    max_z = ret2.getPositions(0)[last_z_index]    
+
+z_offset = max_z - current_z + Z_FINAL_OFFSET
+
+robot_z.move(max_z + Z_FINAL_OFFSET)
+
+print "Z offset = ", z_offset
+
+
+#Updating tool:
+#update_tool(None, x_offset=x_offset, z_offset=z_offset)

script/calibration/ToolCalibration.py

@@ -0,0 +1,60 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+
+robot.assert_tool(TOOL_CALIBRATION)
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
+
+
+initial_pos = robot.get_cartesian_pos()
+
+robot.enable()
+move_to_laser()
+
+robot.align()  
+
+
+run("calibration/ScanYZ")
+
+robot.set_motors_enabled(True)
+
+
+first_y = robot_y.take()
+first_z = robot_z.take()
+first_y = ue.take()
+first_j6 = robot_j6.take()
+if first_y is None:
+    raise Exception("Invalid YZ scan values in first scan")
+
+
+robot.set_joint_motors_enabled(True)
+if first_j6>0:
+    robot_j6.moveRel(-180.0, -1)
+else:
+    robot_j6.moveRel(180.0, -1)    
+
+    
+robot.set_motors_enabled(True)
+run("calibration/ScanYZ")
+
+robot.set_motors_enabled(True)
+
+second_y = robot_y.take()
+second_z = robot_z.take()
+second_y = ue.take()
+second_j6 = robot_j6.take()
+if second_y is None:
+    raise Exception("Invalid XZ scan values in first scan")
+
+
+#Updates the tool
+xoff = (first_x - second_x)/2
+yoff = (first_y - second_y)/2
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+t[0]=xoff 
+t[1]=-yoff 
+robot.set_tool_trsf(t, TOOL_DEFAULT)
+
+
+

script/calibration/ToolCalibration2.py

@@ -0,0 +1,86 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+
+#robot.assert_tool(TOOL_CALIBRATION)
+#cal_tool = TOOL_DEFAULT 
+cal_tool = TOOL_CALIBRATION
+
+robot.set_tool(cal_tool)
+robot.enable()
+move_to_laser()
+
+
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
+initial_pos = robot.get_cartesian_pos()
+
+robot.enable()
+move_to_laser()
+
+#robot.align()  
+
+
+run("calibration/ScanY")
+
+pos1 = robot.get_cartesian_pos()
+x1, y1 = closest_x, closest_y
+
+print "Closest 1: ", [x1, y1]
+print "Position 1: ", pos1
+
+
+pj6 = robot_j6.position
+if pj6>0:
+    robot_j6.move(pj6 - 180.0)
+else:    
+    robot_j6.move(pj6 + 180.0)
+
+
+run("calibration/ScanY")
+
+
+x2, y2 = closest_x, closest_y
+
+print "Closest 2: ", [x2, y2]
+
+
+
+off_x = x1 - x2
+
+#robot.set_motors_enabled(True)
+#robot_x.moveRel(off_x, -1)
+
+#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
+robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
+robot.set_trsf([off_x, 0,0,0,0,0])
+c=robot.compose("pTemp", FRAME_TABLE, "tcp_t" )
+robot.set_pnt(c, "pTemp")
+robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
+
+
+pos2 = robot.get_cartesian_pos()
+print pos2
+
+print "Position 2: ", pos2
+
+
+
+#Updates the tool
+xoff = (pos2[0]-pos1[0])/2
+yoff = (pos2[1]-pos1[1])/2
+
+#print "Offset: ", [xoff, yoff]
+
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+t[0]=xoff 
+t[1]=-yoff 
+print "Offset: ", [t[0], t[1]]
+robot.set_tool_trsf(t, TOOL_DEFAULT)
+
+robot.set_tool(TOOL_DEFAULT)
+d = robot.get_distance_to_pnt("pLaser")
+if d<POSITION_TOLERANCE:
+    print "Moving calibrated tool to laser"
+else:
+    print "Cannot move calibrated tool to laser: toog big offset"

script/calibration/ToolCalibration3.py

@@ -0,0 +1,83 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+cal_tool = TOOL_CALIBRATION
+
+robot.set_tool(cal_tool)
+
+robot.enable()
+move_to_laser()
+
+
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
+initial_pos = robot.get_cartesian_pos()
+
+#robot.align()  
+try:
+    robot.set_frame(FRAME_TABLE)    
+    run("calibration/ScanX")
+finally:
+    robot.set_default_frame()    
+
+pos1 = robot.get_cartesian_pos()
+x1, l1 = closest_x, closest_y
+
+print "Scan 1 result: ", [x1, l1]
+print "Position 1: ", pos1
+
+
+pj6 = robot_j6.position
+if pj6>0:
+    robot_j6.move(pj6 - 180.0)
+else:    
+    robot_j6.move(pj6 + 180.0)
+
+
+try:
+    robot.set_frame(FRAME_TABLE)    
+    run("calibration/ScanX")
+finally:
+    robot.set_default_frame()        
+
+pos2 =robot.get_cartesian_pos()
+x2, l2 = closest_x, closest_y
+
+print "Scan 2 result: ", [x2, l2]
+print "Position 2: ", pos1
+
+off_l = l2 - l1
+print "Offset l: ", off_l
+
+#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
+robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
+robot.set_trsf([0, -off_l, 0, 0, 0, 0])
+c=robot.compose("pTemp", FRAME_TABLE, "tcp_t" )
+robot.set_pnt(c, "pTemp")
+robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
+
+
+pos3 = robot.get_cartesian_pos()
+print "Position 3: ", pos3
+
+
+#Updates the tool
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+print "Former tool: " + str(t)
+
+xoff = (pos3[0]-pos1[0])/2
+yoff = (pos3[1]-pos1[1])/2 
+xrot = math.degrees(math.atan(yoff/t[2]))
+yrot = math.degrees(math.atan(xoff/t[2]))
+t[0]=xoff 
+t[1]=-yoff 
+print "Calibrated tool: " + str(t)
+robot.set_tool_trsf(t, TOOL_DEFAULT)
+
+robot.set_tool(TOOL_DEFAULT)
+d = robot.get_distance_to_pnt("pLaser")
+if d<POSITION_TOLERANCE:
+    print "Moving calibrated tool to laser"
+    #move_to_laser()    
+else:
+    print "Cannot move calibrated tool to laser: too big offset"

script/calibration/ToolCalibration6d.py

@@ -0,0 +1,79 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+cal_tool = TOOL_CALIBRATION
+
+robot.set_tool(cal_tool)
+
+robot.enable()
+move_to_laser("pPark")
+
+
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
+initial_pos = robot.get_cartesian_pos()
+
+#robot.align()  
+run("calibration/ScanX")
+
+pos1 = robot.get_cartesian_pos()
+x1, l1 = closest_x, closest_y
+
+print "Scan 1 result: ", [x1, l1]
+print "Position 1: ", pos1
+
+
+pj6 = robot_j6.position
+if pj6>0:
+    robot_j6.move(pj6 - 180.0)
+else:    
+    robot_j6.move(pj6 + 180.0)
+
+
+run("calibration/ScanX")
+
+
+pos2 =robot.get_cartesian_pos()
+x2, l2 = closest_x, closest_y
+
+print "Scan 2 result: ", [x2, l2]
+print "Position 2: ", pos1
+
+off_l = l2 - l1
+print "Offset l: ", off_l
+
+
+#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
+robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
+robot.set_trsf([0, -off_l, 0, 0, 0, 0])
+c=robot.compose("pTemp", FRAME_DEFAULT, "tcp_t" )
+robot.set_pnt(c, "pTemp")
+robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
+
+pos3 = robot.get_cartesian_pos()
+print "Position 3: ", pos3
+
+
+#Updates the tool
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+print "Former tool: " + str(t)
+
+xoff = (pos3[0]-pos1[0])/2
+yoff = (pos3[1]-pos1[1])/2 
+xrot = math.degrees(math.atan(yoff/t[2]))
+yrot = math.degrees(math.atan(xoff/t[2]))
+t[0]=xoff 
+t[1]=-yoff 
+print "Calibrated tool: " + str(t)
+robot.set_tool_trsf(t, TOOL_DEFAULT)
+
+robot.set_tool(TOOL_DEFAULT)
+d = robot.get_distance_to_pnt("pLaser")
+if d<POSITION_TOLERANCE:
+    print "Moving calibrated tool to laser"
+    #move_to_laser()    
+else:
+    print "Cannot move calibrated tool to laser: too big offset"
+
+
+

script/calibration/ToolCalibration6s.py

@@ -0,0 +1,79 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+cal_tool = TOOL_CALIBRATION
+
+robot.set_tool(cal_tool)
+
+robot.enable()
+move_to_laser("pPark")
+
+
+robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
+initial_pos = robot.get_cartesian_pos()
+
+#robot.align()  
+run("calibration/ScanY")
+
+pos1 = robot.get_cartesian_pos()
+l1, y1 = closest_x, closest_y
+
+print "Scan 1 result: ", [l1, y1]
+print "Position 1: ", pos1
+
+
+pj6 = robot_j6.position
+if pj6>0:
+    robot_j6.move(pj6 - 180.0)
+else:    
+    robot_j6.move(pj6 + 180.0)
+
+run("calibration/ScanY")
+
+pos2 =robot.get_cartesian_pos()
+l2, y2 = closest_x, closest_y
+
+print "Scan 2 result: ", [l2, y2]
+print "Position 2: ", pos1
+
+off_l = l2 - l1
+print "Offset l: ", off_l
+
+#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
+robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
+robot.set_trsf([-off_l, 0, 0, 0, 0, 0])
+c=robot.compose("pTemp", FRAME_DEFAULT, "tcp_t" )
+robot.set_pnt(c, "pTemp")
+robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)
+
+
+pos3 = robot.get_cartesian_pos()
+print "Position 3: ", pos3
+
+
+#Updates the tool
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+print "Former tool: " + str(t)
+
+xoff = (pos3[0]-pos1[0])/2
+yoff = (pos3[1]-pos1[1])/2 
+xrot = math.degrees(math.atan(yoff/t[2]))
+yrot = math.degrees(math.atan(xoff/t[2]))
+t[0]=xoff     #X
+t[1]=-yoff    #Y
+#t[3]= xropt  #RX
+#t[4]= yropt  #RY
+ 
+print "Calibrated tool: " + str(t)
+
+
+robot.set_tool_trsf(t, TOOL_DEFAULT)
+
+robot.set_tool(TOOL_DEFAULT)
+d = robot.get_distance_to_pnt("pLaser")
+if d<POSITION_TOLERANCE:
+    print "Moving calibrated tool to laser"
+    #move_to_laser()    
+else:
+    print "Cannot move calibrated tool to laser: too big offset"

script/client/PShellClient.py

@@ -0,0 +1,369 @@
+import threading
+import time
+import sys
+import requests
+import json
+
+try:
+    from urllib import quote  # Python 2
+except ImportError:
+    from urllib.parse import quote  # Python 3
+
+try:
+    from sseclient import SSEClient
+except:
+    SSEClient = None 
+    
+
+class PShellClient:
+    def __init__(self, url):
+        self.url = url
+        self.sse_event_loop_thread = None
+        self.subscribed_events = None
+        self.event_callback = None
+    
+    def _get_response(self, response, is_json=True):
+        if response.status_code != 200: 
+           raise Exception(response.text)
+        return json.loads(response.text) if is_json else response.text
+
+    def _get_binary_response(self, response):
+        if response.status_code != 200: 
+           raise Exception(response.text)
+        return  response.raw.read()     
+    
+    def get_version(self):
+        """Return application version.
+
+        Args:
+
+        Returns:
+            String with application version.
+
+        """
+        return self._get_response(requests.get(url=self.url+"/version"), False)   
+    
+    def get_config(self):
+        """Return application configuration.
+
+        Args:
+
+        Returns:
+            Dictionary.
+        """                
+        return self._get_response(requests.get(url=self.url+"/config"))   
+            
+    def get_state(self):
+        """Return application state.
+
+        Args:
+
+        Returns:
+            String: Invalid, Initializing,Ready, Paused, Busy, Disabled, Closing, Fault, Offline
+        """        
+        return self._get_response(requests.get(url=self.url+"/state"))   
+        
+    def get_logs(self):
+        """Return application logs.
+
+        Args:
+
+        Returns:
+            List of logs.
+            Format of each log: [date, time, origin, level, description]
+
+        """        
+        return self._get_response(requests.get(url=self.url+"/logs"))   
+        
+    def get_history(self, index):
+        """Access console command history.
+
+        Args:
+            index(int): Index of history entry (0 is the most recent)
+
+        Returns:
+            History entry
+
+        """
+        return self._get_response(requests.get(url=self.url+"/history/"+str(index)), False) 
+    
+    def get_script(self, path):
+        """Return script.
+
+        Args:
+            path(str): Script path (absolute or relative to script folder)
+
+        Returns:
+            String with file contents.
+
+        """        
+        return self._get_response(requests.get(url=self.url+"/script/"+str(path)), False) 
+                    
+    def get_devices(self):
+        """Return global devices.
+
+        Args:
+
+        Returns:
+            List of devices.
+            Format of each device record: [name, type, state, value, age]
+
+        """          
+        return self._get_response(requests.get(url=self.url+"/devices"))   
+        
+    def abort(self, command_id=None):
+        """Abort execution of command
+
+        Args:
+            command_id(optional, int): id of the command to be aborted.
+                                       if None (default), aborts the foreground execution. 
+
+        Returns:
+
+        """          
+        if command_id is None:
+            requests.get(url=self.url+"/abort") 
+        else:
+            return requests.get(url=self.url+"/abort/"+str(command_id)) 
+    
+    def reinit(self):
+        """Reinitialize the software.
+ 
+        Args:
+
+        Returns:
+
+        """          
+        requests.get(url=self.url+"/reinit") 
+    
+    def stop(self):
+        """Stop all devices implementing the 'Stoppable' interface.
+ 
+        Args:
+
+        Returns:
+
+        """        
+        requests.get(url=self.url+"/stop")
+    
+    def update(self):
+        """Update all global devices.
+ 
+        Args:
+
+        Returns:
+
+        """        
+        requests.get(url=self.url+"/update")             
+   
+    def eval(self,statement):
+        """Evaluates a statement in the interpreter.
+           If the statement finishes by '&', it is executed in background.
+           Otherwise statement is executed in foreground (exclusive).
+ 
+        Args:       
+            statement(str): input statement
+
+        Returns: 
+            String containing the console return. 
+            If an exception is produces in the interpretor, it is re-thrown here.
+        """             
+        statement = quote(statement)
+        return self._get_response(requests.get(url=self.url+"/eval/"+statement), False)              
+
+    def run(self,script, pars=None, background=False):
+        """Executes script in the interpreter.
+ 
+        Args:       
+            script(str): name of the script (absolute or relative to the script base folder). Extension may be omitted.
+            pars(optional, list or dict): if a list is given, it sets sys.argv for the script.
+                                          If a dict is given, it sets global variable for the script.
+            background(optional, bool): if True script is executed in background.
+
+        Returns: 
+            Return value of the script.
+            If an exception is produces in the interpretor, it is re-thrown here.
+        """          
+        return self._get_response(requests.put(url=self.url+"/run", json={"script":script, "pars":pars, "background":background, "async":False }))   
+    
+    def start_eval(self,statement):
+        """Starts evaluation of a statement in the interpreter.
+           If the statement finishes by '&', it is executed in background.
+           Otherwise statement is executed in foreground (exclusive).
+ 
+        Args:       
+            statement(str): input statement
+
+        Returns: 
+            Command id (int), which is used to retrieve command execution status/result (get_result). 
+        """            
+        statement = quote(statement)
+        return int(self._get_response(requests.get(url=self.url+"/evalAsync/"+statement), False))            
+
+    def start_run(self,script, pars=None, background=False):
+        """Starts execution of a script in the interpreter.
+ 
+        Args:       
+            script(str): name of the script (absolute or relative to the script base folder). Extension may be omitted.
+            pars(optional, list or dict): if a list is given, it sets sys.argv for the script.
+                                          If a dict is given, it sets global variable for the script.
+            background(optional, bool): if True script is executed in background.
+
+        Returns: 
+            Command id (int), which is used to retrieve command execution status/result (get_result). 
+        """        
+        return int(self._get_response(requests.put(url=self.url+"/run", json={"script":script, "pars":pars, "background":background, "async":True })))  
+
+    def get_result(self, command_id=-1):
+        """Gets status/result of a command executed asynchronously (start_eval and start_run).
+ 
+        Args:       
+            command_id(optional, int): command id. If equals to -1 (default) return status/result of the foreground task. 
+
+        Returns: 
+            Dictionary with the fields: 'id' (int): command id
+                                        'status' (str): unlaunched, invalid, removed, running, aborted, failed or completed.
+                                        'exception' (str): if status equals 'failed', holds exception string.
+                                        'return' (obj): if status equals 'completed', holds return value of script (start_run) 
+                                                        or console return (start_eval)
+        """          
+        return self._get_response(requests.get(url=self.url+"/result/"+str(command_id)))  
+
+    def help(self, input = "<builtins>"):
+        """Returns help or auto-completion strings.
+ 
+        Args:       
+            input(optional, str): - ":" for control commands
+                                  - "<builtins>" for builtin functions
+                                  - "devices" for device names
+                                  - builtin function name for function help
+                                  - else contains entry for auto-completion
+
+        Returns: 
+            List
+             
+        """        
+        return self._get_response(requests.get(url=self.url+"/autocompletion/" + input))   
+
+    def get_contents(self, path=None):
+        """Returns contents of data path.
+ 
+        Args:       
+            path(optional, str): Path to data relative to data home path.
+                                 - Folder
+                                 - File
+                                 - File (data root) |  internal path
+                                 - internal path (on currently open data root)
+
+        Returns: 
+            List of contents
+             
+        """           
+        return self._get_response(requests.get(url=self.url+ "/contents" + ("" if path is None else ( "/"+path))), False)    
+   
+    def get_data(self, path, type="txt"):
+        """Returns data on a given path.
+ 
+        Args:       
+            path(str): Path to data relative to data home path.
+                                 - File (data root) |  internal path
+                                 - internal path (on currently open data root)
+            type(optional, str): txt, "json", "bin", "bs"
+
+        Returns: 
+            Data accordind to selected format/.
+             
+        """           
+        if type == "json":
+            return self._get_response(requests.get(url=self.url+ "/data-json/"+path), True)  
+        elif type == "bin":
+            return  self._get_binary_response(requests.get(url=self.url+"/data-bin/"+path, stream=True))
+        elif type == "bs":    
+            from collections import OrderedDict
+            bs = self._get_binary_response(requests.get(url=self.url+"/data-bs/"+path, stream=True))
+            index=0
+            msg = []
+            for i in range(4):
+               size =int.from_bytes(bs[index:index+4], byteorder='big', signed=False)
+               index=index+4
+               msg.append(bs[index:index+size])
+               index=index+size            
+            [main_header, data_header, data, timestamp] = msg
+            main_header = json.loads(main_header, object_pairs_hook=OrderedDict)
+            data_header = json.loads(data_header, object_pairs_hook=OrderedDict)
+            channel =  data_header["channels"][0]
+            channel["encoding"] = "<" if channel.get("encoding", "little") else ">"
+            from bsread.data.helpers import get_channel_reader
+            channel_value_reader = get_channel_reader(channel)
+            return channel_value_reader(data)            
+            
+        return self._get_response(requests.get(url=self.url+ "/data" + ("" if path is None else ( "/"+path))), False)    
+              
+    def print_logs(self):
+        for l in self.get_logs():
+            print ("%s %s %-20s %-8s %s" % tuple(l))
+    
+    def print_devices(self):
+        for l in self.get_devices():
+            print ("%-16s %-32s %-10s %-32s %s" % tuple(l)) 
+                   
+    def print_help(self, input = "<builtins>"):
+        for l in self.help(input):
+            print (l)  
+
+    #Events       
+    def _sse_event_loop_task(self):  
+        try:
+            while True:
+                try:
+                    messages = SSEClient(self.url+"/events")
+                    for msg in messages:
+                        if (self.subscribed_events is None) or (msg.event in self.subscribed_events):
+                            try:
+                                value = json.loads(msg.data)
+                            except:
+                                value = str(msg.data)
+                            self.event_callback(msg.event, value)
+                except IOError as e:
+                    #print(e)
+                    pass
+                except:
+                    print("Error:", sys.exc_info()[1])
+                    #raise
+        finally:
+            print ("Exit SSE loop task")
+            self.sse_event_loop_thread = None
+
+
+    def start_sse_event_loop_task(self, subscribed_events = None, event_callback = None):
+        """
+        Initializes server event loop task.
+        Args:    
+            subscribed_events: list of event names to substribe to. If None subscribes to all.
+            event_callback: callback function. If None, self.on_event is called instead.
+
+        Usage example:
+            def on_event(name, value):
+                if name == "state":
+                    print ("State changed: ",  value)
+                elif name == "record":
+                    print ("Received scan record: ", value) 
+
+            pc.start_sse_event_loop_task(["state", "record"], on_event)    
+           
+        """
+        self.event_callback = event_callback if event_callback is not None else self.on_event
+        self.subscribed_events = subscribed_events
+        if SSEClient is not None:
+            if self.sse_event_loop_thread is None:
+                self.sse_event_loop_thread = threading.Thread(target=self._sse_event_loop_task, \
+                                                     args = (), \
+                                                     kwargs={}, \
+                                                     daemon=True)
+                self.sse_event_loop_thread.start()
+        else:
+            raise Exception ("sseclient library is not instlled: server events are not available")
+
+    def on_event(self, name, value):
+        pass
+        

script/client/TellClient.py

@@ -0,0 +1,184 @@
+from PShellClient import PShellClient
+import json
+import time
+import sys
+
+
+
+class TellClient(PShellClient):
+    def __init__(self, url):
+        PShellClient.__init__(self, url)
+        self.start_sse_event_loop_task(["state", "shell"])   
+        self.state = self.get_state()
+        self.debug=False
+    
+    def on_event(self, name, value):
+        if name == "state":
+            self.state = value
+            print ("State: ",  value)      
+        elif name == "shell":
+            if self.debug:
+                print ("> ",  value)  
+                         
+    def get_state(self):
+        self.state = PShellClient.get_state(self)
+        return self.state
+
+    def wait_ready(self):
+        count = 0
+        #Monitors event but polls every second just n case an event is missed
+        while (True):
+            if self.state != "Busy":
+                break        
+            time.sleep(0.01)
+            count = count + 1
+            if count>=100:
+                count=0
+                self.get_state()
+        if  self.state != "Ready":
+             raise Exception("Invalid state: " + str(self.state))
+             
+    def set_in_mount_position(self, value):
+        self.eval("in_mount_position = " + str(value) +"&")
+
+    def is_in_mount_position(self):
+        return self.eval("in_mount_position&").lower()=="true" 
+
+    def get_samples_info(self):
+        return json.loads(self.eval("get_samples_info()&"))
+
+    def set_samples_info(self, info):
+        #c.run("data/set_samples_info", pars= [info,], background=True)
+        self.eval("set_samples_info(" + json.dumps(info) + ")&")
+    
+    def start_cmd(self, cmd, *argv):
+        cmd = cmd + "("
+        for a in argv:
+            cmd = cmd + (("'" + a + "'") if type(a) is str else str(a) ) + ", "
+        cmd = cmd + ")"
+        ret = self.start_eval(cmd)
+        self.get_state()
+        return ret
+
+    def wait_cmd(self, cmd):
+        self.wait_ready()
+        result = self.get_result(cmd)
+        #print (result)
+        if result["exception"] is not None:
+            raise Exception(result["exception"] )
+        return result["return"]        
+
+    def mount(self, segment, puck, sample, force=False, read_dm=False, auto_unmount=False):
+        #return self.run("motion/mount", pars= [segment,puck, sample, force, read_dm], background=True)
+        return self.start_cmd("mount", segment, puck, sample, force, read_dm, auto_unmount)
+
+    def unmount(self, segment = None, puck = None, sample = None, force=False):
+        return self.start_cmd("unmount", segment, puck, sample, force)
+
+    def scan_pin(self, segment, puck, sample, force=False):
+        return self.start_cmd("scan_pin", segment, puck, sample, force)
+
+    def scan_puck(self, segment, puck, force=False):
+        return self.start_cmd("scan_puck", segment, puck, force)     
+
+    def dry(self, heat_time=30.0, speed=0.5, wait_cold = 30.0):    
+        return self.start_cmd("dry", heat_time, speed, wait_cold)
+        
+    def move_cold(self):
+        return self.start_cmd("move_cold")        
+
+    def trash(self):
+        return self.start_cmd("trash")   
+
+    def abort_cmd(self):
+        self.abort()
+        self.eval("robot.stop_task()&")
+
+    def set_gonio_mount_position(homing = False):
+        if homing:
+            self.eval("home_fast_table()")
+        self.eval("set_mount_position()")
+
+    def get_mounted_sample(self):
+        ret = self.eval("get_setting('mounted_sample_position')&").strip()
+        return None if len(ret)==0 else ret
+        
+    def get_system_check(self):
+        try:
+            ret = self.eval("system_check()&")
+        except Exception as ex:
+            return ex
+        return "Ok"
+
+    def get_robot_state(self):
+        return self.eval("robot.state&")      
+
+
+    def get_robot_status(self):
+        status =  self.eval("robot.take()&")  
+        return status
+            
+    def get_detected_pucks(self):
+       return self.eval("get_detected_pucks()&")   
+
+    def set_pin_offset(self, value):
+        self.eval("set_pin_offset(" + str(value)+ ")&")  
+
+    def get_pin_offset(self):
+       return self.eval("get_pin_offset()&")   
+
+    def print_info(self):    
+        print ("State: " + str(self.get_state()))
+        print ("Mounted sample: " + str(self.get_mounted_sample()))
+        print ("System check: " + str(self.get_system_check()))
+        print ("Robot state: " + str(self.get_robot_state()))
+        print ("Robot status: " + str(self.get_robot_status()))
+        print ("Detected pucks: " + str(self.get_detected_pucks()))
+        print ("Pin offset: " + str(self.get_pin_offset()))
+        print ("Mount position: " + str(self.is_in_mount_position()))
+        print ("")
+
+if __name__ == "__main__": 
+    tell = TellClient("http://Alexandres-MBP.psi.ch:8080")
+    tell.print_info()
+
+    info = [
+       {
+           "userName": "User",
+           "dewarName": "Dewar",
+           "puckName": "Puck",
+           "puckBarcode": "XXX0001",
+           "puckType": "Minispine",
+           "puckAddress": "",
+           "sampleName": "Sample",
+           "sampleBarcode": "YYY0001",
+           "samplePosition": "1",
+           "sampleStatus": "Present",
+           "sampleMountCount": "0" ,
+       },
+    ]
+    print (tell.get_samples_info())
+    tell.set_samples_info(info)
+    print (tell.get_samples_info())
+    
+    tell.abort_cmd()
+    
+    cmd = tell.move_cold()
+    print (tell.wait_cmd(cmd)) 
+
+    cmd = tell.trash()
+    print (tell.wait_cmd(cmd)) 
+        
+    cmd = tell.scan_pin("A", 1, 1)
+    print (tell.wait_cmd(cmd))   
+
+    cmd = tell.scan_puck("A", 1, 1)
+    print (tell.wait_cmd(cmd))   
+     
+    cmd = tell.mount("A", 1, 1)
+    print (tell.wait_cmd(cmd))
+
+    print ("Mounted sample: " + str(tell.get_mounted_sample()))
+    cmd = tell.unmount()
+    print (tell.wait_cmd(cmd))   
+    print ("Mounted sample: " + str(tell.get_mounted_sample()))

script/client/sseclient.py

@@ -0,0 +1,163 @@
+import codecs
+import re
+import time
+import warnings
+
+import six
+
+import requests
+
+
+# Technically, we should support streams that mix line endings.  This regex,
+# however, assumes that a system will provide consistent line endings.
+end_of_field = re.compile(r'\r\n\r\n|\r\r|\n\n')
+
+class SSEClient(object):
+    def __init__(self, url, last_id=None, retry=3000, session=None, chunk_size=1024, **kwargs):
+        self.url = url
+        self.last_id = last_id
+        self.retry = retry
+        self.chunk_size = chunk_size
+
+        # Optional support for passing in a requests.Session()
+        self.session = session
+
+        # Any extra kwargs will be fed into the requests.get call later.
+        self.requests_kwargs = kwargs
+
+        # The SSE spec requires making requests with Cache-Control: nocache
+        if 'headers' not in self.requests_kwargs:
+            self.requests_kwargs['headers'] = {}
+        self.requests_kwargs['headers']['Cache-Control'] = 'no-cache'
+
+        # The 'Accept' header is not required, but explicit > implicit
+        self.requests_kwargs['headers']['Accept'] = 'text/event-stream'
+
+        # Keep data here as it streams in
+        self.buf = u''
+
+        self._connect()
+
+    def _connect(self):
+        if self.last_id:
+            self.requests_kwargs['headers']['Last-Event-ID'] = self.last_id
+
+        # Use session if set.  Otherwise fall back to requests module.
+        requester = self.session or requests
+        self.resp = requester.get(self.url, stream=True, **self.requests_kwargs)
+        self.resp_iterator = self.resp.iter_content(chunk_size=self.chunk_size)
+
+        # TODO: Ensure we're handling redirects.  Might also stick the 'origin'
+        # attribute on Events like the Javascript spec requires.
+        self.resp.raise_for_status()
+
+    def _event_complete(self):
+        return re.search(end_of_field, self.buf) is not None
+
+    def __iter__(self):
+        return self
+
+    def __next__(self):
+        decoder = codecs.getincrementaldecoder(
+            self.resp.encoding)(errors='replace')
+        while not self._event_complete():
+            try:
+                next_chunk = next(self.resp_iterator)
+                if not next_chunk:
+                    raise EOFError()
+                self.buf += decoder.decode(next_chunk)
+
+            except (StopIteration, requests.RequestException, EOFError) as e:
+                time.sleep(self.retry / 1000.0)
+                self._connect()
+
+                # The SSE spec only supports resuming from a whole message, so
+                # if we have half a message we should throw it out.
+                head, sep, tail = self.buf.rpartition('\n')
+                self.buf = head + sep
+                continue
+
+        # Split the complete event (up to the end_of_field) into event_string,
+        # and retain anything after the current complete event in self.buf
+        # for next time.
+        (event_string, self.buf) = re.split(end_of_field, self.buf, maxsplit=1)
+        msg = Event.parse(event_string)
+
+        # If the server requests a specific retry delay, we need to honor it.
+        if msg.retry:
+            self.retry = msg.retry
+
+        # last_id should only be set if included in the message.  It's not
+        # forgotten if a message omits it.
+        if msg.id:
+            self.last_id = msg.id
+
+        return msg
+
+    if six.PY2:
+        next = __next__
+
+
+class Event(object):
+
+    sse_line_pattern = re.compile('(?P<name>[^:]*):?( ?(?P<value>.*))?')
+
+    def __init__(self, data='', event='message', id=None, retry=None):
+        self.data = data
+        self.event = event
+        self.id = id
+        self.retry = retry
+
+    def dump(self):
+        lines = []
+        if self.id:
+            lines.append('id: %s' % self.id)
+
+        # Only include an event line if it's not the default already.
+        if self.event != 'message':
+            lines.append('event: %s' % self.event)
+
+        if self.retry:
+            lines.append('retry: %s' % self.retry)
+
+        lines.extend('data: %s' % d for d in self.data.split('\n'))
+        return '\n'.join(lines) + '\n\n'
+
+    @classmethod
+    def parse(cls, raw):
+        """
+        Given a possibly-multiline string representing an SSE message, parse it
+        and return a Event object.
+        """
+        msg = cls()
+        for line in raw.splitlines():
+            m = cls.sse_line_pattern.match(line)
+            if m is None:
+                # Malformed line.  Discard but warn.
+                warnings.warn('Invalid SSE line: "%s"' % line, SyntaxWarning)
+                continue
+
+            name = m.group('name')
+            if name == '':
+                # line began with a ":", so is a comment.  Ignore
+                continue
+            value = m.group('value')
+
+            if name == 'data':
+                # If we already have some data, then join to it with a newline.
+                # Else this is it.
+                if msg.data:
+                    msg.data = '%s\n%s' % (msg.data, value)
+                else:
+                    msg.data = value
+            elif name == 'event':
+                msg.event = value
+            elif name == 'id':
+                msg.id = value
+            elif name == 'retry':
+                msg.retry = int(value)
+
+        return msg
+
+    def __str__(self):
+        return self.data

script/client/tell.py

@@ -0,0 +1,79 @@
+import sys
+import os
+import code
+import readline
+import rlcompleter
+import atexit
+
+from TellClient import TellClient
+
+tell = TellClient("http://PC12288:8080")
+
+def info():
+    tell.print_info()
+
+def help():
+    print ("Commands: \n\thelp()\n\tinfo()\n\tmount(segment, puck, sample)\n\tunmount() \n\tdry() "  \
+                       "\n\tscan(segment, puck, sample=None) \n\tmove_cold() \n\ttrash() \n\tabort() " \
+                       "\n\tset_pin_offset(value)")
+
+def assert_transfer_allowed():
+    if not tell.is_in_mount_position():
+        raise Exception("Gonio is not in mount position")
+    
+def mount(segment, puck, sample):
+    assert_transfer_allowed()
+    cmd = tell.mount(segment, puck, sample, True, True, True)
+    print (tell.wait_cmd(cmd))   
+
+def unmount():
+    assert_transfer_allowed()
+    cmd=tell.unmount(force=True)  
+    print (tell.wait_cmd(cmd))   
+
+def move_cold():
+    cmd=tell.move_cold()      
+    print (tell.wait_cmd(cmd))   
+
+def trash():
+    cmd=tell.trash() 
+    print (tell.wait_cmd(cmd))   
+
+def dry():    
+    cmd=tell.dry()
+    print (tell.wait_cmd(cmd))   
+        
+def scan(segment, puck, sample=None):
+    if sample is None:
+        cmd=tell.scan_puck(segment, puck, True)      
+    else:
+        cmd=tell.scan_pin(segment, puck, sample, True)      
+    print (tell.wait_cmd(cmd))   
+
+def abort(): 
+    tell.abort_cmd()
+
+def set_pin_offset(value):
+    tell.set_pin_offset(value)
+
+info()
+help()
+
+#for line in sys.stdin:
+#    tell.print_info()
+#print ("", end='\r> ')
+
+historyPath = os.path.expanduser("./.history")
+def save_history(historyPath=historyPath):    
+    readline.write_history_file(historyPath)
+if os.path.exists(historyPath):
+    readline.read_history_file(historyPath)
+atexit.register(save_history)
+
+#vars = globals(); vars.update(locals())
+vars =locals()
+readline.set_completer(rlcompleter.Completer(vars).complete)
+readline.parse_and_bind("tab: complete")
+
+code.interact(banner = "", local=vars)
+

script/data/get_samples_info.py

@@ -0,0 +1,4 @@
+
+
+
+set_return(get_samples_info(as_json=False))

script/data/pucks.py

@@ -0,0 +1,51 @@
+def get_puck_names():
+    return [str(a)+str(b) for a in BLOCKS for b in range(1,6)]
+
+
+def get_puck_info():
+    ret = []
+    #for puck in get_puck_names():
+    for puck in BasePlate.pucks:
+        ret.append({"address" : str(puck.name), "status": str(puck.status), "barcode" : str(puck.id)})
+    return ret
+
+
+def get_puck_obj(address):
+    return BasePlate.getChild(address)
+
+
+def get_puck_obj_by_id(puck_id):
+    for puck in BasePlate.pucks:
+        if puck.id == puck_id:
+            return puck
+    return None            
+
+
+def set_puck_info(puck_info):
+    #print puck_info
+    for i in puck_info:
+        p=get_puck_obj(i["address"])
+        if p is not None:
+            if (p.status == 'Present') and ( i["status"] =='Present'):
+                if i['barcode'] and (i['barcode'] != str(None)):
+                    #print "Setting ", p, " to ", i['barcode']
+                    p.id = i['barcode']
+                
+def clear_puck_info():
+    save_puck_info([])
+
+def save_puck_info():
+    data = get_puck_info()
+    output_file = open( get_context().setup.expandPath("{context}/pucks_info.json") , "w")
+    output_file.write(json.dumps(data))
+    output_file.close()    
+
+def restore_puck_info():
+    try:
+        inputfile = open(get_context().setup.expandPath("{context}/pucks_info.json"), "r")   
+        info = json.loads(inputfile.read())
+    except:
+        print >> sys.stderr, "Error reading pucks info file: " + str(sys.exc_info()[1])
+        info = []
+    set_puck_info(info)
+    

script/data/reports.py

@@ -0,0 +1,39 @@
+from statsutils import *
+
+
+def report(start, end):
+    """
+    Data format for start and end: "dd/mm/yy" or "dd/mm/yy hh:mm:ss.mmm"
+
+    """
+    cmds = ["mount%", "unmount%", "dry%", "recover%", "trash%", "robot_recover%", "scan%", "homing%"]
+    
+    conn = get_stats_connection()
+    try:
+        print_stats(cmds, start, end)
+    
+        for cmd in cmds:
+            print_cmd_stats (cmd, start, end)
+
+        
+    finally:
+        conn.close()   
+
+def print_recs(cmd,start, end, result=("%%")):
+    """
+    Data format for start and end: "dd/mm/yy" or "dd/mm/yy hh:mm:ss.mmm"
+    Result: "error", "abort, "success". Defaulr for all
+
+    """
+    
+    conn = get_stats_connection()
+    try: 
+        print_cmd_records(cmd,start, end, result)
+    finally:
+        conn.close()   
+        
+
+#Print all records:
+#       
+
+#report("01/03/19","01/04/19")

script/data/samples.py

@@ -0,0 +1,350 @@
+import json
+import re
+import org.python.core.PyDictionary as PyDictionary
+
+SAMPLE_INFO_KEYS = ["userName", "dewarName", "puckName", "puckBarcode", "puckType", "puckAddress", 
+                    "sampleName", "samplePosition", "sampleBarcode", "sampleStatus", "sampleMountCount"]
+samples_info = []
+
+
+def set_samples_info(info):
+    global samples_info
+    if (is_string(info)):
+        info = json.loads(info)
+    if not is_list(info):
+        raise Exception("Sample info must be a list (given object type is " + str(type(info)) + ")") 
+    #Sanitize list
+    remove = []
+    for sample in info:
+        try:
+            if set(sample.keys()) != set(SAMPLE_INFO_KEYS):
+                raise Exception()
+        except:
+            remove.append(sample)
+    for el in remove: 
+        info.remove(el)
+        print "Invalid samples info element: " + str(el)
+    samples_info = info
+    save_samples_info()
+    #Trust beamline on assignments, so update puck info
+    update_puck_table()
+   
+
+def clear_samples_info():
+    set_samples_info([])
+
+def save_samples_info():
+    data = get_samples_info(True)
+    output_file = open( get_context().setup.expandPath("{context}/samples_info.json") , "w")
+    output_file.write(data)
+    output_file.close()   
+    get_context().sendEvent("samples_updated", True)
+    save_puck_info() 
+
+def restore_samples_info():
+    restore_puck_info()
+    try:
+        inputfile = open(get_context().setup.expandPath("{context}/samples_info.json"), "r")   
+        info = inputfile.read()
+    except:
+        print >> sys.stderr, "Error reading sample info file: " + str(sys.exc_info()[1])
+        info = []
+    set_samples_info(info)
+            
+def get_samples_info(as_json=True):
+    global samples_info    
+    return json.dumps(to_list(samples_info)) if as_json else samples_info
+
+def has_puck_datamatrix(datamatrix):
+    if samples_info is not None:
+        for si in samples_info:
+            if si["puckBarcode"] == datamatrix:
+                return True   
+    return False
+
+def add_puck_datamatrix(barcode, address = "", dewar = "Unknown", user = "Unknown", puck = "Unknown", type = "unipuck", sample_prefix = "Sample"):
+    if has_puck_datamatrix(barcode):
+        raise Exception("Datamatrix already defined: " + str(barcode))
+    for s in range(1,17):
+        info = \
+            {  "userName": user, \  
+               "dewarName": dewar, \	
+               "puckName": puck, 	\
+               "puckBarcode": address if barcode is None else barcode, \	
+               "puckType": type, 	\
+               "puckAddress": address,\ 	
+               "sampleName": sample_prefix + " " + str(s), \	
+               "samplePosition": str(s),\ 	
+               "sampleStatus": "Unknown", 	\
+               "sampleMountCount": "0",    
+               }  
+        samples_info.append(info)    
+    save_samples_info()
+
+def remove_puck_datamatrix(barcode):
+    remove = []
+    for si in samples_info:
+        if si["puckBarcode"] == barcode:
+            remove.append(si)
+    for el in remove: samples_info.remove(el)
+    save_samples_info()
+
+    
+barcode_clean_re = re.compile("[^a-zA-Z0-9]")
+
+def is_same_datamatrix(table, reading):
+    if table == reading:
+        return True
+    table2 = barcode_clean_re.sub("", table.upper())
+    reading2 = barcode_clean_re.sub("", reading.upper())
+    if table2 == reading2:
+        log("Assigning aproximative datamatrix " + reading )    
+        return True    
+    return False
+
+    
+def set_puck_datamatrix(puck, datamatrix):
+    if puck is None: 
+        puck = ""
+    if datamatrix is None: 
+        datamatrix = ""
+    if samples_info is not None:
+        for si in samples_info:
+            #if si["puckBarcode"] == datamatrix:
+            if is_same_datamatrix(si["puckBarcode"],datamatrix):
+                si["puckAddress"] = puck
+            elif si["puckAddress"] == puck:
+                si["puckAddress"] = ""
+    save_samples_info()
+
+def reset_puck_datamatrix(puck = None):
+    if samples_info is not None:
+        for si in samples_info:
+            if (si["puckAddress"] == puck) or (puck is None):
+                si["puckAddress"] = ""
+    save_samples_info()   
+    for p in BasePlate.getChildren():
+        if (p.name == puck) or (puck is None):
+            p.id = None
+
+def get_puck_datamatrix():
+    ret = {}
+    for si in samples_info:
+        if si["puckBarcode"] is not None and si["puckBarcode"]!="":
+            ret[si["puckBarcode"]] =  si["puckAddress"]
+    return ret
+
+def get_puck_address(barcode):
+    try:
+        return get_puck_datamatrix()[barcode]
+    except:
+        return None
+
+
+def update_puck_table():
+    dms = get_puck_datamatrix()
+    for barcode in dms.keys():
+        address = dms[barcode]
+        puck = get_puck_obj(address)
+        if puck is not None:
+            puck.id = barcode
+
+
+#Sample mount/unmount
+
+def same_address(puck_add_1, sample_pos_1, puck_add_2, sample_pos_2):    
+    if str(puck_add_1) != str(puck_add_2):         
+        return False    
+    try:
+        sample_pos_1 = int(sample_pos_1)
+    except: 
+        return False
+    try:
+        sample_pos_2 = int(sample_pos_2)
+    except: 
+        return False    
+    return sample_pos_1 == sample_pos_2
+
+
+def update_samples_info_sample_mount(puck_address, sample_position, sample_detected, sample_id):
+    try:
+        if (samples_info is not None) and (puck_address is not None):            
+            for si in samples_info:
+                if same_address( si["puckAddress"], si["samplePosition"], puck_address, sample_position):
+                    if sample_detected:
+                        if si["sampleStatus"] != "Mounted":
+                            si["sampleStatus"] = "Mounted"                            
+                            try:
+                                mount_count = int(si["sampleMountCount"])
+                            except:
+                                mount_count = 0                            
+                            si["sampleMountCount"] = mount_count + 1
+                    else:
+                        si["sampleStatus"] = "Unknown"
+                        
+                    if sample_id is not None:
+                        si["sampleBarcode"] = sample_id
+                                           
+                else:
+                    if si["sampleStatus"] == "Mounted":
+                        si["sampleStatus"] = "HasBeenMounted"
+            save_samples_info()  
+    except:
+        pass
+
+    
+def update_samples_info_sample_unmount(puck_address, sample_position):
+    try:
+        if (samples_info is not None) and (puck_address is not None):
+            for si in samples_info:
+                if same_address( si["puckAddress"], si["samplePosition"], puck_address, sample_position):
+                    si["sampleStatus"] = "HasBeenMounted"
+                    save_samples_info()  
+                    return
+    except:
+        pass
+
+def update_samples_info_sample_scan(puck_address, sample_position, sample_detected, sample_id):
+    try:
+        if (samples_info is not None) and (puck_address is not None):
+            for si in samples_info:
+                if same_address( si["puckAddress"], si["samplePosition"], puck_address, sample_position):
+                    if sample_detected:
+                        if si["sampleStatus"] == "Unknown":
+                                si["sampleStatus"] = "Present"
+                    else:
+                        if si["sampleStatus"] == "Present":
+                                si["sampleStatus"] = "Unknown"
+                    if sample_id is not None:
+                        si["sampleBarcode"] = sample_id                        
+                    save_samples_info()  
+                    return
+    except:
+        pass 
+
+
+
+test_sample_data = [    \
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0001", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "A1",\ 	
+               "sampleName": "MySample 1", \	
+               "samplePosition": 1,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0001", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "A1",\ 	
+               "sampleName": "MySample 2", \	
+               "samplePosition": 2,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0001", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "A1",\ 	
+               "sampleName": "MySample 3", \	
+               "samplePosition": 3,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \               
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0001", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "A1",\ 	
+               "sampleName": "MySample 4", \	
+               "samplePosition": 4,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0001", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "A1",\ 	
+               "sampleName": "MySample 5", \	
+               "sampleBarcode": "", 	\
+               "samplePosition": 5,\ 	
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \   
+
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0002", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "C2",\ 	
+               "sampleName": "MySample 1", \	
+               "samplePosition": 1,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0002", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "C2",\ 	
+               "sampleName": "MySample 2", \	
+               "samplePosition": 2,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0002", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "C2",\ 	
+               "sampleName": "MySample 3", \	
+               "samplePosition": 3,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \               
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0002", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "C2",\ 	
+               "sampleName": "MySample 4", \	
+               "samplePosition": 4,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \
+            {  "userName": "Alexandre", \  
+               "dewarName": "TEST", \	
+               "puckName": "My puck", 	\
+               "puckBarcode": "AAA0002", \	
+               "puckType": "unipuck", 	\
+               "puckAddress": "C2",\ 	
+               "sampleName": "MySample 5", \	
+               "samplePosition": 5,\ 	
+               "sampleBarcode": "", 	\
+               "sampleStatus": "Present", 	\
+               "sampleMountCount": 0,    
+               } , \                                                           
+      ] 
+  

script/data/set_samples_info.py

@@ -0,0 +1,4 @@
+
+
+
+set_samples_info(args[0])

script/devices/BarcodeReader.py

@@ -0,0 +1,83 @@
+
+
+class BarcodeReader(DeviceBase):
+
+    def __init__(self, name, microscan, microscan_cmd):
+        DeviceBase.__init__(self, name)
+        self.microscan = microscan
+        self.microscan_cmd = microscan_cmd
+
+        
+    def doInitialize(self):
+        self.disable()
+        self.readout = None
+        self.processing = False
+        self.task_callable=None
+        
+    def enable(self):
+        self.microscan_cmd.write("<H>")
+        self.setState(State.Ready)
+    
+    def disable(self):    
+        self.microscan_cmd.write("<I>")
+        self.setState(State.Disabled)
+    
+    def get(self,timeout=1.0):    
+        self.state.assertReady()
+        try:
+            self.setState(State.Busy)
+            self.microscan.flush()
+            ret =  self.microscan.waitString(int(timeout * 1000)) 
+            if ret is not None:
+                ret = ret.strip()   
+            self.setCache(ret, None)
+            return ret
+        except:
+            self.setCache(None, None)
+            return None
+        finally:
+            if self.state == State.Busy:
+                self.setState(State.Ready)
+
+    def doUpdate(self):
+        self.get()
+
+
+    def read(self,timeout=1.0):    
+        if self.processing:
+            raise Exception("Ongoing read operation")
+        self.processing = True
+        try:
+            initial = self.state
+            if initial == State.Disabled:
+                self.enable()
+            try:
+                return self.get(timeout)
+            finally:
+                if initial == State.Disabled:
+                    self.disable()
+        finally:
+            self.processing = False
+    
+    def _read_task(self, timeout):
+        global readout
+        self.readout = self.read(timeout)
+        return self.readout
+    
+    def start_read(self, timeout=1.0):
+        self.readout = None
+        self.task_callable = fork((self._read_task, (timeout,)))   
+
+    def get_readout(self):
+        return self.readout
+
+    def wait_readout(self):
+        if self.task_callable is not None:
+            join(self.task_callable)
+            self.task_callable = None
+        return self.readout
+
+
+add_device(BarcodeReader("barcode_reader", mscan_pin, mscan_pin_cmd), force = True)         
+add_device(BarcodeReader("barcode_reader_puck", mscan_puck, mscan_puck_cmd), force = True)         
+        

script/devices/Gonio.py

@@ -0,0 +1,43 @@
+
+
+def home_fast_table():
+    caput ("SAR-EXPMX1:ASYN.AOUT", "enable plc 1")
+
+def get_fx_pos():
+    return caget("SAR-EXPMX:MOT_FX.RBV",  'f') 
+
+def set_fx_pos(pos):
+    return caput("SAR-EXPMX:MOT_FX.VAL", float(pos)) 
+
+
+def get_fy_pos():
+    return caget("SAR-EXPMX:MOT_FY.RBV",  'f') 
+
+def set_fy_pos(pos):
+    return caput("SAR-EXPMX:MOT_FY.VAL", float(pos)) 
+
+def get_ry_pos():
+    return caget("SAR-EXPMX:MOT_ROT_Y.RBV",  'f') 
+
+def set_ry_pos(pos):
+    return caput("SAR-EXPMX:MOT_ROT_Y.VAL", float(pos)) 
+
+def get_cz_pos():
+    return caget("SAR-EXPMX:MOT_CZ.RBV",  'f') 
+
+def set_cz_pos(pos):
+    return caput("SAR-EXPMX:MOT_CZ.VAL", float(pos)) 
+
+def get_cx_pos():
+    return caget("SAR-EXPMX:MOT_CX.RBV",  'f') 
+
+def set_cx_pos(pos):
+    return caput("SAR-EXPMX:MOT_CX.VAL", float(pos))         
+        
+    
+def set_mount_position():
+    set_fx_pos(0.0)
+    set_fy_pos(0.0)
+    set_ry_pos(0.0)
+    set_cz_pos(0.0)
+    set_cx_pos(0.0)

script/devices/Hexiposi.py

@@ -0,0 +1,175 @@
+import ch.psi.pshell.device.DiscretePositionerBase as DiscretePositionerBase
+import requests
+import json
+
+class Hexiposi(DiscretePositionerBase):
+    def __init__(self, name, url):         
+         DiscretePositionerBase.__init__(self, name, ["A","B","C","D","E","F"])
+         self.PORT_SET=8002
+         self.PORT_GET=8002
+         if not url.startswith("http://"):
+             url = "http://" + url
+         if not url.endswith(":"):
+             url =  url + ":"
+         self.url_set = url + str (self.PORT_SET)+ "/TellWeb/"
+         self.url_get =  url + str (self.PORT_GET)+ "/TellWeb/"
+         self.moved = True     
+         self.homing_state = State.Disabled
+         self.rback = self.UNKNOWN_POSITION 
+         self.timeout = 3.0
+         self.offline = False
+
+    def doInitialize(self):           
+        super(Hexiposi, self).doInitialize()
+        self.val = self.doReadReadback()   
+        
+    def get_response(self, response):
+        if (response.status_code!=200):
+            raise Exception (response.text)
+        return json.loads(response.text)
+
+    def get_status(self):
+        try:
+            self.status =  self.get_response(requests.get(url=self.url_get+"get", timeout=self.timeout))
+            self.estop = self.status["estop"]
+            self.homed = self.status["homed"]
+            self.error = self.status["errorCode"]
+            self.remote = self.status["mode"] == "remote"
+            self.moving = self.status["errorCode"]
+            self.pos = self.status["position"]
+            self.moving = self.status["moving"]
+            self.offset = self.status["offset"]        
+            self.dpos = self.status["discretePosition"]    
+            if (self.homed==False): rback =  self.UNKNOWN_POSITION 
+            elif self.dpos == 1: rback = "B" # "A"
+            elif self.dpos == 2: rback = "C" # "B"
+            elif self.dpos == 4: rback = "D" #  "C"
+            elif self.dpos == 8: rback = "E" #  "D"
+            elif self.dpos == 16: rback = "F" #  "E"
+            elif self.dpos == 32: rback = "A" # "F"
+            else: rback = self.UNKNOWN_POSITION 
+            if (rback == self.UNKNOWN_POSITION) or (rback != self.rback):
+                self.moved = True
+            self.rback = rback
+            self.offline = False
+            return self.status
+        except:
+            self.offline = True
+            self.updateState()
+            raise
+    
+    def set_deadband(self, value = 0.1):        #degrees
+        ret = self.get_response(requests.get(url=self.url_set+"setDeadband?deadband=" + str(value), timeout=self.timeout))
+        if ret["deadbandOutput"] == value:
+            return value
+        raise Excepiton("Error setting deadband: " + str(ret))
+
+    def move_pos(self, pos):        
+        return self.get_response(requests.get(url=self.url_set+"set?pos=" + str(pos), timeout=self.timeout))
+
+    def move_home(self):                
+        ret = self.get_response(requests.get(url=self.url_set+"set?home=1", timeout=self.timeout))
+        try:
+            self.waitState(self.homing_state,1200)                
+        except:
+            pass
+        return ret
+
+    def stop_move(self):
+        return self.get_response(requests.get(url=self.url_set+"set?stop=1", timeout=self.timeout))
+
+    def set_offset(self, offset):
+        return self.get_response(requests.get(url=self.url_set+"setOffset?offset="+str(offset), timeout=self.timeout))
+     
+    def doUpdate(self):
+        self.get_status()
+        super(Hexiposi, self).doUpdate()
+        
+
+    def doStop(self):
+        self.stop_move()
+
+    def doRead(self):
+        return str(self.val)
+    
+    def doReadReadback(self):
+        self.get_status()
+        return self.rback
+
+    def doWrite(self, val):
+        #val = ord(val) - ord('A') +1  
+        val = ord(val) - ord('B') +1
+        if val==0: val=6  #A            
+        if val<1 or val>6:
+            raise Exception("Invalid value: " + str(val))
+        moving = val != self.val 
+        self.val = val
+        self.move_pos(self.val)
+        #Workaround as state does not changes immediatelly
+        if moving:
+            #try:
+            #    self.waitState(State.Busy,1200)                
+            #except:
+            #    print sys.exc_info()[1]
+            start = time.time()
+            while self.state != State.Busy:
+                if time.time() - start > 1.5:
+                    print "Timeout waiting Hexiposi busy"
+                    break
+                self.update()   
+
+    def is_in_position(self, pos):
+        return self.take() == pos
+
+
+    def assert_in_position(self, pos):
+        if not is_in_position(pos):
+            raise Exception ("Hexiposi is not in position")
+    
+    def assert_homed(self):
+        if self.homed == False:
+            raise Exception ("Hexiposi is not homed")
+        
+    def assert_in_known_position(self):
+        self.get_status()
+        if self.rback == 'Unknown':
+            raise Exception("Hexiposi is in an unknown position, please home.")
+
+    #def isReady(self):
+    #    self.get_status()
+    #    return self.moving == False
+    
+    def updateState(self):   
+        if self.isSimulated():
+            self.setState(State.Ready)
+        elif self.offline: 
+            self.setState(State.Offline)                    
+        elif self.homed == False:
+            self.setState(self.homing_state)            
+        elif self.moving: 
+            self.setState(State.Busy)            
+        else:
+            self.setState(State.Ready)
+    
+        
+#http://myriox06da:8002/TellWeb/get 
+dev = Hexiposi("hexiposi", "myriotell6d")
+
+#If no Rotation Lid is mounted set it to simulated
+#dev.setSimulated()
+
+add_device(dev, True)
+hexiposi.polling=1000
+#print dev.url
+#print dev.get_status()
+
+class hexiposi_position(ReadonlyRegisterBase):
+    def doRead(self):
+        try:
+            return float(hexiposi.pos)
+        except:
+            return float("nan")
+            
+add_device(hexiposi_position(), True)
+hexiposi_position.polling = 1000
+hexiposi.set_deadband(1.0)

script/devices/LaserDistance.py

@@ -0,0 +1,27 @@
+class LaserDistance(ReadonlyRegisterBase):
+    def __init__(self):
+        ReadonlyRegisterBase.__init__(self, "laser_distance")
+        
+    def doRead(self):
+        ret = ue.readable.read()
+        ret = None if ret is None else (0.0 if math.isnan(ret) else ret)
+        return ret
+
+class ListenerAI (DeviceListener):
+    def onValueChanged(self, device, value, former):        
+        if laser_distance is not None:                    
+            value = None if value is None else (0.0 if math.isnan(value) else value)                        
+            laser_distance.setCache(value, None)            
+
+
+for l in ue.listeners:
+    if Nameable.getShortClassName(l.getClass()) == "ListenerAI":
+        ue.removeListener(l)
+
+
+listenerAI = ListenerAI()
+ue.addListener(listenerAI)
+        
+laser_distance=LaserDistance()   
+add_device(laser_distance, True)
+laser_distance.update()

script/devices/LedCtrl.py

@@ -0,0 +1,46 @@
+import ch.psi.pshell.device.DiscretePositionerBase as DiscretePositionerBase
+
+
+class LedPositioner(DiscretePositionerBase):
+    def __init__(self):
+        DiscretePositionerBase.__init__(self, "led_ctrl", ["On", "Off"])
+        self.setState(State.Ready)
+        self.val = self.doReadReadback()
+        
+    def doRead(self):
+        return self.val
+    
+    def doReadReadback(self):
+        return "On" if get_led_state() else "Off"
+
+    def doWrite(self, val):
+        self.val = val
+        if self.val == "On":
+            set_led_state(True)
+        else:
+            set_led_state(False)
+
+
+add_device(LedPositioner(), True)
+led_ctrl.polling = 1000
+
+
+
+
+import ch.psi.pshell.device.ProcessVariableConfig as ProcessVariableConfig
+class LedLevel(ProcessVariableBase):
+    def __init__(self, name):
+        ProcessVariableBase.__init__(self, name, ProcessVariableConfig())
+        
+    def doRead(self):
+        return get_led_level()
+        
+    def doWrite(self, val):
+        return set_led_level(val)
+        
+led_level = LedLevel("led_level")
+led_level.config.minValue = 0.0
+led_level.config.maxValue = 100.0  
+led_level.config.unit = "%"
+add_device(led_level, True)
+led_level.polling = 1000

script/devices/OneWire.py

@@ -0,0 +1,134 @@
+import traceback
+from datetime import datetime
+
+class Detector(ReadonlyRegisterBase):
+    def __init__(self, name):
+        ReadonlyRegisterBase.__init__(self, name) 
+        self.reset()
+    
+    def set_inputs(self, inputs):
+        self.inputs = inputs
+        if self.take() != inputs.values():
+            self.setCache(inputs.values(), None)    
+            if self.getParent()!=None:
+                self.getParent().updateCache()
+        if (len(self.take()) == 0):
+            self.setState(State.Offline)        
+        else: 
+            self.setState(State.Ready)        
+
+    def set_input(self, index, val):
+        self.inputs[index] = val
+        self.set_inputs(self.inputs)      
+
+    def reset(self):
+        self.id = None
+        self.sn = None
+        self.status = None
+        self.type = None
+        self.set_inputs({})        
+
+class Esera(TcpDevice):    
+    def __init__(self, name, server, timeout = 1000, retries = 1):
+         TcpDevice.__init__(self, name, server) 
+         self.setMode(self.Mode.FullDuplex)
+         self.detectors = []
+         for i in range(30):
+             self.detectors.append(Detector("Detector " + str(i+1)))    
+         self.setChildren(self.detectors)     
+         self.completed_initializatiod = False         
+         self.debug = False         
+
+    def start(self):
+        self.getLogger().info("Starting controller")
+        self.write("set,sys,run,1\n")
+
+    def stop(self):
+        self.getLogger().info("Stopping controller")
+        self.write("set,sys,run,0\n")
+
+    def list(self):
+        self.write("get,owb,listall1\n")
+
+    def req_data(self):
+        self.write("get,sys,data\n")
+
+    def doInitialize(self):           
+        super(Esera, self).doInitialize()
+        self.init_timestamp = time.time()
+        try:
+            self.setState(State.Ready) #So can communicate
+            for det in self.detectors:
+                 det.reset()
+            self.list()    
+            time.sleep(1.0)    
+            self.check_started()
+            self.req_data()
+        except:
+            print  >> sys.stderr, traceback.format_exc()
+            self.getLogger().warning(traceback.format_exc())
+            raise
+
+    def doUpdate(self):
+        self.check_started()
+        self.req_data()     
+
+    def updateCache(self):
+        #print "Update"
+        cache = []
+        for det in self.detectors:
+            cache.append(det.take())                          
+        self.setCache(cache, None)    
+        
+
+    def check_started(self):          
+        if not self.completed_initializatiod:
+            init = True
+            for det in self.detectors:
+                if det.id == None:
+                    init = False
+                    break           
+            if init:                
+                self.completed_initializatiod = True
+                print("Completed initialization")
+                self.getLogger().info("Completed initialization")
+                self.start()
+     
+    def onString(self, msg):
+        if self.debug:
+            print datetime.now()  , " - " , msg
+        tokens = msg.split("|")
+        if len(tokens)>1:
+            try:
+                if msg[:3] == "LST":                
+                    #LST|1_OWD1|3AF361270000009E|S_0|DS2413| 
+                    if tokens[1] > 1:                
+                        index = int(tokens[1].split("_")[1][3:]) - 1
+                        if index < len(self.detectors):
+                            det = self.detectors[index]
+                            det.id = tokens[1] 
+                            det.sn= tokens[2] if len(tokens)>2 else None
+                            det.status = int(tokens[3][2:]) if len(tokens)>3 else None
+                            det.type = tokens[4] if len(tokens)>4 else None                    
+                            if det.status!= 0:
+                                det.set_inputs({})
+                else:                                  
+                     for det in self.detectors:
+                         if det.id is not None and msg.startswith(det.id):                             
+                             det_id = int(tokens[0][len(det.id)+1:])
+                             det.set_input(det_id, int(tokens[1]))
+            except:
+                print  >> sys.stderr, traceback.format_exc()
+                self.getLogger().log(traceback.format_exc())            
+
+     
+
+
+
+
+
+add_device(Esera("onewire", "129.129.126.83:5000"), force = True)   
+onewire.setPolling(500)
+add_device(onewire.detectors[0], force = True)   
+add_device(onewire.detectors[1], force = True)   
+add_device(onewire.detectors[2], force = True)   

script/devices/RobotModbus.py

@@ -0,0 +1,46 @@
+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_mb"), force = True)                
+        

script/devices/RobotMotors.py

@@ -0,0 +1,62 @@
+
+import ch.psi.pshell.device.PositionerConfig as PositionerConfig
+
+
+
+ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]
+
+
+class RobotCartesianMotor (PositionerBase):
+    def __init__(self, robot, index):
+         PositionerBase.__init__(self, robot.name + "_" + ROBOT_MOTORS[index], PositionerConfig())
+         self.index = index
+         self.robot = robot
+        
+    #ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True))
+    def doInitialize(self):
+        self.setCache(self.doRead(), None)
+                  
+    def doRead(self):
+        return float("nan") if self.robot.cartesian_destination is None else float(self.robot.cartesian_destination[self.index])
+
+    def doWrite(self, value):        
+        if self.robot.cartesian_destination is not None:
+            #print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value)
+            self.robot.cartesian_destination[self.index] = float(value)  
+            self.robot.set_pnt(robot.cartesian_destination , "tcp_p")        
+            self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN)        
+        
+    def doReadReadback(self):
+        return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.index])
+
+
+
+
+ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"]
+
+
+class RobotJointMotor (PositionerBase):
+    def __init__(self, robot, index):
+         PositionerBase.__init__(self, robot.name + "_" + ROBOT_JOINT_MOTORS[index], PositionerConfig())
+         self.index = index
+         self.robot = robot         
+            
+    def doInitialize(self):        
+        self.setpoint = self.doReadReadback()
+        self.setCache(self.doRead(), None)        
+                  
+    def doRead(self):        
+        return self.setpoint
+
+    def doWrite(self, value):                
+        #print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value)
+        self.setpoint = value
+        joint = self.robot.herej()
+        joint[self.index] = value
+        self.robot.set_jnt(joint, name="tcp_j")
+        self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN)       
+    
+    def doReadReadback(self):
+        return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index])
+        
+                

script/devices/RobotSC.py

@@ -0,0 +1,358 @@
+TOOL_CALIBRATION = "tCalib"
+TOOL_SUNA= "tSuna"
+TOOL_DEFAULT= TOOL_SUNA
+
+FRAME_TABLE = "fTable"
+
+DESC_FAST = "mFast"
+DESC_SLOW = "mSlow"
+DESC_SCAN = "mScan"
+DESC_DEFAULT = DESC_FAST
+
+AUX_SEGMENT = "X"
+
+
+DEFAULT_ROBOT_POLLING = 500
+TASK_WAIT_ROBOT_POLLING = 50
+
+
+run("devices/RobotTCP")
+
+
+simulation = False
+
+joint_forces = False
+
+class RobotSC(RobotTCP):    
+    def __init__(self, name, server, timeout = 1000, retries = 1):
+        RobotTCP.__init__(self, name, server, timeout, retries)   
+        self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "recover", "moveDewar", "moveCold", "movePark", "moveGonio","moveHeater", "moveScanner","moveHome", "moveAux"])
+        self.set_known_points(["pPark", "pGonioA", "pDewar", "pGonioG", "pScan", "pHeater", "pHeat", "pHeatB", "pLaser","pHelium", "pHome", "pCold", "pAux"])
+        self.setPolling(DEFAULT_ROBOT_POLLING)          
+        self.last_command_timestamp = None
+        self.last_command_position = None
+        #self.setSimulated()   
+        
+    def move_dewar(self):
+        self.start_task('moveDewar')  
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_dewar()
+        self.last_command_position = "dewar"
+        self.last_command_timestamp = time.time()
+
+    def move_cold(self):
+        self.start_task('moveCold')  
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_cold()
+        self.last_command_position = "cold"
+        self.last_command_timestamp = time.time()
+
+    def move_home(self):
+        self.start_task('moveHome')  
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_home()
+        self.last_command_position = "home"
+        self.last_command_timestamp = time.time()
+
+    def get_dewar(self, segment, puck, sample):
+        segment = self.toSegmentNumber(segment)
+        self.start_task('getDewar',segment, puck, sample, is_room_temp()) 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_dewar()
+        self.last_command_position = "dewar"
+        self.last_command_timestamp = time.time()
+
+    def put_dewar(self, segment, puck, sample):
+        segment = self.toSegmentNumber(segment)
+        self.assert_dewar()
+        self.start_task('putDewar',segment, puck, sample, is_room_temp()) 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        #self.assert_dewar()
+        self.assert_cold()
+        self.last_command_position = "dewar"
+        self.last_command_timestamp = time.time()
+
+    def put_gonio(self):  
+        assert_detector_safe()  
+        pin_offset = get_pin_offset()
+        pin_angle_offset = get_pin_angle_offset()
+        print "Pin offset = " + str(pin_offset)
+        self.start_task('putGonio', pin_offset) 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_gonio()
+        self.last_command_position = "gonio"
+        self.last_command_timestamp = time.time()
+
+    def get_gonio(self):
+        assert_detector_safe()
+        pin_offset = get_pin_offset()
+        print "Pin offset = " + str(pin_offset)
+        self.start_task('getGonio', pin_offset) 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_gonio()
+        self.last_command_position = "gonio"
+        self.last_command_timestamp = time.time()
+
+    def get_aux(self, sample):
+        self.assert_aux()
+        self.start_task('getAuxiliary', sample) 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_aux()
+        self.last_command_position = "aux"
+        self.last_command_timestamp = time.time()
+
+    def put_aux(self, sample):
+        self.assert_aux()
+        self.start_task('putAuxiliary', sample) 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_aux()
+        self.last_command_position = "aux"
+        self.last_command_timestamp = time.time()
+
+    def move_scanner(self):
+        self.start_task('moveScanner') 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_scanner()
+        self.last_command_position = "scanner"
+        self.last_command_timestamp = time.time()
+
+    def move_laser(self):
+        self.start_task('moveScanner') 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_laser()
+        self.last_command_position = "scanner"
+        self.last_command_timestamp = time.time()
+
+    #def do_scan(self):
+    #    self.start_task('doScan') 
+    #    self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+    #    self.assert_scan_stop()
+
+    def move_gonio(self):
+        assert_detector_safe()
+        self.start_task('moveGonio') 
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_gonio()
+        self.last_command_position = "gonio"
+        self.last_command_timestamp = time.time()
+ 
+
+    def move_park(self):
+        self.start_task('movePark')   
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_park()
+        self.last_command_position = "park"
+        self.last_command_timestamp = time.time()
+
+    def move_heater(self, speed=-1, to_bottom=True):
+        self.start_task('moveHeater', speed, to_bottom)   
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        if to_bottom:
+            self.assert_heater_bottom()
+        else:
+            self.assert_heater()
+        self.last_command_position = "heater"
+        self.last_command_timestamp = time.time()
+
+
+    def robot_recover(self):
+        self.start_task('recover')
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_home()
+        self.last_command_position = "home"
+        self.last_command_timestamp = time.time()
+
+    def move_aux(self):
+        self.start_task('moveAux')  
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_aux()
+        self.last_command_position = "aux"
+        self.last_command_timestamp = time.time()
+
+    def get_calibration_tool(self):
+        self.start_task('getCalTool')  
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)  
+        self.assert_scanner() 
+        self.last_command_position = "scanner"
+        self.last_command_timestamp = time.time()
+    
+    def put_calibration_tool(self):
+        self.start_task('putCalTool')  
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_scanner() 
+        self.command_timestamps["scanner"] = time.time()          
+
+        
+
+    def toSegmentNumber(self, segment):
+        if is_string(segment):
+            segment = ord(segment.upper()) - ord('A') +1
+        return segment
+    
+       
+    def on_event(self,ev):
+        #print "EVT: " + ev
+        pass
+    def on_change_working_mode(self, working_mode):
+        if get_device("hexiposi") is not None:
+            hexiposi.moved = True    #Force image processing on first sample
+    
+    def doUpdate(self):    
+        #start = time.time()        
+        RobotTCP.doUpdate(self)
+        global simulation
+        if not simulation:
+            if joint_forces:
+                if self.state != State.Offline:
+                    self.get_joint_forces()
+                    for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: 
+                        dev.update()        
+        #print time.time() -start
+
+    def start_task(self, program, *args,  **kwargs):
+        #TODO: Check safe position
+        return RobotTCP.start_task(self, program, *args,  **kwargs)
+
+    def stop_task(self):
+        RobotTCP.stop_task(self)
+        #TODO: Restore safe position          
+          
+
+    def set_remote_mode(self):
+        robot.set_profile("remote")
+                
+    def set_local(self):
+        robot.set_profile("default")   
+    
+    def is_park(self):
+        return  self.is_in_point("pPark")   
+
+    def is_cold(self):
+        return  self.is_in_point("pCold")   
+    
+    def is_home(self):
+        return  self.is_in_point("pHome")
+
+    def is_dewar(self):
+        return  self.is_in_point("pDewar")   
+
+    def is_heater(self):
+        return  self.is_in_point("pHeat")   
+
+    def is_heater_home(self):
+        return  self.is_in_point("pHeater")  
+
+    def is_heater_bottom(self):
+        return  self.is_in_point("pHeatB")   
+
+    def is_gonio(self):
+        return  self.is_in_point("pGonioA")   
+         
+    def is_helium(self):
+        return  self.is_in_point("pHelium")   
+
+    def is_scanner(self):
+        return  self.is_in_point("pScan") 
+
+    def is_aux(self):
+        return  self.is_in_point("pAux")
+                
+    def is_laser(self):
+        return  self.is_in_point("pLaser")   
+
+    def is_cleared(self):
+        #return self.is_home() or self.is_park() or self.is_dewar() or self.is_dewar_home()        
+        return self.get_current_point() is not None  
+
+    def assert_heater_home(self):
+        self.assert_in_point("pHeater") 
+
+    def assert_cold(self):
+        self.assert_in_point("pCold") 
+    
+    def assert_heater(self):
+        self.assert_in_point("pHeat") 
+
+    def assert_heater_bottom(self):
+        self.assert_in_point("pHeatB")
+
+    def assert_park(self):
+        self.assert_in_point("pPark")   
+    
+    def assert_home(self):
+        self.assert_in_point("pHome")
+
+    def assert_dewar(self):
+        self.assert_in_point("pDewar")   
+
+    def assert_gonio(self):
+        self.assert_in_point("pGonioA")  
+
+    def assert_helium(self):
+        self.assert_in_point("pHelium")
+
+    def assert_scanner(self):
+        self.assert_in_point("pScan") 
+
+    def assert_aux(self):
+        self.assert_in_point("pAux") 
+
+    def assert_laser(self):
+        self.assert_in_point("pLaser")   
+
+    def assert_cleared(self):
+        if not self.is_cleared():
+            raise Exception("Robot not in cleared position")  
+
+    def wait_ready(self):
+        robot.waitState(State.Ready, 1000) #robot.state.assertReady()
+
+if simulation:
+    add_device(RobotSC("robot","localhost:1000"),force = True)
+else:
+    add_device(RobotSC("robot", "TellRobot6D:1000"), force = True) 
+    #add_device(RobotSC("robot", "129.129.110.110:1000"), force = True)      
+
+#robot.latency = 0.005   
+robot.set_default_desc(DESC_DEFAULT)
+robot.default_speed = 20
+robot.set_frame(FRAME_DEFAULT)
+robot.set_tool(TOOL_DEFAULT)
+robot.setPolling(DEFAULT_ROBOT_POLLING)       
+   
+robot.get_current_point()   #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE
+
+class jf1(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[0]
+class jf2(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[1]
+class jf3(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[2]
+class jf4(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[3]
+class jf5(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[4]
+class jf6(ReadonlyRegisterBase):
+    def doRead(self):
+        return  None if robot.joint_forces == None else robot.joint_forces[5]
+class jfc(ReadonlyRegisterBase):
+    def doRead(self):
+        if robot.joint_forces == None:
+            return   float('NaN')  
+        if robot.powered == False:
+            return  float('NaN') 
+        return (robot.joint_forces[1]+74)/4  + (robot.joint_forces[2]+30)/4 + (robot.joint_forces[4]-0.8)/0.2
+
+if joint_forces:
+    add_device(jf1(), force = True)   
+    add_device(jf2(), force = True)   
+    add_device(jf3(), force = True)   
+    add_device(jf4(), force = True)   
+    add_device(jf5(), force = True)   
+    add_device(jf6(), force = True)   
+    add_device(jfc(), force = True)   

script/devices/RobotTCP.py

@@ -0,0 +1,919 @@
+import threading
+
+FRAME_DEFAULT = "world"
+FLANGE = "flange"
+
+MAX_NUMBER_PARAMETERS = 20
+
+run("devices/RobotMotors")
+
+class RobotTCP(TcpDevice, Stoppable):    
+    def __init__(self, name, server, timeout = 1000, 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    
+         self.working_mode = "invalid"     
+         self.status = "invalid" 
+         self.powered = None
+         self.settled = None
+         self.empty = None
+         self.working_mode = None
+         self.status = None
+         self.lock = threading.Lock()
+         self.joint_forces = None
+         self.current_task = None         
+         self.current_task_ret = None
+         self.high_level_tasks = []    
+         self.known_points = []     
+         self.current_points = []
+         self.cartesian_destination = None  
+         #self.flange_pos = [None] * 6
+         self.cartesian_pos = [None] * 6
+         self.joint_pos = [None] * 6
+         self.cartesian_motors_enabled = False
+         self.cartesian_motors = []
+         self.joint_motors_enabled = False
+         self.joint_motors = []
+         self.tool = None
+         self.default_desc = None
+         self.tool_open = None
+         #self.tool_trsf = [0.0] * 6
+         self.frame = FRAME_DEFAULT
+         self.polling_interval = 0.01
+         self.reset = True           
+         self.default_tolerance = 5    
+         self.default_speed = 100   
+         self.latency = 0
+         self.last_msg_timestamp = 0
+
+         self.task_start_retries = 3
+         self.exception_on_task_start_failure = True  #Tasks may start and be finished when checked
+                  
+
+    def doInitialize(self):           
+        super(RobotTCP, self).doInitialize()
+        self.reset = True        
+
+    def set_tool(self, tool):
+        self.tool = tool
+        #self.tool_trsf = self.get_tool_trsf()
+        self.evaluate("tcp_curtool=" + tool)         
+        if self.cartesian_motors_enabled:            
+            self.update()
+            self.set_motors_enabled(True)       
+        self.is_tool_open()     
+    
+    def get_tool(self):    
+        return self.tool
+
+    def set_frame(self, frame):
+        self.frame = frame      
+        self.evaluate("tcp_curframe=" + frame)               
+        if self.cartesian_motors_enabled:            
+            self.update()
+            self.set_motors_enabled(True)       
+            self.waitCacheChange(5000)
+    
+    def get_frame(self):    
+        return self.frame
+
+    def set_default_frame(self):
+         self.set_frame(FRAME_DEFAULT)
+    
+    def assert_tool(self, tool=None):
+        if tool  is None: 
+            if self.tool is None:
+                 raise Exception("Tool is undefined")    
+        elif self.tool != tool:
+            raise Exception("Invalid tool: " + self.tool)        
+
+    def set_default_desc(self,default_desc):
+        self.default_desc=default_desc
+    
+    def get_default_desc(self):    
+        return self.default_desc
+
+    def set_tasks(self,tasks):
+        self.high_level_tasks=tasks
+    
+    def get_tasks(self):    
+        return self.high_level_tasks
+
+    def set_known_points(self, points):
+        self.known_points=points
+    
+    def get_known_points(self):    
+        return self.known_points
+
+    def get_current_points(self, tolerance = None):  
+        ret = self.is_in_points(*self.known_points, tolerance = tolerance)
+        current_points = []
+        for i in range(len(ret)):
+            if ret[i] == True:
+                current_points.append(self.known_points[i])
+        return  current_points 
+            
+    def get_current_point(self, tolerance = None): 
+        current_points = self.get_current_points(tolerance)
+        if (current_points is not None) and ( len(current_points) >0):
+            return current_points[0] 
+        return None
+
+    def get_current_points_cached(self):
+        return self.current_points
+    
+    def get_current_point_cached(self):
+        if (self.current_points is not None) and (len (self.current_points) >0):
+            return  self.current_points[0] 
+        return None
+        
+    def assert_in_known_point(self, tolerance = None):
+        if self.get_current_point(tolerance) is None:
+            raise Exception ("Robot not in known point")        
+
+    def _sendReceive(self, msg_id, msg = "", timeout =  None):  
+        if self.latency >0:
+            timespan = time.time() - self.last_msg_timestamp
+            if timespan<self.latency:
+                time.sleep(self.latency-timespan)
+        tx = self.header if (self.header != None) else  ""
+        tx = tx + msg_id + " " + msg        
+        if (len(tx)>150):
+            raise Exception("Exceeded maximum message size")
+        self.getLogger().finer("TX = '" +  str(tx)+ "'")
+        if (self.trailer != None): tx = tx + self.trailer
+        if self.isSimulated():
+            return ""        
+        rx = self.sendReceive(tx, msg_id, self.trailer , 0,  self.timeout if timeout is None  else timeout, self.retries)
+        self.last_msg_timestamp = time.time()
+        rx=rx[:-1] #Remove 0A
+        self.getLogger().finer("RX = '" + str(rx) + "'")
+        if rx[:3] != msg_id:                            
+            if (time.time()-start) >= timeout:
+                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, timeout =  None):                
+        self.lock.acquire()
+        try:
+            id = "%03d" % self.msg_id
+            self.msg_id = (self.msg_id+1)%1000        
+            return self._sendReceive(id, msg, timeout)        
+        finally:
+            self.lock.release()
+    
+    def execute(self, command, *args, **kwargs):        
+        timeout = None if (kwargs is None) or (not kwargs.has_key("timeout")) else kwargs["timeout"]                
+        msg = str(command)
+        if len(args)>MAX_NUMBER_PARAMETERS:
+            raise Exception("Exceeded maximum number of parameters")
+        for i in range(len(args)):            
+            msg += (self.cmd_separator if  (i==0) else self.array_separator) + str(args[i])                 
+        rx = self.call(msg, timeout)
+        if rx.count(self.array_separator)>0:
+            return rx.split(self.array_separator)
+        return rx
+         
+    def evaluate(self, cmd, timeout=None):
+        ret = self.execute('eval', cmd, timeout=timeout)
+        if is_string(ret):
+            if ret.strip() != "": raise Exception(ret)
+
+    def get_var(self, name):
+        return self.execute('get_var', name)
+    
+    #Makes app crash
+    #def get_str(self, name='s'):
+    #    return self.execute('get_str', name)
+
+    def get_arr(self, name, size):
+        return self.execute('get_arr', name, size)
+
+    def get_bool(self, name = "tcp_b"):
+        return True if (self.execute('get_bool', name).strip() == '1') else False        
+
+    def get_int(self, name ="tcp_n"):
+        return int(self.get_var(name))
+
+    def get_float(self, name ="tcp_n"):
+        return float(self.get_var(name))
+
+    def get_int_arr(self, size, name="tcp_a"):
+        # not working. A Jython bug in PyUnicaode?
+        #return [int(x) for x in self.get_arr("tcp_a", size)] 
+        ret = []
+        a=self.get_arr(name, size)
+        for i in range(size):
+            ret.append(int(a[i]))
+        return ret
+
+    def get_float_arr(self, size, name="tcp_a"):
+        #return [float(x) for x in self.get_arr("tcp_a", size)]        
+        a=self.get_arr(name, size)
+        ret = []; 
+        for i in range(size): ret.append(float(a[i])); 
+        return ret        
+    
+    def get_trsf(self, name="tcp_t"):
+        a = self.execute('get_trf', name)
+        ret = []
+        for i in range(6): ret.append(float(a[i]))
+        return ret        
+
+    def set_trsf(self, l, name="tcp_t"):             
+        self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
+
+    def get_jnt(self, name="tcp_j"):
+        a = self.execute('get_jnt', name)
+        ret = []
+        for i in range(6): ret.append(float(a[i]))
+        return ret   
+
+    def set_jnt(self, l, name="tcp_j"):             
+        self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
+
+    def get_pnt(self, name="tcp_p"):
+        #a = self.execute('get_pnt', name)
+        #ret = []
+        #for i in range(6): ret.append(float(a[i]))
+        #return ret        
+        return self.get_trsf(name+".trsf")
+        
+    #trsf = (x,y,z,rx,ry,rz)
+    #TODO: config = (shoulder, elbow, wrist)
+    def set_pnt(self, trsf, name="tcp_p", config=None):
+        self.set_trsf(trsf, name+".trsf")       
+
+    def get_tool_trsf(self, name=None):
+        if name is None: 
+            self.assert_tool()
+            name = self.tool
+        return self.get_trsf(name+".trsf")
+
+    def set_tool_trsf(self, trsf, name=None):
+        if name is None: 
+            self.assert_tool()
+            name = self.tool
+        self.set_trsf(trsf, name+".trsf")       
+
+    def eval_int(self, cmd):
+        if self.isSimulated():
+            return 0
+        self.evaluate("tcp_n=" + cmd)
+        return self.get_int()
+
+    def eval_float(self, cmd):
+        if self.isSimulated():
+            return 0.0                
+        self.evaluate("tcp_n=" + cmd)
+        return self.get_float()
+
+    def eval_bool(self, cmd):
+        if self.isSimulated():
+            return False    
+        self.evaluate("tcp_b=" + cmd)
+        return self.get_bool()
+
+    #def eval_str(self, cmd):
+    #    self.evaluate("s=" + cmd)
+    #    return self.get_str()
+
+    def eval_jnt(self, cmd):
+        self.evaluate("tcp_j=" + cmd)
+        return self.get_jnt()
+
+    def eval_trf(self, cmd):
+        self.evaluate("tcp_t=" + cmd)
+        return self.get_trsf()
+
+    def eval_pnt(self, cmd):
+        self.evaluate("tcp_p=" + cmd)
+        return self.get_pnt()
+
+
+    #Robot control        
+    def is_powered(self):
+        self.powered = self.eval_bool("isPowered()")
+        return self.powered
+
+    def enable(self):
+        if not self.is_powered():
+            self.evaluate("enablePower()")
+            time.sleep(1.0)
+            if not self.is_powered():
+                raise Exception("Cannot enable power")
+    
+    #waits for power to be actually cut off
+    def disable(self):
+        self.evaluate("disablePower()", timeout=5000)
+
+    def get_monitor_speed(self):
+        self.speed =  self.eval_int("getMonitorSpeed()")
+        return self.speed
+
+    def set_monitor_speed(self, speed):
+        ret = self.eval_int("setMonitorSpeed(" + str(speed) + ")")
+        if (ret==-1): raise Exception("The robot is not in remote working mode")
+        if (ret==-2): raise Exception("The monitor speed is under the control of the operator")
+        if (ret==-3): raise Exception("The specified speed is not supported")
+
+    def set_default_speed(self):
+        set_monitor_speed(self.default_speed)
+    
+    def is_calibrated(self):
+        return self.eval_bool("isCalibrated()") 
+
+    def save_program(self):
+        ret = self.execute('save',  timeout=5000)        
+        if str(ret) != "0": raise Exception("Error saving program: " + str(ret))
+            
+    def _update_working_mode(self, mode, status):
+        cur_mode = self.working_mode
+        if mode==1:                 
+            self.working_mode = "manual"
+            self.status = "hold" if status==6 else "move"
+        elif mode==2: 
+            self.working_mode = "test"
+            self.status = "hold" if status==3 else "move"
+        elif mode==3: 
+            self.working_mode = "local"
+            self.status = "hold" if status==2 else "move"
+        elif mode==4: 
+            self.working_mode = "remote"
+            self.status = "hold" if status==2 else "move"
+        else:
+            self.working_mode = "invalid"
+            self.status = "invalid"
+        if self.working_mode != cur_mode:
+            try:
+                self.on_change_working_mode(self.working_mode)
+            except:
+                pass
+    
+    def read_working_mode(self):
+        try:
+            mode =  self.eval_int("workingMode(tcp_a)")          
+            status = int(self.get_var("tcp_a[0]"))                        
+            self._update_working_mode(mode, status)
+            self._update_state()    
+        except:
+            self.working_mode = "invalid"
+            self.status = "invalid"        
+        return self.working_mode
+
+    def get_emergency_stop_sts(self):
+        st = self.eval_int("esStatus()")        
+        if (st== 1): return "active"
+        if (st== 2): return "activated"
+        return "off"
+
+    def get_safety_fault_signal(self):
+        fault = self.eval_bool("safetyFault(s)")
+        #if (fault):
+        #    return get_str()            
+        return fault
+
+    #Motion control
+    def stop(self):
+        self.evaluate("stopMove()")
+
+    def resume(self):
+        self.evaluate("restartMove()")
+
+    def reset_motion(self, joint=None, timeout=None):
+        #TODO: in new robot robot.resetMotion() is freezing controller
+        #self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")"))
+        if joint is None:
+            self.execute('reset', timeout=timeout)
+        else:
+            self.execute('reset', str(joint), timeout=timeout)
+
+    def is_empty(self):
+        self.empty = self.eval_bool("isEmpty()")
+        self._update_state()    
+        return self.empty
+
+    def is_settled(self):
+        self.settled = self.eval_bool("isSettled()")
+        self._update_state()    
+        return self.settled
+        
+    def get_move_id(self):
+        return self.eval_int("getMoveId()")
+
+    def set_move_id(self, id):
+        return self.evaluate("setMoveId(" + str(id) + " )")
+
+    def get_joint_forces(self):        
+        try:
+            self.evaluate("getJointForce(tcp_a)")        
+            self.joint_forces = self.get_float_arr(6)            
+            return self.joint_forces
+        except:
+            self.joint_forces = None
+            raise
+
+    def movej(self, joint_or_point, tool=None, desc=None, sync=False):
+        if desc is None: desc = self.default_desc
+        if tool is None: tool = self.tool
+        #If joint_or_point is a list assumes ir is a point
+        if not is_string(joint_or_point):
+            robot.set_pnt(joint_or_point , "tcp_p")   
+            joint_or_point =  "tcp_p"         
+                    
+        #TODO: in new robot movel and movej is freezing controller
+        #ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
+        ret = int(self.execute('movej',joint_or_point, tool, desc))
+        
+        if sync:
+            self.wait_end_of_move()        
+        return ret
+        
+    def movel(self, point, tool=None, desc=None, sync=False):
+        if desc is None: desc = self.default_desc
+        if tool is None: tool = self.tool
+        if not is_string(point):
+            robot.set_pnt(point , "tcp_p")   
+            point =  "tcp_p"        
+        #TODO: in new robot movel and movej is freezing controller
+        #ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
+        ret = int(self.execute('movel',point, tool, desc))
+        
+        if sync:
+            self.wait_end_of_move()        
+        return ret
+
+    def movec(self, point_interm, point_target, tool=None, desc=None, sync=False):
+        if desc is None: desc = self.default_desc
+        if tool is None: tool = self.tool
+        
+        #TODO: in new robot  movel and movej is freezing controller
+        #ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")
+        ret = int(self.execute('movec', point_interm, point_target, tool, desc))
+                
+        if sync:
+            self.wait_end_of_move()        
+        return ret
+
+    def wait_end_of_move(self):
+        time.sleep(0.05)
+        self.update()  
+        #time.sleep(0.01)
+        #self.waitCacheChange(-1)              
+        self.waitReady(-1)
+        #self.waitState(State.Ready, -1)
+            
+
+    #Tool - synchronized as can freeze communication
+    """
+    def open_tool(self, tool=None, timeout=3000):
+        #This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
+        self.waitState(State.Ready, -1)
+        if tool is None: tool = self.tool
+        return self.evaluate("open(" + tool + " )", timeout=timeout)
+
+    def close_tool(self, tool=None, timeout=3000):
+        #This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
+        self.waitState(State.Ready, -1)
+        if tool is None: tool = self.tool
+        return self.evaluate("close(" + tool + " )", timeout=timeout)
+    """
+    #Tool - Not synchronized calls: atention to open/close only when state is Ready
+    def open_tool(self, tool=None):
+        if tool is None: tool = self.tool
+        self.evaluate(tool + ".gripper=true")
+        self.tool_open = True
+
+    def close_tool(self, tool=None):
+        if tool is None: tool = self.tool
+        self.evaluate(tool + ".gripper=false")
+        self.tool_open = False
+
+    def is_tool_open(self, tool=None):
+        if tool is None: tool = self.tool
+        self.tool_open =  robot.eval_bool(tool + ".gripper")        
+        return self.tool_open
+
+
+    #Arm position
+    def herej(self):
+        return self.eval_jnt("herej()")
+    
+    def distance_t(self, trsf1, trsf2):    
+        return self.eval_float("distance(" + trsf1 + ", " + trsf2 + ")")
+    
+    def distance_p(self, pnt1, pnt2):    
+        return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")   
+
+    def compose(self, pnt, frame = None, trsf = "tcp_t"):    
+        if frame is None: frame = self.frame
+        return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")
+
+    def here(self, tool=None, frame=None):
+        if tool is None: tool = self.tool    
+        if frame is None: frame = self.frame
+        return self.eval_pnt("here(" + tool + ", " + frame + ")")
+
+    def joint_to_point(self, tool=None, frame=None, joint="tcp_j"):
+        if tool is None: tool = self.tool    
+        if frame is None: frame = self.frame
+        return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")
+
+    def point_to_joint(self, tool=None, initial_joint="tcp_j", point="tcp_p"):
+        if tool is None: tool = self.tool    
+        if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"):
+            return self.get_jnt()
+
+    def position(self, point, frame=None):
+        if frame is None: frame = self.frame
+        return self.eval_trf("position(" + point + ", " + frame + ")")
+    
+    #Profile
+    def get_profile(self):
+        return self.execute('get_profile', timeout=2000)
+
+    def set_profile(self, name, pwd = ""):
+        self.execute('set_profile', str(name), str(pwd), timeout=5000)
+    
+    
+    #Task control
+    def task_create(self, program, *args,  **kwargs):        
+        program = str(program)
+        priority = 10 if (kwargs is None) or (not kwargs.has_key("priority")) or (kwargs["priority"] is None) else kwargs["priority"]                
+        name = str(program if (kwargs is None) or (not kwargs.has_key("name")) else kwargs["name"])                
+        
+        if self.get_task_status(name)[0] != -1:
+            raise Exception("Task already exists: " + name)
+
+        if priority<1 or priority > 100:
+            raise Exception("Invalid priority: " + str(priority))
+
+        cmd = program + '('
+        for i in range(len(args)): 
+            val = args[i]
+            if type(val) == bool:
+                if val == True: val = "true"
+                elif val == False: val = "false"
+            cmd += str(val) + (',' if  (i<(len(args)-1)) else '')
+        cmd+=')'
+
+        #TODO: in new robot exec taskCreate is freezing controller
+        #REMOVE if bug is fixed
+        self.execute('task_create',name, str(priority), program, *args)
+        #self.evaluate('taskCreate "' + name + '", ' + str(priority) + ', ' + cmd)        
+                        
+        
+        
+    def task_suspend(self, name):
+        self.evaluate('taskSuspend("' + str(name)+ '")')
+
+    #waits until the task is ready for restart
+    def task_resume(self, name):
+        self.evaluate('taskResume("' + str(name)+ '",0)', timeout = 2000)
+
+    #waits for the task to be actually killed
+    def task_kill(self, name):
+        #self.evaluate('taskKill("' + str(name)+ '")', timeout = 5000)
+        self.execute('kill', str(name), timeout=5000)
+
+    def get_task_status(self, name):
+        if self.isSimulated():
+            return (-1,"Stopped")
+        code = self.eval_int('taskStatus("' + str(name)+ '")')
+        #TODO: String assignments in $exec causes application to freeze
+        #status = self
+        if code== -1: status = "Stopped"
+        elif code== 0: status = "Paused"
+        elif code== 1: status = "Running"
+        #else: status = self.execute('get_help', code)        
+        else: status = "Error"  
+        return (code,status)
+
+    def get_tasks_status(self, *pars):
+        ret = self.execute("task_sts", *pars)
+        ret = ret[0:len(pars)]
+        for i in range(len(ret)):
+            try:
+                ret[i] = int(ret[i])
+            except:
+                ret[i] = None   
+        return ret
+
+    def _update_state(self):
+        #self.setState(State.Busy if self.status=="move" else State.Ready)
+        if self.state==State.Offline:
+            print "Communication resumed"        
+        if self.reset or (self.state==State.Offline):
+            self.check_task()
+            if self.current_task is not None:
+                print "Ongoing task: " + self.current_task            
+        if (not self.settled) or (self.current_task is not None): self.setState(State.Busy)
+        elif not self.empty: self.setState(State.Paused)
+        else: self.setState(State.Ready)                
+
+    def doUpdate(self):        
+        try:
+            start = time.time()
+            cur_task = self.current_task #Can change in background so cache it
+            if self.isSimulated():
+                sts = [1, 1,"1", 100, "1", "1", 0, 0, \
+                       0, 0, 0, 0, 0, 0, \
+                       0, 0, 0, 0, 0, 0, \
+                      ]
+            else:
+                sts = self.execute("get_status", cur_task)
+            self._update_working_mode(int(sts[0]), int(sts[1]))
+            self.powered = sts[2] == "1"
+            self.speed = int(sts[3])
+            self.empty = sts[4] == "1"
+            self.settled = sts[5] == "1"
+           
+            #TODO: add tool open
+            if cur_task is not None:                                    
+                if int(sts[6]) < 0:         
+                    log("Task "+ str(cur_task) + " finished with code: " + str(sts[7]), False)
+                    try:
+                        self.current_task, self.current_task_ret = None, int(sts[7])
+                    except:
+                        self.current_task, self.current_task_ret = None, None
+            for i in range(6):
+                self.joint_pos[i] = float(sts[8 + i])
+            for i in range(6):
+                self.cartesian_pos[i] = float(sts[14 + i])
+
+            ev_index = 20 #7
+            ev = sts[ev_index] if len(sts)>ev_index else ""            
+            if len(ev.strip()) >0:
+                self.getLogger().info(ev)
+                self.on_event(ev) 
+               
+            self._update_state()    
+            self.reset = False
+            self.setCache({"powered": self.powered,
+                           "speed": self.speed,
+                           "empty": self.empty,
+                           "settled": self.settled,
+                           "task": cur_task,
+                           "mode": self.working_mode,
+                           "status": self.status,
+                           "open": self.tool_open,
+                           "pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion 
+                           }, None)
+
+            if self.cartesian_motors_enabled:
+                for m in self.cartesian_motors:
+                    m.readback.update()
+
+            if self.joint_motors_enabled:
+                for m in self.joint_motors:
+                    m.readback.update()
+                    
+        except:
+            if self.state != State.Offline:
+                print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
+                self.setState(State.Offline)            
+
+    #Cartesian space
+    """
+    def get_cartesian_pos(self): 
+        self.assert_tool()           
+        return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())")
+        #self.set_jnt(self.herej())
+        #return self.joint_to_point(tool, frame)        
+    """
+
+
+    #TODO: in new robot robot.evaluate("tcp_p=jointToPoint(tSuna, world, herej())") is freezing controller (cannot call herej directly)
+    #We can consider atomic because tcp_j is only accessed in comTcp
+    def get_cartesian_pos(self, tool=None, frame=None): 
+        if tool is None: 
+            self.assert_tool()           
+            tool = self.tool    
+        if frame is None: 
+            frame = self.frame
+         #Do not work        
+        #self.evaluate("tcp_j=herej(); tcp_p=jointToPoint(" + tool + ", " + frame + ", tcp_j)")
+        #return self.get_pnt()
+        a = self.execute('get_pos', tool, frame)
+        ret = []
+        for i in range(6): ret.append(float(a[i]))
+        return ret   
+
+
+    def get_flange_pos(self, frame=None):            
+        return get_cartesian_pos(FLANGE, frame)
+
+
+    def get_cartesian_destination(self, tool=None, frame=None):
+        if tool is None: 
+            self.assert_tool()           
+            tool = self.tool    
+        if frame is None: 
+            frame = self.frame        
+        return self.here(tool, frame)
+
+    def get_distance_to_pnt(self, name):   
+        #self.here(self.tool, self.frame) #???        
+        self.set_pnt(self.get_cartesian_pos() )
+        return  self.distance_p("tcp_p", name)
+
+    def is_in_point(self, p, tolerance = None): #Tolerance in mm
+        if (tolerance is None) and p in self.known_points:
+            #If checking a known point with default tolerance, updates the position cache
+            return p in self.get_current_points()
+        tolerance = self.default_tolerance if tolerance == None else tolerance
+        d = self.get_distance_to_pnt(p)
+        if d<0:
+            raise Exception ("Error calculating distance to " + p + " : " + str(d))
+        return d<tolerance
+
+    def get_distance_to_pnts(self, *pars):
+        ret = self.execute("dist_pnt", *pars)
+        ret = ret[0:len(pars)]
+        for i in range(len(ret)):
+            try:
+                ret[i] = float(ret[i])
+            except:
+                ret[i] = None         
+        return ret
+
+    def is_in_points(self, *pars, **kwargs): #Tolerance in mm
+        tolerance = self.default_tolerance if (kwargs is None) or (not kwargs.has_key("tolerance")) or (kwargs["tolerance"] is None) else kwargs["tolerance"]                
+        ret = self.get_distance_to_pnts(*pars)
+        for i in range(len(ret)):        
+            if ret[i]<0:
+                ret[i] = None
+            else:
+                ret[i] = ret[i]<tolerance
+        if (tolerance == self.default_tolerance) and (set(self.known_points).issubset(set(pars))): #Only update cache if tolerance is default
+            current_points = []
+            for i in range(len(ret)):
+                if ret[i] == True:
+                    current_points.append(self.known_points[i])        
+            self.current_points = current_points                       
+        return ret
+             
+    def assert_in_point(self, p, tolerance = None): #Tolerance in mm         
+        if not self.is_in_point(p, tolerance):
+            raise Exception ("Not in position " + p)
+
+    
+    #Cartesian peudo-motors
+    def set_motors_enabled(self, value):
+        if value !=self.cartesian_motors_enabled:
+            self.cartesian_motors_enabled = value
+            if value:    
+                for i in range(6):
+                    self.cartesian_motors.append(RobotCartesianMotor(self, i))
+                    add_device(self.cartesian_motors[i], True)        
+                self.cartesian_destination = self.get_cartesian_destination()
+            else:
+                for m in self.cartesian_motors:
+                    remove_device(m)
+                self.cartesian_motors = []            
+                self.cartesian_destination = None
+        else:
+            if value:
+                self.cartesian_destination = self.get_cartesian_destination()
+                for m in self.cartesian_motors:
+                    m.initialize()                
+        
+
+    #Cartesian peudo-motors
+    def set_joint_motors_enabled(self, value):
+        if value !=self.joint_motors_enabled:
+            self.joint_motors_enabled = value
+            if value:    
+                for i in range(6):
+                    self.joint_motors.append(RobotJointMotor(self, i))
+                    add_device(self.joint_motors[i], True)                        
+            else:
+                for m in self.joint_motors:
+                    remove_device(m)
+                self.joint_motors = []            
+        else:
+            if value:
+                for m in self.joint_motors:
+                    m.initialize()                
+    #Alignment
+    def align(self, point = None, frame = None, desc = None):
+        if frame is None: frame = self.frame
+        self.assert_tool()
+        if desc is None: desc = self.default_desc
+        if point is None:
+            self.set_pnt(self.get_cartesian_pos() )
+        else:
+            self.set_pnt(point)        
+        np=self.eval_trf('align(tcp_p.trsf, ' + frame + '.trsf)')
+        self.set_pnt(np)
+        self.movej("tcp_p", self.tool, desc)
+        #TODO: The first command is not executed (but receive a move id)
+        self.movej("tcp_p", self.tool, desc, sync = True)        
+        return np
+        
+    #High-level, exclusive motion task.
+    def start_task(self, program, *args,  **kwargs):    
+        tasks = [t for t in self.high_level_tasks]
+        if (self.current_task is not None) and (not self.current_task in tasks): 
+            tasks.append(self.current_task)
+        if not program in tasks: 
+            tasks.append(program)
+        #for task in tasks:
+        #    if self.get_task_status(task)[0]>=0:
+        #        raise Exception("Ongoing high-level task: " + task)        
+        ts = self.get_tasks_status(*tasks)
+        for i in range(len(ts)):
+            if ts[i] > 0:
+                raise Exception("Ongoing high-level task: " + tasks[i])        
+        
+        self.clear_task_ret()
+
+        for i in range(self.task_start_retries):
+            self.task_create(program, *args, **kwargs)                                    
+            (code, status) = self.get_task_status(program)                                           
+            if code>=0: break
+            if i < self.task_start_retries-1:
+                ret = self.get_task_ret(False)
+                if ret == 0: #Did't start
+                    log("Retrying starting : " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")", False)        
+                    print "Retrying starting : " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")"                
+                else :
+                    print "Retrying aborted : " + str(program) + str(args) + " - ret: " + str(ret) + " - status: " + str(status) + " (" + str(code) + ")"                
+                    break
+            else:
+                log("Failed starting : " + str(program) + str(args), False)       
+                print "Failed starting : " + str(program)  + str(args)
+                if self.exception_on_task_start_failure:
+                    raise Exception("Cannot start task: " + program + str(args))                                
+                              
+        log("Task started: " + str(program) + str(args) + " - status: " + str(status) + " (" + str(code) + ")", False)        
+        self.current_task, self.current_task_ret  = program, None
+        self.update()               
+        #self._update_state()
+
+        #TODO: remove
+        if self.current_task is None:
+                log("Task finished in first polling : " + str(program) , False)        
+                print "Task finished in first polling : " + str(program) 
+                
+        return code
+        
+    def stop_task(self):
+        tasks = [t for t in self.high_level_tasks]        
+        if (self.current_task is not None) and (not self.current_task in tasks): 
+            tasks.append(self.current_task)
+        for task in tasks:
+            #if self.get_task_status(task)[0]>=0:                
+                self.task_kill(task)             
+        self.reset_motion()   
+
+    def get_task_ret(self, cached = True):
+        return self.current_task_ret if cached else self.eval_int("tcp_ret")        
+
+    def clear_task_ret(self):
+        return self.evaluate("tcp_ret=0")
+        
+    def get_task(self):
+        return self.current_task
+        
+    def check_task(self):
+        if self.current_task is None:        
+            for task in self.high_level_tasks:
+                if self.get_task_status(task)[0]>=0:                
+                     self.current_task, self.current_task_ret = task, None
+                     log("Task detected: " + str(self.current_task), False)        
+        return self.current_task
+    
+    def wait_task_finished (self, polling = None):
+        cur_polling = self.polling
+        if polling is not None:
+            self.polling = polling
+        try:
+            while self.get_task() != None:
+                time.sleep(self.polling_interval)  
+        finally:
+            if polling is not None:
+                self.polling = cur_polling        
+        ret = self.get_task_ret()        
+        return ret
+
+    def assert_no_task(self):
+        task = self.check_task()
+        if task != None:
+            raise Exception("Ongoing task: " + task)
+        
+    def on_event(self,ev):
+        pass
+
+    def on_change_working_mode(self, working_mode):
+        pass
+            

script/devices/SmartMagnet.py

@@ -0,0 +1,129 @@
+class SmartMagnet(DeviceBase):
+    def __init__(self, name):
+        #DeviceBase.__init__(self, name,  get_context().pluginManager.getDynamicClass("SmartMagnetConfig")())
+       DeviceBase.__init__(self, name, DeviceConfig({
+                   "holdingCurrent":0.0, 
+                   "restingCurrent":0.0, 
+                   "mountCurrent":0.0, 
+                   "unmountCurrent":0.0, 
+                   "reverseCurrent":0.0, 
+                   "reverseTime":0.0, 
+                   }))    
+
+    def doInitialize(self):
+        super(SmartMagnet, self).doInitialize()
+        self.get_current()
+
+    def set_current(self, current):
+        self.setCache(current, None)
+        smc_current.write(current)
+
+    def get_current(self):
+        cur = smc_current.read()
+        self.setCache(cur, None)
+        return cur
+
+    def get_current_rb(self):
+        self.assert_status()
+        return smc_current_rb.read()
+
+    def get_status(self):
+        return smc_magnet_status.read()
+
+    def assert_status(self):
+        pass
+        #if self.get_status() == False:
+        #    raise Exception("Smart Magnet is in faulty status.")
+
+    def is_mounted(self):
+        self.assert_status()
+        m1 = smc_mounted_1.read()
+        m2 = smc_mounted_2.read()
+        if m2==m1:
+            raise Exception("Smart Magnet has invalid detection.")
+        return m2
+
+    def set_supress(self, value):
+        smc_sup_det.write(value)
+
+    def get_supress(self):
+        return smc_sup_det.read()
+
+    def check_mounted(self, idle_time =1.0, timeout = -1, interval = 0.01):
+        self.assert_status()
+        start = time.time()        
+        last = None
+        while True:
+            try:
+                det = self.is_mounted()
+            except:
+                det = None
+            if det != last:
+                settling_timestamp = time.time()
+                last = det
+            else:
+                if det is not None:
+                    if (time.time()-settling_timestamp > idle_time):
+                        return det                                         
+            if timeout >= 0:
+                if (time.time()  - start) > timeout:
+                    raise Exception("Timeout waiting for Smart Magnet detection.")
+            time.sleep(interval)
+     
+
+    def doUpdate(self):
+         try:
+             if self.get_supress():
+                 self.setState(State.Paused)
+             elif self.is_mounted():
+                 self.setState(State.Busy)
+             else:
+                 self.setState(State.Ready)
+         except:
+             self.setState(State.Fault)
+
+    def set_holding_current(self):
+        self.set_current(self.config.getFieldValue("holdingCurrent"))
+
+    def set_resting_current(self):
+        self.set_current(self.config.getFieldValue("restingCurrent"))   
+
+    def set_mount_current(self):
+        self.set_current(self.config.getFieldValue("mountCurrent"))  
+
+    def set_unmount_current(self):
+        self.set_current(self.config.getFieldValue("unmountCurrent")) 
+
+    def set_reverse_current(self):
+        self.set_current(self.config.getFieldValue("reverseCurrent"))
+
+    def set_default_current(self):
+        if self.is_mounted():
+            self.set_holding_current()
+        else:
+            self.set_resting_current()
+
+    def is_resting_current(self):
+        is_resting = 2.0 > abs( float(self.get_current()) - float(self.config.getFieldValue("restingCurrent")) )
+        return is_resting
+
+
+    def apply_reverse(self):
+        reverse_wait = float(self.config.getFieldValue("reverseTime"))
+        if reverse_wait >0:
+            self.set_reverse_current()
+            time.sleep(reverse_wait)
+        
+    #Setting resting current to better detect sample
+    def apply_resting(self):
+        if not self.is_resting_current():
+            self.set_resting_current()
+
+
+    
+
+add_device(SmartMagnet("smart_magnet"), force = True)                
+        
+smart_magnet.polling = 1000
+
+smart_magnet.set_default_current()

script/devices/Wago.py

@@ -0,0 +1,175 @@
+LED_LEVEL_ROOM_TEMPERATURE = 0.4
+LED_LEVEL_LN2 = 1.0
+
+    
+###################################################################################################
+#  Leds
+###################################################################################################
+
+def set_led_level(level):
+     level = max(min(float(level),100.0),0.0)
+     set_setting("led_level", level)
+     led_ctrl_1.write(led_ctrl_1.config.maxValue * level / 100.0)
+     led_ctrl_2.write(led_ctrl_2.config.maxValue * level / 100.0)
+     led_ctrl_3.write(led_ctrl_3.config.maxValue * level / 100.0)        
+
+def get_led_level():
+    level = get_setting("led_level")
+    return float(0 if level is None else level)
+    
+def set_led_state(value):
+    """
+    Turn leds on and off
+    """
+    if value:
+        set_led_level(100.0)
+    else:
+        set_led_level(0.0)
+
+def get_led_state():
+    """
+    Returns led state (on/off)
+    """
+    return led_ctrl_1.read() > 0 
+
+#TODO: The range should be set automatically reading LN2 sensor.
+def set_led_range(room_temp = True):
+    """
+    Led range should be limitted in room temperature
+    """
+    max_val = LED_LEVEL_ROOM_TEMPERATURE if room_temp else LED_LEVEL_LN2
+    led_ctrl_1.config.maxValue = max_val
+    led_ctrl_1.config.save()
+    led_ctrl_2.config.maxValue = max_val
+    led_ctrl_2.config.save()
+    led_ctrl_3.config.maxValue = max_val
+    led_ctrl_3.config.save()
+    led_ctrl_1.initialize()
+    led_ctrl_2.initialize()
+    led_ctrl_3.initialize()
+    if led_ctrl_1.read() > max_val:
+        led_ctrl_1.write(max_val)
+    if led_ctrl_2.read() > max_val:
+        led_ctrl_2.write(max_val)
+    if led_ctrl_3.read() > max_val:
+        led_ctrl_3.write(max_val)
+    set_led_level(get_led_level())
+
+
+def is_led_room_temp():
+    return led_ctrl_1.config.maxValue <= LED_LEVEL_ROOM_TEMPERATURE
+
+
+
+
+###################################################################################################
+#  Safety release
+###################################################################################################
+
+def release_local():
+    """
+    Release local safety
+    """        
+    release_local_safety.write(False)
+    time.sleep(0.01)
+    release_local_safety.write(True)
+    time.sleep(0.01)
+    release_local_safety.write(False)
+
+def release_psys():
+    """
+    Release psys safety
+    """    
+    if is_manual_mode():
+        raise Exception ("Cannot release PSYS in manual mode")
+    release_psys_safety.write(False)
+    time.sleep(0.01)
+    release_psys_safety.write(True)
+    time.sleep(0.01)
+    release_psys_safety.write(False)
+
+
+
+###################################################################################################
+#  Drier
+###################################################################################################
+MAX_HEATER_TIME = 120000
+
+def set_air_stream(state):
+    """
+    """
+    valve_1.write(state)
+    valve_2.write(not state)
+
+
+set_heater_chrono = None
+
+def monitor_heater_time():
+    time.sleep(0.5)
+    try:
+        while get_heater():
+            if set_heater_chrono.isTimeout(MAX_HEATER_TIME):
+                set_heater(False)
+                log("Heater timeout expired: turned off", False)
+                return
+            time.sleep(0.1)
+    except:
+        print sys.exc_info()
+
+
+def get_heater():
+    """
+    """
+    return gripper_dryer.read()
+
+
+def set_heater(state):
+    """
+    """
+    global set_heater_chrono
+    if  get_heater() != state:
+        gripper_dryer.write(state)    
+        if state: 
+            set_heater_chrono = Chrono()      
+            fork(monitor_heater_time)
+
+
+def get_air_stream():
+    """
+    """
+    return valve_1.read()
+
+
+
+def set_pin_cleaner(state):
+    """
+    """
+    valve_7.write(state)
+    valve_8.write(not state)
+
+    
+
+def get_pin_cleaner():
+    """
+    """
+    return valve_7.read()
+
+
+monitor_pin_cleaner_task = None
+
+def monitor_pin_cleaner(timeout):
+    global monitor_pin_cleaner_task
+    this = monitor_pin_cleaner_task
+    time.sleep(timeout)
+    if this == monitor_pin_cleaner_task:
+        set_pin_cleaner(False)    
+
+
+DEFAULT_PIN_CLEANER_TIMEOUT = 5.0
+def start_pin_cleaner(timeout=DEFAULT_PIN_CLEANER_TIMEOUT):
+    global monitor_pin_cleaner_task
+    set_pin_cleaner(True)
+    set_pin_cleaner_chrono = Chrono()      
+    monitor_pin_cleaner_task = fork((monitor_pin_cleaner,(timeout,),))[0]
+    
+

script/hexiposi_positon.scd

@@ -0,0 +1,11 @@
+[ 
+  [ [ true, "hexiposi_position", "Device", 1, 1, null ] ],
+  [ [ "1", null, null, null, null, null, null, null ],
+    [ "2", null, null, null, null, null, null, null ],
+    [ "3", null, null, null, null, null, null, null ],
+    [ "4", null, null, null, null, null, null, null ],
+    [ "5", null, null, null, null, null, null, null ] ],
+  [ [ ] ],
+  [ [ "", 1000, 100 ],
+    [ "", "" ] ]  
+]

script/imgproc/CameraCalibration.py

@@ -0,0 +1,156 @@
+import ch.psi.pshell.device.Camera as Camera
+import ch.psi.pshell.imaging.RendererMode as RendererMode
+import ch.psi.pshell.imaging.Calibration as Calibration
+from ch.psi.pshell.imaging.Overlays import *
+import ch.psi.pshell.imaging.Pen as Pen
+import ch.psi.utils.swing.SwingUtils as SwingUtils
+import javax.swing.SwingUtilities as SwingUtilities
+#from swingutils.threads.swing import callSwing
+#SIMULATION = ch.psi.pshell.imaging.FileSource
+"""
+img.camera.setColorMode(Camera.ColorMode.Mono)
+img.camera.setDataType(Camera.DataType.UInt8)
+img.camera.setGrabMode(Camera.GrabMode.Continuous)
+img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
+img.camera.setExposure(50.00)
+img.camera.setAcquirePeriod(200.00)
+img.camera.setGain(0.0)
+img.config.rotationCrop=True
+"""
+
+MOVE_HEXIPOSI = not is_manual_mode()
+ROTATION_OFFSET = 180.0
+
+if MOVE_HEXIPOSI:
+    release_safety()   #enable_motion()   
+
+sensor_width,sensor_height = img.camera.getSensorSize()
+img.camera.setROI(0, 0,sensor_width, sensor_height)  
+img.config.rotation=0
+img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight =0,0,-1,-1
+img.config.setCalibration(None)
+img.camera.stop()
+img.camera.start()
+
+
+p = show_panel(img)
+dlg = SwingUtilities.getWindowAncestor(p)
+dlg.setSize(800,800)
+frm=SwingUtils.getFrame(p)
+dlg.setLocationRelativeTo(frm)
+
+p.setMode(RendererMode.Fit)
+ov_text = Text(Pen(java.awt.Color.GREEN.darker()), "", java.awt.Font("Verdana", java.awt.Font.PLAIN, 24), java.awt.Point(20,20))
+ov_text.setFixed(True)
+p.addOverlay(ov_text)
+
+
+try:
+    #Find image center and Prosilica ROI
+    ov_text.update("Click on the center of the Dewar...")
+    p.refresh()
+    dc = p.waitClick(60000)
+    print dc
+    width, height = min(dc.x, sensor_width-dc.x)*2, min(dc.y, sensor_height-dc.y)*2
+    width, height = width - width%16, height - height%16
+    width, height = min(width,1000),  min(height,1000)
+    print width, height
+    roi_x = int(dc.x- width/2)
+    roi_y = int(dc.y- height/2)
+    roi_w = int(width)
+    roi_h = int(height)
+    set_setting("roi_x", roi_x)
+    set_setting("roi_y", roi_y)
+    set_setting("roi_w", roi_w)
+    set_setting("roi_h", roi_h)    
+    img.camera.setROI(roi_x, roi_y, width, height)  
+except:
+    img.camera.setROI(int(get_setting("roi_x")), int(get_setting("roi_y")), int(get_setting("roi_w")), int(get_setting("roi_h")))  
+finally:    
+    img.camera.stop()
+    img.camera.start()
+
+
+#Configure source
+CC4 = (-129.9, -150)
+CD5 = (129.9, -150)
+CA5 = (-129.9, 150)
+CF4 = (129.9, 150)
+
+DX = 259.8
+DY = 300.0
+
+ROI_X = 470.0
+ROI_Y = 470.0
+
+def rotate(x,y, degrees):
+    rotation = math.radians(degrees)
+    rw, rh = img.getImage().getWidth(), img.getImage().getHeight()
+    ox, oy = x - (rw / 2), y - (rh / 2)
+    x = ox * math.cos(rotation) - oy * math.sin(rotation) + rw / 2;
+    y = oy * math.cos(rotation) + ox * math.sin(rotation) + rh / 2;
+    return x,y
+
+
+set_led_state(True)
+try:
+    if MOVE_HEXIPOSI: set_hexiposi("C")
+    ov_text.update("Click on the center of C4 (19) position...")
+    p.refresh()
+    pc4 = p.waitClick(60000)
+    print pc4
+    if MOVE_HEXIPOSI: set_hexiposi("D")
+    ov_text.update("Click on the center of D5 (13) position...")
+    p.refresh()
+    pd5 = p.waitClick(60000)
+    print pd5
+    if MOVE_HEXIPOSI: set_hexiposi("F")
+    ov_text.update("Click on the center of F4 (04) position...")
+    p.refresh()
+    pf4 = p.waitClick(60000)
+    print pf4      
+    if MOVE_HEXIPOSI: set_hexiposi("A")
+    ov_text.update("Click on the center of A5 (28) position...")
+    p.refresh()
+    pa5 = p.waitClick(60000)
+    print pa5
+   
+    
+    vc1x, vc1y, vc2x, vc2y = (pc4.x + pd5.x )/2.0, (pc4.y + pd5.y )/2.0, (pa5.x + pf4.x )/2.0, (pa5.y + pf4.y )/2.0
+    hc1x, hc1y, hc2x, hc2y = (pc4.x + pa5.x )/2.0, (pc4.y + pa5.y )/2.0, (pd5.x + pf4.x )/2.0, (pd5.y + pf4.y )/2.0
+    cx, cy = (vc1x + vc2x)/2, (hc1y + hc2y)/2
+    
+    a1 = math.degrees(math.atan((cx-vc1x)/(vc1y-cy)))
+    a2 = math.degrees(math.atan((cx-vc2x)/(vc2y-cy)))
+    a = (a1+a2)/2
+
+
+    dy = math.hypot(vc2y - vc1y, vc2x - vc1x)
+    dx = math.hypot(hc2x - hc1x, hc2y - hc1y)
+    print dy, dx, cx, cy
+    sx, sy = DX/dx, DY/dy 
+
+
+    #Rotating center of puck
+    rcx, rcy =   rotate(cx, cy, -a)  
+    
+
+    
+    roi_w, roi_h = int(ROI_X / sx), int(ROI_Y / sy)
+    roi_x, roi_y = int(rcx-roi_w/2), int(rcy-roi_h/2)
+
+    print a, sx, sy, roi_w, roi_h 
+   
+    img.config.rotation=-a + ROTATION_OFFSET
+    img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = roi_x, roi_y, roi_w, roi_h
+    img.config.setCalibration(Calibration(sx, sy, -roi_w/2, -roi_h/2))
+    img.config.save()
+
+    set_return ("Success calibrating the camera")
+    
+finally: 
+    set_led_state(False)
+    p.removeOverlay(ov_text)
+    img.refresh()
+
+  

script/imgproc/CoverDetection.py

@@ -0,0 +1,98 @@
+################################################################################################### 
+# Procedure to detect the cover orientation
+################################################################################################### 
+assert_imaging_enabled()
+
+
+#Parameters
+FRAMES_INTEGRATION = 3
+STEP_SIZE = 2
+POSITION_NAMES = [ 'A','B','C','D', 'E', 'F']
+
+#POSITION_ANGLES = [ 330,  30, 90, 150, 210, 270 ]
+POSITION_ANGLES = [ 0,  60, 120, 180, 240, 300 ]
+POSITION_TOLERANCE = 3
+MINIMUM_CONFIDENCE = 3
+DEBUG = cover_detection_debug
+#REFERENCE_IMG = "ref2"
+REFERENCE_IMG = "ref1"
+BORDER = 7
+
+#Load reference image
+ref = load_image(str("{images}/cover/" + REFERENCE_IMG + ".png") , title="Line")
+
+#Pre-process camera image
+#ip = load_image("{images}/cover/Cover_000" + str(index) + ".png", title="Img")
+ip = integrate_frames(FRAMES_INTEGRATION)
+ip = grayscale(ip, True)
+smooth(ip)
+#bandpass_filter(ip, 30, 1000)
+edges(ip)
+auto_threshold(ip, method = "MaxEntropy")
+#binary_erode(ip, True)
+#binary_dilate(ip, True)
+ip.getProcessor().erode(1, 255)
+cx,cy = int(ip.width/2), int(ip.height/2)
+ip = sub_image(ip, cx-ref.width/2, cy-ref.height/2, ref.width, ref.height)
+
+if BORDER>0:
+    sip = sub_image(ip, BORDER,BORDER,  ref.width-2*BORDER, ref.height-2*BORDER)
+    ip = pad_image(sip, BORDER, BORDER, BORDER, BORDER, fill_color=Color.WHITE)
+    
+#Show ROI of pre-processed image 
+if DEBUG:
+    image_panel = show_panel(ip.bufferedImage)
+
+
+#Calculate correlation between image and reference, rotating the reference from 0 to 360
+import ch.psi.pshell.imaging.Utils.integrateVertically as integrateVertically
+ydata = []
+xdata = range (0,360,STEP_SIZE)
+for i in xdata:
+    r = ref.duplicate()
+    r.getProcessor().setBackgroundValue(0.0)
+    r.getProcessor().rotate(float(i))
+    op = op_fft(r, ip, "correlate") 
+    bi = op.getBufferedImage()
+    p = integrateVertically(bi)
+    ydata.append(sum(p))
+
+
+#Calculate angle of the highest correlation, and confidence level
+peaks = estimate_peak_indexes(ydata, xdata,  (min(ydata) + max(ydata))/2,  25.0)
+peaks_x = map(lambda x:xdata[x], peaks)
+peaks_y = map(lambda x:ydata[x], peaks)
+if len(peaks_x) > 1:
+    #remoce close peaks between 350 deg and 10 deg
+    if ((peaks_x[0]<10) and  (peaks_x[1]>350)) or ((peaks_x[1]<10) and  (peaks_x[0]>350)):
+        peaks.pop(1)
+        peaks_x.pop(1)
+        peaks_y.pop(1)
+        
+        
+confidence = None if len(peaks_x)<2 else int(((float(peaks_y[0])/peaks_y[1])-1) * 1000)
+angle = (None if len(peaks_x)==0 else peaks_x[0])
+ 
+#From angle and confidence level estimate hexiposi position
+position = None
+if angle is not None:
+    for i in range(len(POSITION_NAMES)):    
+        if abs(POSITION_ANGLES[i] - angle) <= POSITION_TOLERANCE:
+            position = POSITION_NAMES[i]
+
+#Plot the correlations values  agains angle
+if DEBUG:
+    plot(ydata, xdata=xdata)
+
+#Output results
+if DEBUG:
+    print "Peaks", peaks        
+    print "Peak indexes: " + str(peaks_x)
+    print "Peak values:  " + str(peaks_y)
+    print "Angle:  " , angle
+    print "Position:  " , position
+    print "Confidence:  " , confidence
+
+#Set return value
+set_return  ([position, angle, confidence])
+

script/imgproc/CoverDetectionCalibration.py

@@ -0,0 +1,40 @@
+#Parameters
+FRAMES_INTEGRATION = 3
+MINIMUM_CONFIDENCE = 10
+DEBUG = cover_detection_debug
+REFERENCE_IMG = "ref1"
+ERODE_ITERATIONS = 2
+
+#Load reference image
+SIZE = [128,128]
+BORDER = 7
+
+hexiposi.move("A")
+#Pre-process camera image
+#ip = load_image("{images}/cover/Cover_000" + str(index) + ".png", title="Img")
+ip = integrate_frames(FRAMES_INTEGRATION)
+ip = grayscale(ip, True)
+smooth(ip)
+#bandpass_filter(ip, 30, 1000)
+edges(ip)
+
+auto_threshold(ip, method = "MaxEntropy")
+#binary_dilate(ip, True, 2)
+for i in range(ERODE_ITERATIONS):
+    ip.getProcessor().erode(1, 255)
+
+cx,cy = int(ip.width/2), int(ip.height/2)
+ip = sub_image(ip, cx-SIZE[0]/2, cy-SIZE[1]/2, SIZE[0], SIZE[1])
+
+invert(ip)
+ip = grayscale(ip, True)
+#smooth(ip)
+
+if BORDER > 0:
+    sip = sub_image(ip, BORDER,BORDER, SIZE[0]-2*BORDER, SIZE[1]-2*BORDER)
+    ip = pad_image(sip, BORDER, BORDER, BORDER, BORDER)
+if DEBUG:
+    image_panel = show_panel(ip.bufferedImage)
+
+save_image(ip, str("{images}/cover/" + REFERENCE_IMG + ".png") ,"png")
+    

script/imgproc/CreateMask.py

@@ -0,0 +1,29 @@
+
+mask_img = new_image(img.getOutput().getWidth(), img.getOutput().getHeight(), image_type="byte", title = "mask_img", fill_color = Color.BLACK)
+
+mask_radius = 14
+mask_points = []
+
+
+def to_img_coords(absolute_coords):
+    return [img.getCalibration().convertToImageX(absolute_coords[0]), img.getCalibration().convertToImageY(absolute_coords[1])]
+
+for p in _puck_list:
+    mask_points.append(to_img_coords(p.led_mini))
+    mask_points.append(to_img_coords(p.led_uni))    
+
+
+
+i = mask_img.getBufferedImage()
+for p in mask_points:
+    #i.setRGB(p[0], p[1], 0xFFFFFF)
+    for x in range (p[0]-mask_radius,  p[0]+mask_radius):
+        for y in range (p[1]-mask_radius,  p[1]+mask_radius):
+            if math.hypot(x-p[0], y-p[1]) <= mask_radius:
+                i.setRGB(x,y, 0xFFFFFF)
+
+
+mask_img = load_image(i)
+#show_panel( mask_img.getBufferedImage())
+
+set_return(mask_img)

script/imgproc/LedDetectionFilter.py

@@ -0,0 +1,102 @@
+################################################################################################### 
+# Example of using ImageJ functionalities through ijutils.
+################################################################################################### 
+
+import datetime
+from ijutils import *
+import java.awt.Color as Color
+
+import ch.psi.pshell.imaging.Filter as Filter
+from ch.psi.pshell.imaging.Overlays import *
+import ch.psi.pshell.imaging.Pen as Pen
+
+
+integration_count = 10
+integration_continuous = False
+integration_partial = False
+frames = []            
+roi = get_roi()
+
+
+color_roi = Color(0, 128, 0)
+
+renderer = show_panel(img)
+renderer.clearOverlays()
+ov_roi_shape = Ellipse(Pen(color_roi, 0,), java.awt.Point(roi[0], roi[1]), java.awt.Dimension(roi[2], roi[3]))       
+ov_roi_bound = Rect(Pen(color_roi, 0, Pen.LineStyle.dotted), java.awt.Point(roi[0], roi[1]), java.awt.Dimension(roi[2], roi[3]))       
+ov_roi_center = Crosshairs(Pen(color_roi, 0),  java.awt.Point(roi_center[0],roi_center[1]), java.awt.Dimension(15,15))
+
+renderer.addOverlays([ov_roi_shape, ov_roi_bound,ov_roi_center])
+
+
+
+
+last_ret = (None, None)
+def detect_led(ip):
+    roi = get_roi()
+    global roi_center, roi_radius, integration_count, integration_continuous, integration_partial, frames
+    global count , last_ret 
+    aux = sub_image(ip, roi[0], roi[1], roi[2], roi[3])
+    grayscale(aux)
+    #gaussian_blur(aux)
+    if (integration_count>1):             
+        frames.append(aux)        
+        if len(frames) >integration_count:
+            del frames[0]        
+        if not integration_continuous:            
+            if (len(frames)< integration_count):                
+                if last_ret[1] is not None: invert(last_ret[1])                
+                return last_ret                   
+        if (not integration_partial) and len(frames) <integration_count:            
+            return last_ret
+        aux = integrate(frames)
+    
+    #aux = get_channel(aux, "blue")
+    invert(aux)
+    #subtract_background(aux) 
+    #Tested ok: Huang, Mean, MaxEntropy, Percentile, Triangle, Yen
+    auto_threshold(aux, method = "Percentile")
+    #binary_open(aux)
+    (results,output) =  analyse_particles(aux, 250,1000, 
+            fill_holes = True, exclude_edges = False, print_table=False,
+            output_image = "outlines", minCirc = 0.3
+            , maxCirc = 1.0)
+    r=results
+
+
+    points = ""
+    npoints = 0
+    for row in range (r.counter):
+        if in_roi(r.getValue("XM",row), r.getValue("YM",row)):
+            points = points + " (" + str(int(r.getValue("XM", row))+roi[0]) + ", "  + str(int(r.getValue("YM", row))+roi[1]) + ")"
+            npoints = npoints + 1
+    print str(npoints) + " - " + points
+        
+    last_ret = (results,output)
+    if not integration_continuous:
+        frames = []  
+
+    #if npoints!=12:        
+    #    save_image(op_image(aux, output,"xor", in_place=False),  "{images}/" + str(datetime.datetime.now().strftime("%Y%m%d_%H%M%S"))+".png", "png")        
+    #return (results,aux)    
+    return (results,output)
+
+
+
+
+ip = None
+
+class MyFilter(Filter):
+    def process(self, image, data):
+        global roi_center, roi_radius, ip   
+        ip = load_image(image)                            
+        (results,output) = detect_led(ip)
+        if output is not None:
+            invert(output)
+            output = pad_image(output, roi[0], 0,roi[1], 0)        
+            op_image(ip, output, "xor")        
+        return ip.getBufferedImage()
+
+#Setting the filter to a source
+img.setFilter(MyFilter())
+

script/imgproc/LedDetectionProc.py

@@ -0,0 +1,106 @@
+################################################################################################### 
+# Procedure to detect the puck light spots.
+################################################################################################### 
+assert_imaging_enabled()
+
+
+COVER_PRESENT = True
+ROOM_TEMP = is_room_temp()
+USE_MASK = True
+
+if get_exec_pars().source == CommandSource.ui:
+    PLOT = None
+    RENDERER = None
+    TEXT = None
+
+if COVER_PRESENT:
+    cover_position =  hexiposi.readback.take()
+    if (cover_position is None) or (cover_position == "Unknown"):    
+        raise Exception("Unknown cover position")
+    else:
+        block_id = cover_position.upper()[0]
+else:
+    block_id = None    
+print "Block id: ", block_id
+
+
+
+number_frames = 5 if ROOM_TEMP else 10
+number_backgrounds = 5 if ROOM_TEMP else 5
+minimum_size = 78     # r = 5 # 150   
+maximum_size = 750    # r = 15 #1500
+min_circ = 0.2
+
+threshold_method = "MaxEntropy" if ROOM_TEMP else "Default" #Apparently good for LN2: Default, Intermodes, IsoData, Otsu
+threshold_method,threshold_range = "Manual",  (0, 215)
+
+exclude_edges = True
+led_latency = 0.5 #0.1
+
+
+
+set_led_state(False)
+time.sleep(led_latency)
+img.waitNext(2000)
+
+background = average_frames(number_backgrounds)
+#background = integrate_frames(number_backgrounds)
+
+set_led_state(True)
+time.sleep(led_latency)
+img.waitNext(2000)
+image = average_frames(number_frames)
+#image = integrate_frames(number_frames)
+
+set_led_state(False)
+
+op_image(image, background, "subtract", float_result=True, in_place=True)
+image=grayscale(image)
+
+if RENDERER is not None and RENDERER.isShowing():
+    RENDERER.setImage(None, image.getBufferedImage(), None)    
+else:    
+    RENDERER = show_panel(image.getBufferedImage())
+RENDERER.clearOverlays()
+
+if USE_MASK:
+    mask_img = run("imgproc/CreateMask")    
+    #mask_img=grayscale(mask_img)
+    #show_panel( mask_img.getBufferedImage())
+    op_image(image, mask_img, "and", float_result=False, in_place=True)
+    RENDERER.setImage(None, image.getBufferedImage(), None)    
+
+invert(image)
+if threshold_method == "Manual":
+    threshold(image, threshold_range[0], threshold_range[1])
+else:
+    auto_threshold(image, method = threshold_method) #Tested ok:  MaxEntropy, Triangle, Yen
+(r,output) =  analyse_particles(image, minimum_size,maximum_size, 
+        fill_holes = True, exclude_edges = exclude_edges, print_table=False,
+        output_image = "outlines", minCirc = min_circ
+        , maxCirc = 1.0)
+
+points = []
+for row in range (r.counter):
+    if in_roi(r.getValue("XM",row), r.getValue("YM",row)):
+        x, y = int(r.getValue("XM", row)), int(r.getValue("YM", row))
+        cx, cy = img.getCalibration().convertToAbsoluteX(x), img.getCalibration().convertToAbsoluteY(y)
+        points.append([cx,cy])
+        if RENDERER is not None:
+            RENDERER.addOverlay(Crosshairs(Pen(java.awt.Color.MAGENTA),  java.awt.Point(x,y), java.awt.Dimension(15,15)))
+
+
+clear_detection(block_id)
+detect_pucks(points, block_id)
+if PLOT is not None:
+    plot_base_plate(points, p=PLOT)    
+
+ret = get_puck_detection_dict(block_id)
+
+
+if TEXT is not None:
+    TEXT.setText(str(ret)) 
+
+
+set_return(ret)
+

script/imgproc/Utils.py

@@ -0,0 +1,113 @@
+
+###################################################################################################
+#  Image processing utilities
+###################################################################################################
+
+
+from ijutils import *
+from ch.psi.pshell.imaging.Overlays import *
+import ch.psi.pshell.imaging.Pen as Pen
+import java.awt.Rectangle as Rectangle
+
+def get_img_cover_pos():
+    [position, angle, confidence] = run("imgproc/CoverDetection") 
+    return position
+
+def assert_img_in_cover_pos(pos = None):
+    if pos==None: 
+        pos = hexiposi.take()
+    elif type(pos) is int:
+        pos = chr( ord('A') + (pos-1))
+    elif is_string(pos):
+        pos = pos.upper()    
+    img_segment = get_img_cover_pos()
+    if img_segment != pos:
+        raise Exception ("Image detection of cover does not match position: " + str(img_segment))
+    
+
+def in_roi(x,y):
+    global roi_center, roi_radius, roi_border
+    return math.hypot(x-roi_center[0], y-roi_center[1]) < (roi_radius-roi_border)
+
+
+def integrate(ips):    
+    roi =  get_roi()
+    aux = None
+    for i in range(len(ips)):
+        if i==0:
+            aux = new_image(roi[2], roi[3], image_type="float", title = "sum", fill_color = None)
+        op_image(aux, ips[i], "add", float_result=True, in_place=True)    
+    return aux
+
+def average (ips):   
+    aux = integrate(ips)     
+    op_const(aux, "divide", len(ips), in_place=True)    
+    return aux
+
+def grab_frames(samples):
+    frames = []            
+    for i in range(samples):
+        aux = get_image()        
+        frames.append(aux)        
+    return frames
+    
+def average_frames(samples = 1):
+    return average(grab_frames(samples))
+
+def integrate_frames(samples = 1):
+    return integrate(grab_frames(samples))
+
+
+roi_center = (600, 600) #(800, 600)
+roi_radius = 600
+roi_border = 30
+
+def get_roi():
+    #roi_center = (img.output.width/2, img.output.height/2)
+    #roi_radius = min(roi_center[0], roi_center[1])
+    #return (roi_center[0] - roi_radius, roi_center[1] - roi_radius, 2* roi_radius, 2*roi_radius)
+    global roi_center, roi_radius
+    roi_center = (img.output.width/2, img.output.height/2)
+    roi_radius = min(roi_center[0], roi_center[1])    
+    return (0,0,img.output.width, img.output.height)
+
+def get_image():
+    roi =  get_roi()
+    #ip = load_image(img.output)                            
+    #ret = ip if (roi is None) else sub_image(ip, roi[0], roi[1], roi[2], roi[3])
+    #grayscale(ret, do_scaling=True)
+
+    ret = load_image(Utils.grayscale(img.output, Rectangle(roi[0], roi[1], roi[2], roi[3]) if (roi is not None) else None))
+    return ret
+
+#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,
+
+
+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)

script/local.groovy

@@ -0,0 +1,3 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// Deployment specific global definitions - executed after startup.groovy
+///////////////////////////////////////////////////////////////////////////////////////////////////

script/local.js

@@ -0,0 +1,4 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// Deployment specific global definitions - executed after startup.js
+///////////////////////////////////////////////////////////////////////////////////////////////////
+

script/local.py

@@ -0,0 +1,438 @@
+###################################################################################################
+#  Deployment specific global definitions - executed after startup.py
+###################################################################################################
+import traceback
+from ch.psi.pshell.serial import TcpDevice
+from ch.psi.pshell.modbus import ModbusTCP
+import ch.psi.mxsc.Controller as Controller
+import ch.psi.pshell.core.Nameable as Nameable
+import ch.psi.utils.Chrono as Chrono
+import ch.psi.mxsc.Controller as Controller
+
+
+
+run("setup/Layout")
+run("data/reports")
+
+
+
+
+###################################################################################################
+# Configuration
+###################################################################################################
+
+IMAGING_ENABLED_PREFERENCE = "imaging_enabled"
+PUCK_TYPES_PREFERENCE = "puck_types"
+BARCODE_READER_SCAN_PUCKS = "barcode_reader_scan_pucks"
+ROOM_TEMPERATURE_ENABLED_PREFERENCE = "room_temperature_enabled"
+BEAMLINE_STATUS_ENABLED_PREFERENCE = "beamline_status_enabled"
+VALVE_CONTROL_ENABLED_PREFERENCE = "valve_control"
+
+def is_imaging_enabled():
+    setting = get_setting(IMAGING_ENABLED_PREFERENCE)
+    return not (str(setting).lower() == 'false')
+
+def set_imaging_enabled(value):
+    set_setting(IMAGING_ENABLED_PREFERENCE, (True if value else False) )
+
+def assert_imaging_enabled():
+    if is_imaging_enabled() == False:
+        raise Exception ("Imaging is disabled")
+
+#"unipuck", "minispine" or "mixed"
+def set_puck_types(value):
+    set_setting(PUCK_TYPES_PREFERENCE, True if value else False )
+    
+def get_puck_types():
+    setting = get_setting(PUCK_TYPES_PREFERENCE)
+    if setting == "unipuck" or  setting == "minispine":
+        return setting
+    return "mixed"
+
+
+def is_barcode_reader_scan_pucks():
+    setting = get_setting(BARCODE_READER_SCAN_PUCKS)
+    return False if setting is None else setting.lower() == "true" 
+
+def set_barcode_reader_scan_pucks(value):
+    set_setting(BARCODE_READER_SCAN_PUCKS, True if value else False )
+
+def is_valve_controlled():
+    setting = get_setting(VALVE_CONTROL_ENABLED_PREFERENCE)
+    return False if setting is None else setting.lower() == "true" 
+
+def set_valve_controlled(value):
+    set_setting(VALVE_CONTROL_ENABLED_PREFERENCE, True if value else False )
+
+def reset_mounted_sample_position():
+    set_setting("mounted_sample_position", None)  
+
+
+def get_puck_barcode_reader(): 
+    if is_barcode_reader_scan_pucks():
+        return barcode_reader
+    else:
+        return barcode_reader_puck
+
+#In order to apply current config
+set_imaging_enabled(is_imaging_enabled())    
+set_puck_types(get_puck_types())
+set_barcode_reader_scan_pucks(is_barcode_reader_scan_pucks())
+set_valve_controlled(is_valve_controlled())
+
+
+force_dry_mount_count = get_setting("force_dry_mount_count")
+if force_dry_mount_count is None:
+    set_setting("force_dry_mount_count", 0)
+
+force_dry_timeout = get_setting("force_dry_timeout")
+if force_dry_timeout is None:
+    set_setting("force_dry_timeout", 0)
+
+
+cold_position_timeout = get_setting("cold_position_timeout")
+if cold_position_timeout is None:
+    set_setting("cold_position_timeout", 0)
+
+
+def is_room_temperature_enabled():
+    setting = get_setting(ROOM_TEMPERATURE_ENABLED_PREFERENCE)
+    return str(setting).lower() == 'true'
+
+set_setting(ROOM_TEMPERATURE_ENABLED_PREFERENCE, is_room_temperature_enabled())
+
+
+def is_beamline_status_enabled():
+    setting = get_setting(BEAMLINE_STATUS_ENABLED_PREFERENCE)
+    return str(setting).lower() == 'true'
+
+set_setting(BEAMLINE_STATUS_ENABLED_PREFERENCE, is_beamline_status_enabled())
+
+###################################################################################################
+#  Scripted devices and pseudo-devices
+###################################################################################################
+
+for script in ["devices/RobotSC", "devices/Wago", "devices/BarcodeReader", "devices/LaserDistance",  \
+               "devices/LedCtrl", "devices/SmartMagnet", "devices/HexiPosi", "devices/Gonio"]:
+    try:
+        run(script)
+    except:
+        print  >> sys.stderr, traceback.format_exc()    
+
+#if is_imaging_enabled():
+if get_device("img") is not None:
+    add_device(img.getContrast(), force = True)
+    add_device(img.getCamera(), force = True)
+
+
+###################################################################################################
+#  Utility modules
+###################################################################################################
+
+run("data/samples")
+run("data/pucks")
+run("motion/tools")
+run("motion/mount")
+run("motion/unmount")
+run("motion/get_dewar")
+run("motion/put_dewar")
+run("motion/get_gonio")
+run("motion/put_gonio")
+run("motion/move_dewar")
+run("motion/move_gonio")
+run("motion/move_heater")
+run("motion/move_home")
+run("motion/move_park")
+run("motion/move_cold")
+run("motion/move_scanner")
+run("motion/move_aux")
+run("motion/get_aux")
+run("motion/put_aux")
+run("motion/dry")
+run("motion/trash")
+run("motion/homing_hexiposi")
+run("motion/calibrate_tool")
+run("motion/scan_pin")
+run("motion/robot_recover")
+run("motion/recover")
+run("tools/Math")
+if is_imaging_enabled():
+    run("imgproc/Utils")
+
+def system_check(robot_move=True):
+    if not air_pressure_ok.read():
+        raise Exception("Air pressure is not ok")
+    if not n2_pressure_ok.read():
+        raise Exception("N2 pressure is not ok")
+    hexiposi.assert_in_known_position()
+
+    if robot_move:
+        if not feedback_local_safety.read():
+            raise Exception("Local safety not released")
+        auto = not is_manual_mode()
+        if auto:
+            if not feedback_psys_safety.read():
+                raise Exception("Psys safety not released")
+        if not guiding_tool_park.read():
+            raise Exception("Guiding tool not parked")
+
+def system_check_msg():
+    try:
+        system_check(True)
+        return "Ok"
+    except:
+        return sys.exc_info()[1]
+
+def get_puck_dev(segment, puck):
+    if type(segment) is int:
+        segment = chr( ord('A') + (segment-1))
+
+    return Controller.getInstance().getPuck(str(segment).upper() + str(puck))
+
+def get_puck_elect_detection(segment, puck):
+    return str(get_puck_dev(segment, puck).detection)
+
+def get_puck_img_detection(segment, puck):
+    return str(Controller.getInstance().getPuck(str(segment).upper() + str(puck)).imageDetection)
+
+def assert_puck_detected(segment, puck):
+    if (segment == AUX_SEGMENT) and (puck == 1):
+        return
+    if get_puck_elect_detection(segment, puck) != "Present":
+        raise Exception ("Puck " + str(segment).upper() + str(puck) + " not present")
+
+
+def start_puck_detection():
+    run("tools/RestartPuckDetection")
+
+def check_puck_detection():
+    return run("tools/CheckPuckDetection")
+
+def stop_puck_detection():
+    run("tools/StopPuckDetection")
+
+
+
+def get_detected_pucks():
+    ret = []
+    for i in range(30): 
+        p = BasePlate.getPucks()[i]
+        if (str(p.getDetection()) == "Present"): 
+            ret.append(str(p.getName()))
+    return ret
+
+def get_pucks_info():
+    ret = []
+    for i in range(30): 
+        p = BasePlate.getPucks()[i]
+        name = p.getName()
+        det = str(p.getDetection())
+        barcode = "" if p.getId() is None else p.getId()
+        
+        ret.append({"puckAddress": name, "puckState": det, "puckBarcode":barcode})
+    return json.dumps(ret)
+
+
+###################################################################################################
+#  Device initialization
+###################################################################################################
+
+try:
+    set_heater(False)
+    set_air_stream(False)
+    set_pin_cleaner(False)
+except:
+   print  >> sys.stderr, traceback.format_exc() 
+
+
+try:
+    release_local_safety.write(False)
+    release_psys_safety.write(False)
+except:
+   print  >> sys.stderr, traceback.format_exc()
+   
+try:
+    hexiposi.set_offset(16.5)
+    hexiposi.polling=500    
+    
+except:
+   print  >> sys.stderr, traceback.format_exc()    
+
+try:
+    robot.setPolling(DEFAULT_ROBOT_POLLING)          
+    robot.set_tool(TOOL_DEFAULT)
+    robot.set_frame(FRAME_DEFAULT)
+    robot.set_motors_enabled(True)
+    robot.set_joint_motors_enabled(True)
+except:
+   print  >> sys.stderr, traceback.format_exc()    
+
+if is_imaging_enabled():
+  try:
+    import ch.psi.pshell.device.Camera as Camera
+    #img.camera.setColorMode(Camera.ColorMode.Mono)
+    #img.camera.setDataType(Camera.DataType.UInt8)
+    img.camera.setGrabMode(Camera.GrabMode.Continuous)
+    img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
+    img.camera.setExposure(25.00)
+    img.camera.setAcquirePeriod(200.00)
+    img.camera.setGain(0.0)
+    #img.camera.setROI(200, 0,1200,1200)
+    """
+    img.camera.setROI(300, 200,1000,1000)    
+    img.config.rotation=17
+    img.config.rotationCrop=True
+    img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = 50,50,900,900
+    """
+    img.camera.setROI(int(get_setting("roi_x")), int(get_setting("roi_y")), int(get_setting("roi_w")), int(get_setting("roi_h")))  
+    
+    img.camera.stop()
+    img.camera.start()
+  except:
+   print  >> sys.stderr, traceback.format_exc()    
+
+#TODO: The Smart Magnet keeps moving sample if detecting is enabled
+#      Detection keeps disabled unless during moount/unmount
+try:
+    smart_magnet.set_supress(True)    
+except:
+   print  >> sys.stderr, traceback.format_exc()    
+
+#gripper_cam.paused = True
+###################################################################################################
+#  Device monitoring
+###################################################################################################
+
+DEWAR_LEVEL_RT = 5.0
+is_room_temperature = False
+
+def is_room_temp():
+    return is_room_temperature
+
+
+class DewarLevelListener (DeviceListener):
+    def onValueChanged(self, device, value, former):
+        global is_room_temperature
+        if value is not None:
+            is_room_temperature =  value <= DEWAR_LEVEL_RT
+dewar_level_listener = DewarLevelListener()
+
+for l in dewar_level.listeners:
+    #if isinstance(l, DewarLevelListener):    #Class changes...
+    if Nameable.getShortClassName(l.getClass()) == "DewarLevelListener":
+        dewar_level.removeListener(l)
+        
+dewar_level.addListener(dewar_level_listener)
+dewar_level_listener.onValueChanged(dewar_level, dewar_level.take(), None)
+
+
+
+
+class HexiposiListener (DeviceListener):
+    def onValueChanging(self, device, value, former):
+        robot.assert_cleared()
+        
+hexiposi_listener = HexiposiListener()
+hexiposi.addListener(hexiposi_listener)
+
+###################################################################################################
+#  Global variables & application state
+###################################################################################################
+
+
+context = get_context()
+
+cover_detection_debug = False
+
+in_mount_position = False
+
+
+def assert_mount_position():
+    print "Source: ",  get_exec_pars().source 
+    if not in_mount_position and get_exec_pars().source == CommandSource.server :
+        raise Exception("Not in mount position")
+
+
+def is_puck_loading():
+    return robot.state == State.Ready and robot.take()["pos"] == 'pPark' and \
+    feedback_psys_safety.take() == False and \
+    not guiding_tool_park.read()
+
+def set_pin_offset(val):
+    if abs(val) >5:
+        raise Exception("Invalid pin offset: " + str(val)) 
+    try:
+        set_setting("pin_offset",float(val))
+    except:
+        log("Error setting pin offset: " + str(sys.exc_info()[1]), False)
+
+def get_pin_offset():
+    try:
+        ret =  float(get_setting("pin_offset"))
+        if abs(ret) >5:
+            raise Exception("Invalid configured pin offset: " + str(ret))         
+        return ret
+    except:
+        log("Error getting pin offset: " + str(sys.exc_info()[1]), False)
+        return 0.0
+
+
+def set_pin_angle_offset(val):
+    if (abs(val) > 180.0) or (abs(val) < -180.0):
+        raise Exception("Invalid pin angle offset: " + str(val)) 
+    try:
+        set_setting("pin_angle_offset",float(val))
+    except:
+        log("Error setting pin angle offset: " + str(sys.exc_info()[1]), False)
+
+def get_pin_angle_offset():
+    try:
+        ret =  float(get_setting("pin_angle_offset"))
+        if (abs(ret) > 180.0) or (abs(ret) < -180.0):
+            raise Exception("Invalid configured pin angle offset: " + str(ret))         
+        return ret
+    except:
+        log("Error getting pin angle offset: " + str(sys.exc_info()[1]), False)
+        return 0.0
+
+def is_force_dry():
+    try:
+        dry_mount_counter = int(get_setting("dry_mount_counter"))
+    except:
+        dry_mount_counter = 0
+        
+    try:  
+        dry_timespan = time.time() - float( get_setting("dry_timestamp"))
+    except:
+        dry_timespan = 3600
+
+    try:
+        force_dry_mount_count = int(get_setting("force_dry_mount_count"))
+        if force_dry_mount_count>0 and dry_mount_counter>=force_dry_mount_count:
+            return True
+    except:
+        pass
+
+    try:
+        force_dry_timeout = float(get_setting("force_dry_timeout"))
+        if force_dry_timeout>0 and dry_timespan>=force_dry_timeout:
+            return True
+    except:
+        pass    
+    return False
+    
+def assert_detector_safe():
+    if detector_cleared.read() == False:
+        raise Exception ("Detector not in safe position: move it back")
+    
+
+def onPuckLoadingChange(puck_loading):
+    set_led_state(puck_loading)
+    #pass 
+
+update()
+add_device(Controller.getInstance().basePlate, True)
+restore_samples_info()
+    
+
+print "Initialization complete"

script/motion/calibrate_tool.py

@@ -0,0 +1,32 @@
+def calibrate_tool():
+    """
+    """
+    print "calibrate_tool"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+    (detected, dm) = move_scanner()
+
+    if detected:
+        print "Pin detected, trashing..."
+        trash()
+        (detected, dm) = move_scanner()
+        if detected:
+            raise Exception("Cannot trash pin")
+    
+    robot.open_tool()  
+    robot.get_calibration_tool()
+    
+    run("calibration/ToolCalibration3")
+    
+    robot.put_calibration_tool()
+    
+    robot.save_program()

script/motion/dry.py

@@ -0,0 +1,58 @@
+DEFAULT_DRY_HEAT_TIME = 40.0
+DEFAULT_DRY_SPEED = 0.3
+DEFAULT_DRY_WAIT_COLD = 40.0
+
+def dry(heat_time=None, speed=None, wait_cold = None):    
+    """
+        heat_time (float): in seconds
+        speed (float): % of nominal speed
+        wait_cold(float): if negative, move to dewar after drying
+                          Else move to cold and wait (in seconds) before returning.
+    """
+    print "dry"
+
+    if heat_time is None: 
+        heat_time = DEFAULT_DRY_HEAT_TIME
+
+    if speed is None: 
+        speed = DEFAULT_DRY_SPEED
+
+    if wait_cold is None: 
+        wait_cold = DEFAULT_DRY_WAIT_COLD
+
+    if robot.simulated:
+        time.sleep(10.0)
+        return  
+            
+    #Initial chec
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    set_status("Drying")
+    
+    #Enabling
+    enable_motion()    
+
+    try:
+        set_heater(True)
+        robot.move_heater(speed, False)    
+        time.sleep(heat_time)
+        robot.move_heater(speed, True)    
+        set_air_stream(True)        
+        robot.move_heater(speed, False)    
+    finally:
+        set_heater(False)
+        set_air_stream(False)
+
+
+    set_setting("dry_mount_counter", 0)
+    set_setting("dry_timestamp",time.time())
+
+    if wait_cold >=0 :
+        robot.move_cold()  
+        time.sleep(wait_cold)
+    else:
+        robot.move_park()

script/motion/get_aux.py

@@ -0,0 +1,21 @@
+def get_aux(sample):
+    """
+    """
+    print "get_aux: ",sample
+    
+    #Initial checks
+    assert_valid_sample(sample)
+    
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    hexiposi.assert_homed()
+    
+    #Enabling
+    enable_motion()
+    
+    if not robot.is_aux():
+        robot.move_aux()     
+            
+    robot.get_aux(sample)

script/motion/get_dewar.py

@@ -0,0 +1,30 @@
+def get_dewar(segment, puck, sample, force=False):
+    """
+    """
+    print "get_dewar: ", segment, puck, sample, force
+    
+    #Initial checks
+    assert_valid_address(segment, puck, sample)
+    assert_puck_detected(segment, puck)
+    
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+    hexiposi.assert_homed()
+    #location = robot.get_current_point()
+    
+
+    #Enabling
+    enable_motion()
+    
+    set_hexiposi(segment)
+    
+    if not force:        
+        visual_check_hexiposi(segment)           
+
+    if not robot.is_dewar():
+        robot.move_dewar()      
+            
+    robot.get_dewar(segment, puck, sample)

script/motion/get_gonio.py

@@ -0,0 +1,20 @@
+def get_gonio(force=False):
+    """
+    """
+    print "get_gonio: ", force
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+
+    if not robot.is_gonio():
+        robot.move_gonio()
+            
+    robot.get_gonio()

script/motion/homing_hexiposi.py

@@ -0,0 +1,18 @@
+def homing_hexiposi():
+    """
+    """
+    print "homing_hexiposi"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+    #location = robot.get_current_point()
+    
+    set_status("Homing hexiposi")
+    hexiposi.move_home()
+    hexiposi.waitState(State.Ready,-1)
+    hexiposi.move_pos(1)
+        
+    

script/motion/mount.py

@@ -0,0 +1,153 @@
+mount_sample_id = None
+mount_sample_detected = None
+
+def mount(segment, puck, sample, force=False, read_dm=False, auto_unmount=False):
+    """
+    """
+    global mount_sample_id, mount_sample_detected
+    print "mount: ", segment, puck, sample, force
+
+    start = time.time()    
+    #time.sleep(2)
+    is_aux = (segment == AUX_SEGMENT)
+
+    #ZACH
+    needs_chilling = not is_aux and (not robot.is_cold())
+    needs_drying = is_aux and robot.is_cold()
+    
+    puck_address = get_puck_address(puck)
+    if puck_address is None:
+        puck_obj = get_puck_obj_by_id(puck)
+        if puck_obj is not None:
+            puck_address = puck_obj.name
+    if puck_address is not None:
+        print "puck address: ", puck_address
+        segment = puck_address[:1]
+        puck = int(puck_address[1:])        
+    #Initial checks
+    assert_detector_safe()    
+    assert_valid_address(segment, puck, sample)
+    assert_puck_detected(segment, puck)
+
+    if robot.simulated:
+        time.sleep(3.0)
+        mount_sample_detected = True     
+        mount_sample_id = "YYY0001"
+        update_samples_info_sample_mount(get_puck_name(segment, puck), sample, mount_sample_detected, mount_sample_id)    
+        set_setting("mounted_sample_position", get_sample_name(segment, puck, sample))
+        return [mount_sample_detected, mount_sample_id]
+            
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+    hexiposi.assert_homed()    
+    assert_mount_position()
+    do_unmount = False    
+    
+    try:
+        #ZACH
+        if needs_chilling:
+            robot.move_cold()
+            time.sleep(30.0)
+
+        if smart_magnet.get_supress() == True:
+            smart_magnet.set_supress(False)
+            time.sleep(0.2)        
+        #To better dectect sample
+        #smart_magnet.apply_reverse()
+        #smart_magnet.apply_resting()
+        #time.sleep(0.5)
+                
+        sample_det = smart_magnet.check_mounted(idle_time=0.25, timeout = 1.0)
+        Controller.getInstance().logEvent("Sample Detection", str(sample_det))       
+        if sample_det == True:
+            if auto_unmount and (get_setting("mounted_sample_position") is not None):
+                #auto_unmount set to true so detection remains enabled
+                sample_det = unmount(force = True, auto_unmount = True)
+                do_unmount = True
+        if sample_det == True:
+            raise Exception("Pin detected on gonio")            
+            
+        set_status("Mounting: " + str(segment) + str(puck) + str(sample))    
+        Controller.getInstance().logEvent("Mount Sample", str(segment) + str(puck) + str(sample))
+        #location = robot.get_current_point()
+    
+        #Enabling. If did unmount then it is already enabled.
+        if not do_unmount:
+            enable_motion()
+
+        #ZACH
+        # a room temp pin is being mounted but the gripper is cold
+        if needs_drying:
+            dry(wait_cold=-1) # move to park after dry
+
+        if is_aux:
+            if not robot.is_aux():
+                robot.move_aux()      
+                
+            robot.get_aux(sample)
+                    
+        else:
+            set_hexiposi(segment)
+            
+            if not force:        
+                visual_check_hexiposi(segment)           
+    
+            if not robot.is_dewar():
+                robot.move_dewar()      
+                
+            robot.get_dewar(segment, puck, sample)
+
+            
+        if read_dm:
+            barcode_reader.start_read(10.0)
+            robot.move_scanner()     
+            #time.sleep(1.0)           
+            
+        robot.move_gonio()    
+        
+        if read_dm:
+            mount_sample_id = barcode_reader.get_readout()
+            print "Datamatrix: " , mount_sample_id
+        else:
+            mount_sample_id = None
+            
+           
+        robot.put_gonio() 
+        cleaner_timer = float(get_setting("pin_cleaner_timer")) 
+        if  cleaner_timer > 0:
+            start_pin_cleaner(cleaner_timer)
+
+        
+        try:
+            dry_mount_count = int(get_setting("dry_mount_counter"))
+        except:
+            dry_mount_count = 0    
+        set_setting("dry_mount_counter", dry_mount_count+1)    
+                
+        if is_aux:
+            robot.move_home() 
+        else:
+            robot.move_cold() 
+            
+        mount_sample_detected = smart_magnet.check_mounted(idle_time=0.25, timeout = 1.0) 
+        Controller.getInstance().logEvent("Sample Detection", str(mount_sample_detected))       
+        update_samples_info_sample_mount(get_puck_name(segment, puck), sample, mount_sample_detected, mount_sample_id)      
+        if mount_sample_detected == False:
+           raise Exception("No pin detected on gonio")
+
+    
+        if is_force_dry():
+           smart_magnet.set_default_current()
+           print "Auto dry"
+           log("Starting auto dry", False)
+           set_exec_pars(then = "dry()")
+
+        set_setting("mounted_sample_position", get_sample_name(segment, puck, sample))
+        return [mount_sample_detected, mount_sample_id]
+    finally:
+        smart_magnet.set_default_current()
+        smart_magnet.set_supress(True)
+

script/motion/move_aux.py

@@ -0,0 +1,18 @@
+def move_aux():
+    """
+    """
+    print "move_aux"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+
+    if not robot.is_aux():
+        robot.move_aux()  

script/motion/move_cold.py

@@ -0,0 +1,22 @@
+def move_cold():
+    """
+    """
+    print "move_cold"
+    
+    if robot.simulated:
+        time.sleep(3.0)
+        return
+            
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+
+    if not robot.is_cold():
+        robot.move_cold()  

script/motion/move_dewar.py

@@ -0,0 +1,18 @@
+def move_dewar():
+    """
+    """
+    print "move_dewar"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+
+    if not robot.is_dewar():
+        robot.move_dewar()  

script/motion/move_gonio.py

@@ -0,0 +1,18 @@
+def move_gonio():
+    """
+    """
+    print "move_gonio"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+
+    if not robot.is_gonio():
+        robot.move_gonio()

script/motion/move_heater.py

@@ -0,0 +1,18 @@
+def move_heater():
+    """
+    """
+    print "move_heater"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+
+    if not robot.is_heater():
+        robot.move_heater()

script/motion/move_home.py

@@ -0,0 +1,18 @@
+def move_home():
+    """
+    """
+    print "move_home"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+
+    if not robot.is_home():
+        robot.move_home()

script/motion/move_park.py

@@ -0,0 +1,18 @@
+def move_park():
+    """
+    """
+    print "move_park"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+
+    if not robot.is_park():
+        robot.move_park()  

script/motion/move_scanner.py

@@ -0,0 +1,30 @@
+
+def move_scanner():
+    """
+    """
+    print "move_scanner"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()    
+
+    barcode_reader.start_read(10.0)
+    if not robot.is_scanner():
+        robot.move_scanner()
+
+    time.sleep(0.25)
+    dm = barcode_reader.get_readout()    
+    if dm is None:
+        detected =  is_pin_detected_in_scanner()
+    else: 
+        detected = True
+
+    print ("Detected: " + str( detected) +  " - Datamatrix: " + str(dm))
+
+    return (detected, dm)

script/motion/put_aux.py

@@ -0,0 +1,21 @@
+def put_aux(sample):
+    """
+    """
+    print "put_aux: ",sample
+    
+    #Initial checks
+    assert_valid_sample(sample)
+    
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    hexiposi.assert_homed()
+
+    #Enabling
+    enable_motion()
+
+    if not robot.is_aux():
+        robot.move_aux()  
+           
+    robot.put_aux(sample)

script/motion/put_dewar.py

@@ -0,0 +1,30 @@
+def put_dewar(segment, puck, sample, force=False):
+    """
+    """
+    print "put_dewar: ", segment, puck, sample, force
+    
+    #Initial checks
+    assert_valid_address(segment, puck, sample)
+    assert_puck_detected(segment, puck)
+    
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+    hexiposi.assert_homed()
+
+    #Enabling
+    enable_motion()
+
+    set_hexiposi(segment)
+
+    if not force:        
+        visual_check_hexiposi(segment)
+
+    #location = robot.get_current_point()
+
+    if not robot.is_dewar():
+        robot.move_dewar()      
+
+    robot.put_dewar(segment, puck, sample)

script/motion/put_gonio.py

@@ -0,0 +1,23 @@
+def put_gonio(force=False):
+    """
+    """
+    print "put_gonio: ", force
+
+
+    if robot.simulated:
+        time.sleep(3.0)
+        return      
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+    
+    if not robot.is_gonio():
+        robot.move_gonio()
+    robot.put_gonio()    

script/motion/recover.py

@@ -0,0 +1,179 @@
+import org.apache.commons.math3.geometry.euclidean.threed.Segment as Segment
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D as Vector3D
+import org.apache.commons.math3.geometry.euclidean.threed.Line as Line3D
+
+RECOVER_DESC = "mRecovery"
+RECOVER_TOOL = TOOL_DEFAULT
+
+known_segments = [ ("pGonioA",       "pGonioG",       10), \               
+                   ("pPark",         "pScan",         40), \
+                   ("pHome",         "pPark",         60), \
+                   ("pScan",         "pHeater",       50), \
+                   ("pHome",         "pDewar",        10), \
+                   ("pHeater",       "pHeatB",        10), \
+                   ("pHome",         "pAux",          50), \
+                 ]
+
+
+def get_robot_position():        
+    return robot.get_cartesian_pos()
+
+
+def get_dist_to_line(segment):
+    tolerance = segment[2]
+    p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
+    p =  get_robot_position()
+    v = Vector3D(p[0], p[1], p[2])
+    v1  = Vector3D(p1[0], p1[1], p1[2])
+    v2  = Vector3D(p2[0], p2[1], p2[2])
+    l = Line3D(v1, v2, 0.01)
+    return l.distance(v)
+
+def get_dist_to_segment(segment):
+    tolerance = segment[2]
+    p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
+    p =  get_robot_position()
+    v = Vector3D(p[0], p[1], p[2])
+    v1  = Vector3D(p1[0], p1[1], p1[2])
+    v2  = Vector3D(p2[0], p2[1], p2[2])
+    l = Line3D(v1, v2, 0.01)
+    d = l.distance(v)    
+
+    pj = get_projection_at_line(segment)
+    
+    d1, d2 = v1.distance(v), v2.distance(v)
+    dp1, dp2 = v1.distance(pj), v2.distance(pj)
+    d12 = v1.distance(v2)
+    
+    
+    if (dp1 + dp2) > (d12 + tolerance):
+        d = max(d,min(d1,d2))
+    return d
+    
+def is_on_segment(segment):
+    tolerance = segment[2]
+    d = get_dist_to_segment(segment)
+
+    if d > tolerance:
+        #print "Current robot position " + str(p) + " not on segment " + str(segment) + " - distance=" + str(d)
+        return False
+    #print "Current robot position " + str(p) + " on segment " + str(segment) + " - distance=" + str(d)
+    return True
+
+def get_projection_at_line(segment):
+    tolerance = segment[2]
+    p1, p2 = robot.get_pnt(segment[0]), robot.get_pnt(segment[1])
+    p =  get_robot_position()
+    v = Vector3D(p[0], p[1], p[2])
+    v1  = Vector3D(p1[0], p1[1], p1[2])
+    v2  = Vector3D(p2[0], p2[1], p2[2])
+    l = Line3D(v1, v2, 0.01)
+    a = l.getAbscissa(v)
+    lv = l.pointAt(a) 
+    return lv
+    
+
+def get_current_segment():
+    for segment in known_segments:
+        if is_on_segment(segment):    
+            return segment    
+    return None
+
+def get_current_distance():    
+    for segment in known_segments:
+        if is_on_segment(segment):  
+            return get_dist_to_segment(segment)   
+    return None
+
+
+def move_to_segment(segment):
+    tolerance = segment[2]
+    p =  get_robot_position()
+    v = Vector3D(p[0], p[1], p[2])
+    lv = get_projection_at_line(segment)
+    dlv = lv.distance(v)
+    if dlv> (tolerance + 0.1):
+        raise Exception( "Error moving from " + str(p) + " to segment - distance=" + str(dlv))
+    d =  [lv.x, lv.y, lv.z, p[3], p[4], p[5]]
+    print "Moving from " + str(p) + " to segment " + str(segment) + " - distance=" + str(dlv)  + " - dest=" + str(d)
+        
+    try: 
+        robot.movel(d, tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True)       
+        print "Done" 
+    except:
+        print sys.exc_info()[1]
+
+#Moves to first point of the segment ehich is safer, unless in the vicinity of the second
+def move_to_safest_point(segment, vicinity_tolerance = 100):
+    d1, d2 = robot.get_distance_to_pnt(segment[0]),  robot.get_distance_to_pnt(segment[1])
+    #Always moving to primary point
+    if False : #(d2<=d1) and (d2 <= vicinity_tolerance):
+        print "Moving to secondary point " + str(segment[1] + " - d1=" + str(d1) + " d2=" + str(d2) )
+        robot.movel(segment[1], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True) 
+    else:
+        print "Moving to primary point " + str(segment[0] + " - d1=" + str(d1) + " d2=" + str(d2) )
+        robot.movel(segment[0], tool=RECOVER_TOOL, desc=RECOVER_DESC, sync=True) 
+    print "Done" 
+    #print "Recovered to point " + str(robot.get_curjoint_or_pointrent_point())
+
+def is_in_dewar():
+    z_hom = robot.get_pnt('pHome')[2]
+    z_cur=get_robot_position()[2]
+    if z_cur < (z_hom + 30):
+        return False
+    d_dwr = robot.get_distance_to_pnt('pDewar')
+    if d_dwr > 300:
+        return False
+    return True
+    
+     
+def recover(move_park = True):
+    #Initial checks    
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+
+    if robot.get_current_point() is not None:
+        raise Exception("Robot is in known location")
+
+    set_status("Recovering")
+    
+    #Enabling
+    enable_motion()
+
+
+    is_on_known_segment = False
+    for segment in known_segments:
+        if is_on_segment(segment):
+            #try:
+            #    robot.set_monitor_speed(5)
+                is_on_known_segment = True
+                move_to_segment(segment)
+                move_to_safest_point(segment)
+                location = robot.get_current_point()
+                if location is None:
+                    raise Exception("Robot didn't reach known point")
+                print "Success recovered to point: " +  str(location)
+                if move_park:
+                    robot.move_park()                  
+                    return "Success recovering to park position"
+                else:
+                    return "Success recovering to point: " +  str(location)                
+            #finally:     
+            #    robot.set_default_speed()
+    if not is_on_known_segment:
+        print ("Robot is not in known segment")
+        if is_in_dewar():
+            robot.robot_recover()        
+            if move_park:
+                robot.move_park()                  
+                return "Success recovering from dewar to park position"
+            else:
+                return "Success recovering from dewar"   
+    else:
+            raise Exception("Robot is not in known segment nor inside the dewar")
+ 
+
+
+
+   

script/motion/robot_recover.py

@@ -0,0 +1,16 @@
+def robot_recover():
+    """
+    """
+    print "robot_recover"
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+
+    set_status("Recovering robot")
+    
+    #Enabling
+    enable_motion()    
+
+    robot.robot_recover()

script/motion/scan_pin.py

@@ -0,0 +1,99 @@
+###TODO: REMOVE ME
+def system_check(robot_move=True):
+   pass
+
+
+def scan_pin(segment, puck, sample, force=False):
+    pin_name = get_sample_name(segment, puck, sample)
+    
+    print "scan pin", pin_name
+    #Initial checks
+    assert_valid_address(segment, puck, sample)
+    #assert_puck_detected(segment, puck)
+    is_aux = (segment == AUX_SEGMENT)
+
+    if robot.simulated:
+        time.sleep(0.5)
+        return "Present"    
+    
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    ###robot.assert_in_known_point()
+    
+    #Enabling
+    
+    set_status("Scanning pin: " + str(pin_name))
+    
+    enable_motion()    
+
+    if is_aux:
+        if not robot.is_aux():
+            robot.move_aux()      
+            
+        robot.get_aux(sample)               
+    else:
+    #    set_hexiposi(segment)
+    #    if not force:        
+    #        visual_check_hexiposi(segment)           
+        
+        if not robot.is_dewar():
+            robot.move_dewar()      
+                
+        robot.get_dewar(segment, puck, sample)
+        
+    (detected, dm) = move_scanner() 
+    update_samples_info_sample_scan(get_puck_name(segment, puck), sample, detected, dm)      
+
+    if is_aux:
+        robot.move_aux()   
+        robot.put_aux( sample)
+    else:
+        robot.move_dewar()        
+        robot.put_dewar(segment, puck, sample)
+    ret = "Empty"
+    if detected:
+        if (dm is None) or (len(dm.strip())==0):
+            ret = "Present"
+        else:
+            ret = str(dm)
+    return ret
+
+    
+def scan_puck(segment, puck, force=False):
+    if segment == AUX_SEGMENT:
+        raise Exception("Cannot scan auxiliary puck")
+    ret = []
+    for i in range(16):
+        ret.append(scan_pin (segment, puck, i+1, force))
+    return ret
+
+
+def scan_gripper():
+    print "scan gripper"
+    #Initial checks
+    
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+    
+    #Enabling
+    
+    set_status("Scanning gripper")
+    
+    enable_motion()    
+
+    (detected, dm) = move_scanner() 
+
+    robot.move_home()
+
+    ret = "Empty"
+    if detected:
+        if (dm is None) or (len(dm.strip())==0):
+            ret = "Present"
+        else:
+            ret = str(dm)
+    return ret

script/motion/tools.py

@@ -0,0 +1,180 @@
+POSITION_TOLERANCE = 50
+
+
+def is_manual_mode():
+    """
+    return if operating in local mode. TODO: should be based on IO, not on robot flag.
+    """    
+    return  robot.working_mode == "manual"
+
+def release_safety():
+    """
+    Release sefety signals acording to working mode
+    """
+    if air_pressure_ok.take() != True:
+        raise Exception("Cannot release safety: air preassure not ok") 
+    if n2_pressure_ok.take() != True:
+        raise Exception("Cannot release safety: n2 pressure not ok")         
+    auto = not is_manual_mode()
+    if auto:
+        if feedback_psys_safety.read() == False:
+            release_psys()  
+            time.sleep(0.5)   
+            if feedback_psys_safety.read() == False:
+                raise Exception("Cannot enable power: check doors")
+
+    if feedback_local_safety.read() == False:
+        release_local()    
+        time.sleep(0.5)    
+        if feedback_local_safety.read() == False:
+            raise Exception("Cannot enable power: check sample changer emergency stop button") 
+            
+def enable_motion():
+    """
+    Check safety and enable arm power if in remote mode
+    """
+    release_safety()  
+    system_check(robot_move=True)  
+    auto = not is_manual_mode()        
+    if auto:
+        if not robot.state.isNormal():
+            raise Exception("Cannot enable power: robot state is " + str(robot.state))
+        robot.enable()
+
+def set_hexiposi(pos, force = False):
+    """
+    Set the hexiposi position in remote mode, or wait for it to be set in manual mode
+    """
+    robot.assert_cleared()
+    if force == False:
+        if hexiposi.position == pos:
+            return 
+        
+    if is_manual_mode():
+        set_status("Move Hexiposi to position " + str(pos) + " ...")
+        try:
+            hexiposi.waitInPosition(pos, -1)
+        finally:
+            set_status(None)    
+    else:
+        hexiposi.move(pos)
+
+#Can be used if cover has following error (no checking readback)
+def _set_hexiposi(pos):
+    hexiposi.moveAsync(pos)
+    time.sleep(1.0)
+    hexiposi.waitReady(-1)
+            
+
+def visual_check_hexiposi(segment):
+    if is_imaging_enabled():
+        #if is_manual_mode(): ?
+        if hexiposi.moved:
+            #Clearing for image processing
+            if not robot.is_park():
+                print "Moving robot to park to clear camera view..."
+                robot.move_park()        
+            assert_img_in_cover_pos(segment)    
+            hexiposi.moved = False           
+
+
+
+def wait_end_of_move():
+    robot.update()
+    while (not robot.settled) or (not robot.empty) or (not robot.isReady()) :
+        time.sleep(0.01)
+
+
+def move_to_home():        
+    #robot.reset_motion("jHome")
+    robot.movej("pHome", robot.tool , DESC_SCAN)    
+    wait_end_of_move()
+
+def move_to_laser(from_point = "pHome"):
+    robot.reset_motion()
+    tool = robot.tool
+    d = robot.get_distance_to_pnt("pLaser")
+    if d<0:
+        raise Exception ("Error calculating distance to laser: " + str(d))
+    if d<POSITION_TOLERANCE:
+        print "FROM LASER"
+        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
+        return
+    d = robot.get_distance_to_pnt("pLaserHome")
+    if d<0:
+        raise Exception ("Error calculating distance to laser appro: " + str(d))
+    if d<POSITION_TOLERANCE:
+        print "FROM APPRO"
+        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
+        return
+        
+    d = robot.get_distance_to_pnt(from_point)
+    if d<0:
+        raise Exception ("Error calculating distance to " + from_point + ": " + str(d))
+    if d<POSITION_TOLERANCE:
+        print "FROM " + from_point
+        robot.movej("pLaserHome", robot.tool, DESC_DEFAULT)
+        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)    
+        return
+    raise Exception ("Must be in home position to start move to laser")
+    
+
+def update_tool(tool=None, x_offset=0.0, y_offset=0.0, z_offset=0.0):
+    #Updating tool:
+    t=robot.get_tool_trsf(tool)
+    t[0]=t[0] - x_offset
+    t[1]=t[1] - y_offset
+    t[2]=t[2] - z_offset
+    robot.set_tool_trsf(t, tool)
+    print "Updated " + (str(robot.tool) if (tool is None) else tool) + ": " + str(t) 
+    robot.save_program()
+
+
+def assert_valid_address(segment, puck, sample):
+    if (segment == AUX_SEGMENT) and (puck == 1):
+        return 
+    if is_string(segment):
+        segment = ord(segment.upper()) - ord('A') +1
+    if (segment<=0) or (segment >6):
+        raise Exception ("Invalid segment: " + str(segment))
+    if (puck<=0) or (puck >5):
+        raise Exception ("Invalid puck: " + str(puck))
+    if (sample<=0) or (sample >16):
+        raise Exception ("Invalid sample: " + str(sample))
+    if get_puck_dev(segment, puck).isDisabled():
+        raise Exception ("Puck is disabled")
+        
+def assert_valid_sample(sample):
+    if (sample<=0) or (sample >16):
+        raise Exception ("Invalid sample: " + str(sample))
+
+def get_puck_name(segment, puck):
+    try:
+        assert_valid_address(segment, puck, 1)
+        if type(segment) is int:
+            segment = chr( ord('A') + (segment-1))
+        elif is_string(segment):
+            segment = segment.upper()    
+        else:
+            return None
+        return segment + str(puck)         
+    except:
+        return None
+
+def get_sample_name(segment, puck, sample):
+    puck_name = get_puck_name(segment, puck)
+    return None if (puck_name is None) else puck_name + str(sample)
+
+
+def is_pin_detected_in_scanner():
+    samples = []
+    for i in range(10):
+        samples.append(laser_distance.read())
+        time.sleep(0.05)
+    av = mean(samples)
+    for s in samples:
+         if s<=1:
+             return False
+         if abs(s-av) > 0.1:
+             return False
+    return True    

script/motion/trash.py

@@ -0,0 +1,36 @@
+def trash():
+    """
+    """
+    print "trash"
+
+    if robot.simulated:
+        time.sleep(3.0)
+        return    
+    
+    #Initial checks
+    robot.assert_no_task()
+    robot.reset_motion()
+    robot.wait_ready()
+    robot.assert_cleared()
+    #robot.assert_in_known_point()
+
+    #Enabling
+    enable_motion()
+
+    robot.move_heater(to_bottom = False)    
+    robot.move_heater(to_bottom = True)    
+
+    try:
+        for i in range(3):
+            robot.open_tool()
+            time.sleep(0.5)
+            robot.close_tool()
+            time.sleep(0.5)
+    finally:
+        robot.open_tool()
+        
+                
+    robot.move_heater(to_bottom =  False)    
+    robot.move_cold()
+
+    

script/motion/unmount.py

@@ -0,0 +1,108 @@
+def unmount(segment = None, puck = None, sample = None, force=False, auto_unmount = False):
+    """
+    Returns if sample is detected in the end
+    """
+    print "unmount: ", segment, puck, sample, force   
+    
+    #ZACH
+    is_aux = (segment == AUX_SEGMENT)
+    needs_chilling = not is_aux and (not robot.is_cold())
+    needs_drying = is_aux and robot.is_cold()
+
+    if (segment is None) or (puck is None) or (sample is None):
+        pos = get_setting("mounted_sample_position")
+        if pos is None:
+            raise Exception("Mounted sample position is not defined")
+        segment, puck , sample = pos[0:1], int(pos[1]), int(pos[2:])
+        print "Mounted sample position: ", segment, puck , sample
+
+    
+    #Initial checks
+    if not auto_unmount:    
+        print "assert detector safe"
+        assert_detector_safe()
+        print "assert valid address"
+        assert_valid_address(segment, puck, sample)
+        print "asser puck detected"
+        assert_puck_detected(segment, puck)
+    
+        if robot.simulated:
+            time.sleep(3.0)
+            update_samples_info_sample_unmount(get_puck_name(segment, puck), sample)   
+            set_setting("mounted_sample_position", None)
+            return False  
+    
+        print "assert no task"
+        robot.assert_no_task()
+        print "reset motion"
+        robot.reset_motion()
+        print "wait ready"
+        robot.wait_ready()
+        print "assert cleared"
+        robot.assert_cleared()
+        #robot.assert_in_known_point()
+        print "assert homed"
+        hexiposi.assert_homed()
+        print "assert mount pos"
+        assert_mount_position()
+    
+    set_status("Unmounting: " + str(segment) + str(puck) + str(sample))
+    Controller.getInstance().logEvent("Unmount Sample", str(segment) + str(puck) + str(sample))
+
+    try:
+        if smart_magnet.get_supress() == True:
+            smart_magnet.set_supress(False)
+            time.sleep(0.2)        
+        
+        #smart_magnet.apply_reverse() 
+        #smart_magnet.apply_resting()
+        
+        if not force:            
+            sample_det = smart_magnet.check_mounted(idle_time=0.5, timeout = 3.0)
+            Controller.getInstance().logEvent("Sample Detection", str(sample_det))       
+            if sample_det == False:
+                raise Exception("No pin detected on gonio")        
+    
+        #Enabling
+        enable_motion()
+
+        if not is_aux:
+            set_hexiposi(segment)
+    
+            if not force:        
+                visual_check_hexiposi(segment)
+            if needs_chilling:
+                robot.move_cold()
+                time.sleep(30.)
+        else:
+            if needs_drying:
+                dry(wait_cold=-1)
+        #location = robot.get_current_point()
+        
+        if not robot.is_gonio():
+            robot.move_gonio()
+                
+        #smart_magnet.set_unmount_current()
+
+        robot.get_gonio()
+
+        smart_magnet.apply_reverse()
+        smart_magnet.apply_resting()
+        mount_sample_detected = smart_magnet.check_mounted(idle_time=0.25, timeout = 1.0) 
+        Controller.getInstance().logEvent("Sample Detection", str(mount_sample_detected))       
+
+        if is_aux:
+            robot.move_aux()   
+            robot.put_aux( sample)
+        else:
+            #TODO: Should check if smart magnet detection is off?
+            update_samples_info_sample_unmount(get_puck_name(segment, puck), sample)      
+            robot.move_dewar()        
+            robot.put_dewar(segment, puck, sample)
+        set_setting("mounted_sample_position", None)
+
+        return mount_sample_detected
+    finally:
+        if not auto_unmount:
+            smart_magnet.set_default_current()
+            smart_magnet.set_supress(True)

script/setup/ExposureScan.py

@@ -0,0 +1,33 @@
+import java.awt.Rectangle as Rectangle
+
+import ch.psi.pshell.imaging.Data as Data
+class Exposure(Writable):
+    def write(self,pos):
+        #cam.setExposure(pos)
+        img.camera.setExposure(pos)
+exposure=Exposure()
+
+
+class Contrast(Readable):
+    def read(self):
+        data = img.getData()
+        #roi = Data(data.getRectSelection(500,300,700,600), False)
+        #return data.getGradientVariance(False, Rectangle(480,0,600,670))
+        return data.getGradientVariance(False, None)
+contrast=Contrast()
+
+
+#a= lscan(exposure,img.getContrast(), 0.5, 1.0, 0.01, 0.5)
+#a= lscan(exposure,contrast, 0.2, 0.4, 0.01, 0.7)
+ret= lscan(exposure,contrast, 10.0, 150.0, 5.0, 0.5)
+y, x = ret.getReadable(0), ret.getPositions(0)
+
+#(n, m, s) = fit(a.getReadable(0), xdata=a.getPositions(0))
+#if m is None:
+#    m=max(a.getReadable(0))
+m=x[y.index(max(y))]
+p=get_plots()[0]
+p.addMarker(m, p.AxisId.X, str(m), Color.RED)
+
+print "Setting exposure = ", m
+exposure.write(m)

script/setup/Layout.py

@@ -0,0 +1,292 @@
+################################################################################################### 
+#DEFINITIONS
+################################################################################################### 
+
+PLATE_SIZE = 480
+BLOCK_ROI_TOLERANCE = 12 #mm
+LED_TOLERANCE = 8 #mm Distance between LEDs = 18mm
+PUCK_SIZE = 65
+
+DET_UNIPUCK = "unipuck"
+DET_MINISPINE = "minispine"
+DET_ERROR = "error"
+DET_EMPTY = "empty"
+DET_UNKNOWN = "unknown"
+
+BLOCKS = ('A', 'B', 'C', 'D', 'E', 'F')
+
+#Layout table
+puck_layout = (
+    #Num  Elm    A0 Index A1       Uni      Mini     Center   Angle    Xuni     Yuni      Xmini=Xc  Ymini==Yc
+    (1 ,  'A',   0  , 1,   0.00 ,   57.00 ,  75.00 ,  66.00 ,  0.00  ,  0.00   , 57.00  , 0.00   , 75.00  ),
+    (2 ,  'A',   0  , 2,   0.00 ,   132.00,  150.00,  141.00,  0.00  ,  0.00   , 132.00 , 0.00   , 150.00 ),
+    (3 ,  'F',   0  , 5,   19.11,   180.40,  198.40,  189.40,  19.11 ,  59.06  , 170.46 , 64.95  , 187.47 ),
+    (4 ,  'F',   0  , 4,   40.89,   180.40,  198.40,  189.40,  40.89 ,  118.09 , 136.38 , 129.87 , 149.98 ),
+    (5 ,  'F',   0  , 3,   30.00,   111.90,  129.90,  120.90,  30.00 ,  55.95  , 96.91  , 64.95  , 112.50 ),
+    (6 ,  'F',   60 , 1,   0.00 ,   57.00 ,  75.00 ,  66.00 ,  60.00 ,  49.36  , 28.50  , 64.95  , 37.50  ),
+    (7 ,  'F',   60 , 2,   0.00 ,   132.00,  150.00,  141.00,  60.00 ,  114.32 , 66.00  , 129.90 , 75.00  ),
+    (8 ,  'E',   60 , 5,   19.11,   180.40,  198.40,  189.40,  79.11 ,  177.15 , 34.08  , 194.83 , 37.48  ),
+    (9 ,  'E',   60 , 4,   40.89,   180.40,  198.40,  189.40,  100.89,  177.15 , -34.08 , 194.83 , -37.48 ),
+    (10,  'E',   60 , 3,   30.00,   111.90,  129.90,  120.90,  90.00 ,  111.90 , 0.00   , 129.90 , 0.00   ),
+    (11,  'E',   120, 1,   0.00 ,   57.00 ,  75.00 ,  66.00 ,  120.00,  49.36  , -28.50 , 64.95  , -37.50 ),
+    (12,  'E',   120, 2,   0.00 ,   132.00,  150.00,  141.00,  120.00,  114.32 , -66.00 , 129.90 , -75.00 ),
+    (13,  'D',   120, 5,   19.11,   180.40,  198.40,  189.40,  139.11,  118.09 , -136.38, 129.87 , -149.98),
+    (14,  'D',   120, 4,   40.89,   180.40,  198.40,  189.40,  160.89,  59.06  , -170.46, 64.95  , -187.47),
+    (15,  'D',   120, 3,   30.00,   111.90,  129.90,  120.90,  150.00,  55.95  , -96.91 , 64.95  , -112.50),
+    (16,  'D',   180, 1,   0.00 ,   57.00 ,  75.00 ,  66.00 ,  180.00,  0.00   , -57.00 , 0.00   , -75.00 ),
+    (17,  'D',   180, 2,   0.00 ,   132.00,  150.00,  141.00,  180.00,  0.00   , -132.00, 0.00   , -150.00),
+    (18,  'C',   180, 5,   19.11,   180.40,  198.40,  189.40,  199.11,  -59.06 , -170.46, -64.95 , -187.47),
+    (19,  'C',   180, 4,   40.89,   180.40,  198.40,  189.40,  220.89,  -118.09, -136.38, -129.87, -149.98),
+    (20,  'C',   180, 3,   30.00,   111.90,  129.90,  120.90,  210.00,  -55.95 , -96.91 , -64.95 , -112.50),
+    (21,  'C',   240, 1,   0.00 ,   57.00 ,  75.00 ,  66.00 ,  240.00,  -49.36 , -28.50 , -64.95 , -37.50 ),
+    (22,  'C',   240, 2,   0.00 ,   132.00,  150.00,  141.00,  240.00,  -114.32, -66.00 , -129.90, -75.00 ),
+    (23,  'B',   240, 5,   19.11,   180.40,  198.40,  189.40,  259.11,  -177.15, -34.08 , -194.83, -37.48 ),
+    (24,  'B',   240, 4,   40.89,   180.40,  198.40,  189.40,  280.89,  -177.15, 34.08  , -194.83, 37.48  ),
+    (25,  'B',   240, 3,   30.00,   111.90,  129.90,  120.90,  270.00,  -111.90, 0.00   , -129.90, 0.00   ),
+    (26,  'B',   300, 1,   0.00 ,   57.00 ,  75.00 ,  66.00 ,  300.00,  -49.36 , 28.50  , -64.95 , 37.50  ),
+    (27,  'B',   300, 2,   0.00 ,   132.00,  150.00,  141.00,  300.00,  -114.32, 66.00  , -129.90, 75.00  ),
+    (28,  'A',   300, 5,   19.11,   180.40,  198.40,  189.40,  319.11,  -118.09, 136.38 , -129.87, 149.98 ),
+    (29,  'A',   300, 4,   40.89,   180.40,  198.40,  189.40,  340.89,  -59.06 , 170.46 , -64.95 , 187.47 ),
+    (30,  'A',   300, 3,   30.00,   111.90,  129.90,  120.90,  330.00,  -55.95 , 96.91  , -64.95 , 112.5  ),
+    )
+
+################################################################################################### 
+#Puck class
+################################################################################################### 
+class Puck:
+    def __init__(self, id, block, index, angle, center, led_uni, led_mini):
+        self.id = id
+        self.block = block
+        self.index = index
+        self.angle = angle 
+        self.center = center
+        self.led_uni = led_uni
+        self.led_mini = led_mini
+        self.detect = DET_UNKNOWN
+        
+    def __str__(self):
+        return "Number: " + str(self.id) + "\nBlock: " + str(self.block) + "\nIndex: " + str(self.index) + "\nAngle: " + str(self.angle)  + \
+               "\nCenter: " + str(self.center) + "\nLed Unipuck: " + str(self.led_uni) + "\nLed Minispine: " + str(self.led_mini) 
+
+    def get_name(self):
+        return str(self.block) + str(self.index)
+        
+    def match(self, x, y):
+        if math.hypot(x-self.led_uni[0], y-self.led_uni[1]) <= LED_TOLERANCE:
+            return DET_UNIPUCK
+        if math.hypot(x-self.led_mini[0], y-self.led_mini[1]) <= LED_TOLERANCE:
+            return DET_MINISPINE
+        return None
+    
+    
+
+_block_ids = []        
+_puck_list = []   
+_block_list = []      
+
+for p in(puck_layout):
+    puck = Puck(p[0], p[1], p[3], p[8], (p[11],p[12]), (p[9],p[10]), (p[11],p[12]))
+    _puck_list.append(puck)    
+    if puck.block not in (_block_ids):
+        _block_ids.append(puck.block)    
+
+def get_puck(id):
+    for p in _puck_list:
+        if id==p.id:
+            return p
+    return None    
+
+def get_pucks(block = None):
+    ret = []
+    for  p in _puck_list:
+        if (block is None) or (block==p.block):
+            ret.append(p)
+    return ret
+
+
+################################################################################################### 
+#Block class
+################################################################################################### 
+
+class Block:
+    def __init__(self, id, angle_range, x_range, y_range):
+        self.id = id
+        self.angle_range = angle_range
+        self.x_range = x_range
+        self.y_range = y_range
+        self.roi = (self.x_range[0] - BLOCK_ROI_TOLERANCE, self.y_range[0] - BLOCK_ROI_TOLERANCE, 
+                    self.x_range[1] + BLOCK_ROI_TOLERANCE, self.y_range[1] + BLOCK_ROI_TOLERANCE)
+
+    def __str__(self):
+        return "Id: " + str(self.id) + "\nAngle: " + str(self.angle_range) + "\nX: " + str(self.x_range) + "\nY: " + str(self.y_range) 
+
+
+for id in _block_ids:
+    pucks = get_pucks(id)
+    angles, x, y = [], [], []    
+    for p in pucks: 
+        angles.append(p.angle)
+        x.append(p.led_uni[0])
+        x.append(p.led_mini[0])
+        y.append(p.led_uni[1])
+        y.append(p.led_mini[1])
+    el = Block(id,(min(angles), max(angles)), (min(x), max(x)), (min(y), max(y)))
+    _block_list.append(el)
+    
+def get_block(id):
+    for  e in _block_list:
+        if id==e.id:
+            return e
+    return None
+
+def get_blocks():
+    return _block_list
+    
+
+
+
+################################################################################################### 
+#Detection utilities
+################################################################################################### 
+
+
+def _detect_puck(point_list, puck):    
+    puck.detect = DET_ERROR
+    for point in point_list:
+        match = puck.match(point[0], point[1])
+        if match is not None:
+            if  match==DET_UNIPUCK:
+                puck.detect = DET_EMPTY if (puck.detect==DET_MINISPINE) else DET_UNIPUCK
+            elif  match==DET_MINISPINE:
+                puck.detect = DET_EMPTY if (puck.detect==DET_UNIPUCK) else DET_MINISPINE
+
+def detect_pucks(point_list, id=None):    
+    if (id is None) or (id in BLOCKS):
+        for puck in get_pucks(id):
+            _detect_puck(point_list, puck)
+    else:
+        puck = get_puck(int(id))
+        print puck
+        _detect_puck(point_list, puck)
+                               
+    
+def clear_detection(block_id=None):                     
+    for puck in get_pucks(block_id):
+        puck.detect = DET_UNKNOWN
+    return get_puck_detection_dict(block_id)
+
+
+def get_puck_detection(det_type, block_id=None):
+    ret = []
+    for puck in get_pucks(block_id):
+        if puck.detect == det_type:
+            ret.append(puck)
+    return ret
+
+def get_unipucks(block_id=None):
+    return get_puck_detection(DET_UNIPUCK, block_id)
+
+def get_minispines(block_id=None):
+    return get_puck_detection(DET_MINISPINE, block_id)
+
+def get_empties(block_id=None):
+    return get_puck_detection(DET_EMPTY, block_id)
+
+def get_unknowns(block_id=None):
+    return get_puck_detection(DET_UNKNOWN, block_id)
+
+def get_det_errors(block_id=None):
+    return get_puck_detection(DET_ERROR, block_id)
+
+def get_puck_detection_dict(block_id):
+    ret = {}
+    pucks = []
+    for puck in get_unipucks(block_id):
+        pucks.append(puck.get_name())
+    pucks.sort()    
+    ret["Unipuck"] = pucks
+    pucks = []
+    for puck in get_minispines(block_id):
+        pucks.append(puck.get_name())
+    pucks.sort()
+    ret["Minispine"] = pucks
+    pucks = []
+    for puck in get_det_errors(block_id):
+        pucks.append(puck.get_name())
+    pucks.sort()
+    ret["Error"] = pucks
+    pucks = []
+    for puck in get_empties(block_id):
+        pucks.append(puck.get_name())
+    pucks.sort()
+    ret["Empty"] = pucks
+    pucks = []
+    for puck in get_unknowns(block_id):
+        pucks.append(puck.get_name())
+    pucks.sort()
+    ret["Unknown"] = pucks
+    return ret
+        
+################################################################################################### 
+#Plotting
+################################################################################################### 
+
+from plotutils import *
+
+def plot_base_plate(points = None, show_detect = True, title = None, p = None):
+    colors = (Color.RED, Color.BLUE, Color.MAGENTA,  Color(128,0,128), Color(0,128,0), Color(255,128,0))
+    if p is None: p = plot(None, title=title)[0]
+    p.getAxis(p.AxisId.Y).setInverted(True)
+    plot_circle(p, 0, 0, PLATE_SIZE/2, width = 0, color = Color.GRAY, name = "Plate")
+    plot_point(p, 0, 0, size = 10, color = Color.GRAY, name = "Center")
+    #p.setLegendVisible(True)
+    for block in get_blocks():
+        (xmin, xmax) = block.x_range
+        (ymin, ymax) = block.y_range
+        (xmin, ymin, xmax, ymax ) = block.roi
+        index = get_blocks().index(block)
+        r = plot_rectangle(p, xmin, ymin, xmax, ymax, width =0, color=colors[index], name = block.id)
+        #In the first time the plot shows, it takes some time for the color to be assigned
+        #while r.color is None:
+        #    time.sleep(0.001)
+  
+        if block.id in ('A', 'F'):
+            x, y = (xmin + xmax)/2, ymax + 5
+        elif block.id in ('C', 'D'):
+            x, y = (xmin + xmax)/2, ymin - 5
+        elif block.id == 'B':
+            x, y = xmax + 5,  (ymin + ymax)/2
+        elif block.id == 'E':
+            x, y = xmin - 5,  (ymin + ymax)/2
+        
+        p.addText(x,y, str(block.id), r.color)
+        
+        for puck in get_pucks(block.id):
+            (xu, yu) = puck.led_uni
+            (xm, ym) = puck.led_mini
+            plot_point(p, xu, yu, size = 3, color = r.color, name = str(puck.id)+"u")
+            plot_point(p, xm, ym, size = 7, color = r.color, name = str(puck.id)+"m")
+            plot_circle(p, xu, yu, LED_TOLERANCE, width = 0, color = r.color, name = str(puck.id)+"uc")
+            plot_circle(p, xm, ym, LED_TOLERANCE, width = 0, color = r.color, name = str(puck.id)+"mc")
+            p.addText((xu+xm)/2, (yu+ym)/2, str(puck.id), r.color)
+            c,w =  Color.GRAY,0
+            if show_detect:
+                if  puck.detect == DET_UNIPUCK:
+                    c,w =  Color.BLACK,1
+                elif puck.detect == DET_MINISPINE:
+                    c,w =  Color(150, 100, 50),1
+                elif puck.detect == DET_ERROR:
+                    c =  Color(128,0,0)
+            plot_circle(p, xm, ym, PUCK_SIZE/2, width = w, color = c , name = str(puck.id))
+    if points is not None:
+        for point in points: 
+            c, w = Color.GRAY, 1
+            for puck in get_pucks():
+                match = puck.match(point[0], point[1])
+                if match is not None:
+                    w=2
+                    c = Color.DARK_GRAY if  match == "minispine" else Color.BLACK
+                    
+        
+            plot_cross(p,point[0], point[1], size = 12, width = w, color = c, name = "P"+ str(points.index(point)))
+            #plot_point(p,point[0], point[1], size = 5, color = Color.BLACK, name = "Pc"+ str(points.index(point)))                        

script/tasks/ColdPositionTimeout.py

@@ -0,0 +1,23 @@
+cold_position_timeout = int(get_setting("cold_position_timeout"))
+if cold_position_timeout > 0:    
+    if robot.last_command_position == "cold":         
+        if (time.time() - robot.last_command_timestamp)  > cold_position_timeout:  
+            if robot.is_cold():
+                log("Detected cold position timeout", False)
+                if get_context().state == State.Ready:
+                    if robot.state == State.Ready:
+                        if feedback_psys_safety.take() == True:
+                            #TODO: Chan
+                            get_context().evalLine("dry(wait_cold = -1)") #Dry and park : use get_context().evalLine to change application state
+                        else:
+                            raise Exception("Cannot clear cold position: feedback_psys_safety = False ")                        
+                    else: 
+                        raise Exception("Cannot clear cold position: robot state: " + str(robot.state))                        
+                else:
+                    raise Exception("Cannot clear cold position: system state: " + str(get_context().state))
+                 
+                            
+
+
+
+    

script/tasks/LedMonitoring.py

@@ -0,0 +1,26 @@
+DEWAR_LEVEL_LED = 25.0
+
+try:
+    _level = dewar_level.read()
+except:
+    log("Cannot read Dewar level", False)
+    _level = 0.0
+
+try:      
+    _led_room_temp = is_led_room_temp()
+except:
+    log("Cannot get LED configuration", False)
+    _led_room_temp = False
+
+if _level <= DEWAR_LEVEL_LED:
+    if not _led_room_temp:      
+        log_str="LED Monitoring: Setting LEDs to room temperature range" 
+        log(log_str, False)
+        print (log_str)
+        set_led_range(room_temp = True)
+else:        
+    if _led_room_temp:       
+        log_str="LED Monitoring: Setting LEDs to LN2 range" 
+        log(log_str, False)
+        print (log_str)
+        set_led_range(room_temp = False)

script/tasks/MangnetMonitoring.py

@@ -0,0 +1,24 @@
+try:
+    if feedback_local_safety.take() ==False:
+        magnet_release_state = magnet_release.read()
+        if former_magnet_release_state != magnet_release_state:
+            if magnet_release_state:   
+                print "Pressed release button" 
+                smart_magnet.set_resting_current()
+                smart_magnet_changed = True
+            else:
+                print "Released release button" 
+                smart_magnet.set_default_current()
+                smart_magnet_changed = False
+        former_magnet_release_state = magnet_release_state
+    
+    else:
+        if smart_magnet_changed:
+            smart_magnet.set_default_current()
+        smart_magnet_changed = False
+        former_magnet_release_state = False
+
+except:
+    former_magnet_release_state = False
+    smart_magnet_changed = False
+        

script/test/CameraCalibration.py

@@ -0,0 +1,71 @@
+import ch.psi.pshell.device.Camera as Camera
+import ch.psi.pshell.imaging.RendererMode as RendererMode
+from ch.psi.pshell.imaging.Overlays import *
+
+"""
+img.camera.setColorMode(Camera.ColorMode.Mono)
+img.camera.setDataType(Camera.DataType.UInt8)
+img.camera.setGrabMode(Camera.GrabMode.Continuous)
+img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
+img.camera.setExposure(50.00)
+img.camera.setAcquirePeriod(200.00)
+img.camera.setGain(0.0)
+img.config.rotationCrop=True
+
+
+"""
+#img.camera.setROI(200, 0,1200,1200)
+
+img.camera.setROI(0, 0,1600,1200)
+img.config.rotation=0
+img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight =0,0,-1,-1
+img.config.setCalibration(None)
+img.camera.stop()
+img.camera.start()
+
+#img.camera.setROI(300, 200,1000,1000)
+#img.config.rotation=17
+#img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = 50,50,900,900
+
+p = show_panel(img)
+p.setMode(RendererMode.Fit)
+ov_text = Text(Pen(java.awt.Color.GREEN.darker()), "", java.awt.Font("Verdana", java.awt.Font.PLAIN, 24), java.awt.Point(20,20))
+ov_text.setFixed(True)
+
+
+
+p.addOverlay(ov_text)
+try:
+    ov_text.update("Click on upper reference...")
+    p1 = p.waitClick(60000)
+    print p1
+    ov_text.update("Click on left reference...")
+    p2 = p.waitClick(60000)
+    print p2
+    ov_text.update("Click on right reference...")
+    p3 = p.waitClick(60000)
+    print p3        
+
+    x, y, z = p1.x+p1.y*1j, p2.x+p2.y*1j, p3.x+p3.y*1j
+    w = z-x
+    w /= y-x
+    c = (x-y)*(w-abs(w)**2)/2j/w.imag-x
+    cx, cy, r =  -c.real, -c.imag, abs(c+x)
+    a = math.degrees(math.atan((cx-p1.x)/(p1.y-cy)))
+
+    print cx, cy, r, a
+    
+    #img.camera.setROI(int((1600-cx)/2),int((1200-cy)/2),1000,1000)
+    img.camera.setROI(int(cx-r),int(cy-r),int(2*r),int(2*r))
+    img.config.rotation=-a
+    #remove rotation border
+    d=int(r/11)
+    img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight =d,d, int(2*r-2*d),  int(2*r-2*d)
+    #img.config.setCalibration(None)
+    img.camera.stop()
+    img.camera.start()  
+    
+finally: 
+    p.removeOverlay(ov_text)
+
+  

script/test/CoverDetection.py

@@ -0,0 +1,116 @@
+################################################################################################### 
+# Procedure to detect the cover orientation
+################################################################################################### 
+import ch.psi.pshell.imaging.Utils.integrateVertically as integrateVertically
+img.backgroundEnabled=False
+
+REF = (0,96,125)
+
+line = load_image("{images}/line.png", title="Line")
+#line = load_image("{images}/line360.png", title="Line")
+
+line.getProcessor().setBackgroundValue(0.0)
+
+
+#ip = get_image()
+ip = integrate_frames(10)
+ip = grayscale(ip, True)
+
+
+smooth(ip)
+#bandpass_filter(ip, 30, 1000)
+edges(ip)
+
+#invert(ip)
+#auto_threshold(ip, method = "Default")
+
+
+#auto_threshold(ip, method = "Li")
+auto_threshold(ip, method = "MaxEntropy")
+
+"""
+for m in AutoThresholder.getMethods():   
+    print m
+    aux = ip.duplicate()
+    auto_threshold(aux, method = m)
+    binary_fill_holes(aux, dark_background=False)
+    renderer = show_panel(aux.bufferedImage)
+    time.sleep(1.0)
+"""
+#binary_dilate(ip, dark_background=False)
+#binary_fill_holes(ip, dark_background=False)
+#binary_open(ip, dark_background=Tr)
+
+renderer = show_panel(ip.bufferedImage)
+
+
+
+
+
+
+
+
+#line = sub_image(line, 325, 325, 512, 512)
+#ip = sub_image(ip, 325, 325, 512, 512)
+line = sub_image(line, 453, 453, 256, 256)
+ip = sub_image(ip, 453, 453, 256, 256)
+#op = op_fft(ip, line, "correlate") 
+
+
+renderer = show_panel(ip.bufferedImage)
+
+#renderer = show_panel(op.bufferedImage)
+#line.show()
+ydata = []
+xdata = range (0,180,1)
+for i in xdata:
+    l = line.duplicate()
+    l.getProcessor().setBackgroundValue(0.0)
+    l.getProcessor().rotate(float(i))
+    op = op_fft(ip, l, "correlate") 
+    bi = op.getBufferedImage()
+    p = integrateVertically(bi)
+    ydata.append(sum(p))
+    
+    #renderer = show_panel(op.bufferedImage)
+    #time.sleep(0.001)
+
+def moving_average(arr, n) :
+    ret = []
+    for i in range(len(arr)):
+        ret.append(mean(arr[max(i-n,0):min(i+n,len(arr)-1)]))
+    return ret    
+av = moving_average(ydata, 1)
+    
+p = plot(ydata, xdata=xdata)[0]
+
+p.addSeries(LinePlotSeries("Moving Average"))
+p.getSeries(1).setData(xdata, av)      
+
+peaks = estimate_peak_indexes(ydata, xdata,  (min(ydata) + max(ydata))/2,  25.0)
+left, right = min(peaks), max(peaks)
+if xdata[left]<5 and xdata[right]>(xdata[-1]-5):
+    #del peaks[0 if ydata[right] >  ydata[left]  else -1]
+    peaks.remove(right if ydata[right] >  ydata[left]  else left)
+
+peaks = sorted(peaks[:3])
+peaks_x = map(lambda x:xdata[x], peaks)
+peaks_y = map(lambda x:ydata[x], peaks)
+
+print "Peaks", peaks        
+print "Peak indexes: " + str(peaks_x)
+print "Peak values:  " + str(peaks_y)
+
+
+for i in range(len(peaks)):
+    peak = xdata[peaks[i]]
+    p.addMarker(peak, None, "N="+str(round(peak,2)), Color(80,0,80))
+    if ((peaks[i]>160) and (REF[i]<20)):
+        peaks[i] = peaks[i] - 180.0
+
+print "Peaks x: " + str(peaks_x)
+
+
+d = mean(arrabs(arrsub(REF, peaks_x)))
+
+print "Angle = ", d

script/test/PuckDetection.py

@@ -0,0 +1,24 @@
+################################################################################################### 
+# Example of using ImageJ functionalities through ijutils.
+################################################################################################### 
+
+from ijutils import *
+import java.awt.Color as Color
+
+
+#Image Loading
+ip = load_image("{images}/test2.png", title="Image")
+
+
+#Puck Detection
+aux = grayscale(ip, in_place=False)
+aux.show() 
+plot(get_histogram(aux))
+
+subtract_background(aux)
+threshold(aux,0,50); aux.repaintWindow()
+binary_fill_holes(aux); aux.repaintWindow()
+(results,output_img)=analyse_particles(aux, 10000,50000, 
+            fill_holes = False, exclude_edges = True,print_table=True,
+            output_image = "outlines", minCirc = 0.0, maxCirc = 1.0)
+output_img.show()

script/test/PuckDetectionTest.py

@@ -0,0 +1,11 @@
+import datetime
+index = 0
+prefix =  str(datetime.datetime.now().strftime("%Y%m%d_%H%M%S"))
+
+while True:
+    suffix = "%04d - " % index
+    print (suffix), 
+    run("imgproc/LedDetectionProc.py")
+    index=index+1
+    save_image(image,  "{images}/" +prefix + "_" + suffix +".png", "png")        
+    

script/test/RobotCartesianScan.py

@@ -0,0 +1,28 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+robot.enable()
+move_to_laser()
+
+#robot.reset_motion("jLaser")
+
+#import ch.psi.pshell.device.Timestamp as Timestamp
+#sensor = Timestamp()
+
+#ret = lscan(robot_z, sensor, -50.0, 0.0, 0.5, latency = 0, relative = True)
+#ret = lscan(robot_x, sensor, -50.0, 0.0, 0.5, latency = 0, relative = True)
+
+robot.set_motors_enabled(True)
+ret = lscan(robot_x, ue.readable, -1.5, 1.5, 0.1, latency = 0.01, relative = True)
+
+x,y = ret.getPositions(0), ret.getReadable(0)
+y = [100.0-v for v in y]
+(norm, mea, sigma) = fit(y, xdata=x)
+print "Maximum at " + str(mea)
+
+
+
+robot.set_motors_enabled(True)
+ret = lscan(robot_rz, ue.readable, 0.0, 40.0, 1.0, latency = 0.01, relative = True, range = "auto")
+
+

script/test/SampleDetection.py

@@ -0,0 +1,23 @@
+################################################################################################### 
+# Example of using ImageJ functionalities through ijutils.
+################################################################################################### 
+
+from ijutils import *
+import java.awt.Color as Color
+
+#Image Loading
+ip = load_image("{images}/test2.png", title="Image")
+
+aux = grayscale(ip, in_place=False)
+aux.show() 
+
+invert(aux); aux.repaintWindow()
+#gaussian_blur(aux); aux.repaintWindow()
+subtract_background(aux); aux.repaintWindow()
+auto_threshold(aux); aux.repaintWindow()
+binary_open(aux); aux.repaintWindow()
+#binary_fill_holes(aux); aux.repaintWindow()
+(results,output_img)=analyse_particles(aux, 250,1000, 
+            fill_holes = False, exclude_edges = True,print_table=True,
+            output_image = "outlines", minCirc = 0.7, maxCirc = 1.0)
+output_img.show()

script/test/TestAlign.py

@@ -0,0 +1,9 @@
+
+v = 2.0
+#while(true):
+for  i in range(100):
+    v = v * (-1.0)
+    robot_j4.initialize()
+    robot_j4.moveRel(v)    
+    robot.align()  
+    

script/test/TestBugPcAPI

@@ -0,0 +1,4 @@
+while True:
+    img.initialize()
+    img.waitNext(10000)
+    time.sleep(2.0)

script/test/TestBugPcAPI.py

@@ -0,0 +1,30 @@
+import ch.psi.pshell.prosilica.Prosilica as Prosilica
+import ch.psi.pshell.device.Camera as Camera
+
+
+add_device(Prosilica("img", 25001, "PacketSize=1522;PixelFormat=Mono8;BinningX=1;BinningY=1;RegionX=300;RegionY=200;Width=1000;Height=1000;MulticastEnable=Off"), True)
+
+img.camera.setGrabMode(Camera.GrabMode.Continuous)
+img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
+img.camera.setExposure(50.00)
+img.camera.setAcquirePeriod(200.00)
+img.camera.setGain(0.0)
+#img.camera.setROI(200, 0,1200,1200)
+"""
+img.camera.setROI(300, 200,1000,1000)    
+img.config.rotation=17
+img.config.rotationCrop=True
+img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = 50,50,900,900
+"""
+img.camera.setROI(int(get_setting("roi_x")), int(get_setting("roi_y")), int(get_setting("roi_w")), int(get_setting("roi_h")))  
+
+img.camera.stop()
+img.camera.start()
+
+
+show_panel(img)
+#while True:
+#    img.initialize()
+#    img.waitNext(1000)
+#    time.sleep(0.1)
+

script/test/TestBugPcAPI2.py

@@ -0,0 +1,6 @@
+import ch.psi.pshell.imaging.MjpegSource as MjpegSource
+MjpegSource2 = get_context().pluginManager.getDynamicClass("MjpegSource2")
+add_device(MjpegSource("gripper_cam", "http://axis-accc8e9cc87b.psi.ch/axis-cgi/mjpg/video.cgi"), True)
+#gripper_cam.polling=1000
+gripper_cam.monitored = True
+show_panel(gripper_cam)

script/test/TestBugPcAPI3.py

@@ -0,0 +1,4 @@
+while True:
+    img.initialize()    
+    img.waitNext(1000)        
+    time.sleep(0.1)

script/test/TestCalib.py

@@ -0,0 +1,30 @@
+
+print "calibrate_tool"
+
+#Initial checks
+robot.assert_no_task()
+robot.reset_motion()
+robot.wait_ready()
+robot.assert_cleared()
+#robot.assert_in_known_point()
+
+#Enabling
+enable_motion()
+
+(detected, dm) = move_scanner()
+
+if detected:
+    print "Pin detected, trashing..."
+    trash()
+    (detected, dm) = move_scanner()
+    if detected:
+        raise Exception("Cannot trash pin")
+
+robot.open_tool()        
+robot.get_calibration_tool()
+
+run("calibration/ToolCalibration3")
+
+robot.put_calibration_tool()
+
+robot.save_program()

script/test/TestCameraStability1.py

@@ -0,0 +1,11 @@
+for i in range(10):
+    time.sleep(2)
+    move_home()
+    time.sleep(1)
+    move_park()
+    start = time.time()
+    while get_img_cover_pos() != 'A':
+        print ".",
+        time.sleep(0.001)
+    print "Time = " , ( time.time() - start)
+    

script/test/TestCameraStability2.py

@@ -0,0 +1,14 @@
+cover_detection_debug=True
+while True:
+    for pos in ['A', 'B', 'C', 'D', 'E', 'F']:
+        print "Moving to ", pos
+        hexiposi.move(pos)
+        move_home()
+        move_park()
+        time.sleep(2.0)
+        ret =  run("imgproc/CoverDetection")
+        det =  ret[0]
+        print "Detected: ", det
+        if det != pos:
+            raise Exception("Position error")
+

script/test/TestCameraStability3.py

@@ -0,0 +1,9 @@
+for i in range(10):
+    time.sleep(1)
+    move_home()
+    time.sleep(1)
+    move_park()
+    start = time.time()
+    time.sleep(1.0)
+    print run("imgproc/LedDetectionProc.py")
+    

script/test/TestCmdSynchronization.py

@@ -0,0 +1,32 @@
+import java.util.logging.Level
+
+def get_pos_str():
+    return "point: " + str(robot.get_current_point()) + " - here: " + str(robot.here()) + " - herej: " + str(robot.herej())
+
+enable_motion()
+
+get_context().setLogLevel(java.util.logging.Level.FINER)
+try:
+    while True:
+    #robot.move_dewar()
+    #robot.move_park()
+
+        log("Moving dewar", False)
+        flag = robot.start_task('moveDewar')  
+        print "moveDewar: ", flag        
+        log("Waiting", False)
+        ret = robot.wait_task_finished(TASK_WAIT_ROBOT_POLLING)                   
+        log("Moving dewar finished  (" + str(ret) + ") - pos: " + get_pos_str(), False)        
+        robot.assert_dewar()
+
+
+        log("Moving park", False)
+        flag = robot.start_task('movePark')  
+        print "movePark: ", flag
+        log("Waiting", False)
+        ret = robot.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        log("Moving park finished  (" + str(ret) + ") - pos: " + get_pos_str(), False)        
+        robot.assert_park()    
+
+finally:
+    get_context().setLogLevel(java.util.logging.Level.INFO)

script/test/TestCoverDetection.py

@@ -0,0 +1,19 @@
+
+ca = []
+aa = []
+pa = []
+
+#for i in range(6):
+ #index = i+1
+for i in ['A', 'B', 'C', 'D', 'E', 'F']: 
+     hexiposi.move(i)
+     [position, angle, confidence] = run("imgproc/CoverDetection") 
+     print [position, angle, confidence]
+     pa.append(position)
+     aa.append(angle)
+     ca.append(confidence)
+
+print "---"
+print "Position: " ,pa
+print "Angle: " ,aa
+print "Confidence: " ,ca, " mean: ", mean(ca)

script/test/TestEuclidean.py

@@ -0,0 +1,23 @@
+import org.apache.commons.math3.geometry.euclidean.threed.Segment as Segment
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D as Vector3D
+import org.apache.commons.math3.geometry.euclidean.threed.Line as Line
+
+
+p = Vector3D(0, 0, 3)
+
+p1 = Vector3D(0,0,0)
+p2 = Vector3D(0,1,1 )
+tolerance = 0.001
+
+l = Line(p1, p2, tolerance)
+
+
+print l.distance(p)
+print p1.distance(p)
+print p2.distance(p)
+print "---"
+
+print l.getAbscissa(p)
+print l.pointAt(l.getAbscissa(p))
+
+#print l.closestPoint(Line(p, p, 0.01))

script/test/TestLaserScan.py

@@ -0,0 +1,12 @@
+robot.set_motors_enabled(True)
+steps_x = 10
+steps_y = 8
+#ret = ascan([robot_x, robot_z], ue.readable, [-10.0, -10.0], [10.0, 10.0], [10,10], latency = 0.01, relative = True , zigzag=True)
+ret = ascan([robot_x, robot_z], ue.readable, [-10.0, -10.0], [10.0, 10.0], [steps_y,steps_x], latency = 0.01, relative = True , zigzag=False)
+data = Convert.transpose(Convert.toBidimensional(to_array(ret.getReadable(0),'d'),steps_x+1,steps_y+1))
+plot(data, title="data")
+
+data = ret.getData(0)
+plot(data, title="data2")
+        
+

script/test/TestLayout.py

@@ -0,0 +1,21 @@
+
+
+
+points = [(100, 140), (130, 77), (120, 130), (110, 100)]
+clear_detection()
+detect_pucks(points)
+#detect_pucks(points, 'A')
+#detect_pucks(points, '4')
+plot_base_plate(points)    
+
+
+
+#plot_detected_leds(points)    
+
+
+
+block_id = None
+print get_unipucks(block_id)
+print get_minispines(block_id)
+print  get_empties(block_id)
+print  get_det_errors(block_id)

script/test/TestMicrohawk.py

@@ -0,0 +1,10 @@
+import ch.psi.pshell.serial.TcpDevice as TcpDevice
+import ch.psi.pshell.serial.UdpDevice as UdpDevice
+
+
+microscan = TcpDevice("microscan", "MicroHAWK38C528:2001")
+#microscan = UdpDevice("microscan", "MicroHAWK38C528:2001")
+microscan.initialize()
+
+
+

script/test/TestRecover.py

@@ -0,0 +1,15 @@
+print "Pos=" + str(robot.get_cartesian_pos())
+for p in robot.get_known_points():
+    print p + " = " + str(get_pnt(p))
+
+print "-------------"
+
+for segment in known_segments:
+    is_on_segment(segment)
+
+print "-------------"
+for segment in known_segments:
+    try:
+        move_to_segment(segment)
+    except:
+        print sys.exc_info()[1]

script/test/TestRelays.py

@@ -0,0 +1,12 @@
+for i in range(500):
+    relay1.write(True)
+    time.sleep(1.2)
+    relay1.write(False)
+    time.sleep(1.2)
+"""
+for i in range(5):
+    relays.write(to_array([True,]*16, 'z'))    
+    time.sleep(0.2)
+    relays.write(to_array([False,]*16, 'z'))    
+    time.sleep(0.2)
+"""

script/test/TestRemoveBackground.py

@@ -0,0 +1,31 @@
+number_frames = 5
+number_backgrounds = 5
+minimum_size = 78     # r = 5 # 150   
+maximum_size = 750    # r = 15 #1500
+min_circ = 0.2
+
+threshold_method = "MaxEntropy"
+threshold_method,threshold_range = "Manual",  (0, 215)
+
+exclude_edges = True
+led_latency = 0.5 #0.1
+
+set_led_state(False)
+time.sleep(led_latency)
+img.waitNext(2000)
+
+background = average_frames(number_backgrounds)
+#background = integrate_frames(number_backgrounds)
+
+set_led_state(True)
+time.sleep(led_latency)
+img.waitNext(2000)
+image = average_frames(number_frames)
+#image = integrate_frames(number_frames)
+
+set_led_state(False)
+
+op_image(image, background, "subtract", float_result=True, in_place=True)
+image=grayscale(image)
+
+show_panel(image.getBufferedImage())