script/calibration/ScanX.py

@@ -0,0 +1,81 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+
+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 = [-2.0, 2.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)
+
+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]]
+#x = enforce_monotonic(x)
+#(normalization, mean_val, sigma) = fit_gaussian([-v for v in y], x)
+
+
+closest_x = x[y.indexOf(min(y))]
+closest_y = y[y.indexOf(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_y, p.AxisId.X, str(closest_y), Color.GREEN)
+
+robot.set_motors_enabled(True)
+robot_x.move(closest_x)

script/calibration/ScanY.py

@@ -37,14 +37,14 @@ for i in range(len(d)):
         last_index = i  
 
 if first_index == -1 or last_index < first_index:
-    raise Exception("Invalid X detection")
+    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))
+    raise Exception("Invalid range: " + str(_range))
 
 
 center_index = int((_range[0] + _range[1])/2)

script/calibration/ToolCalibration3.py

@@ -0,0 +1,65 @@
+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()  
+
+run("calibration/ScanX")
+
+pos1 = robot.get_cartesian_pos()
+x1, l1 = closest_x, closest_y
+
+print "Closest 1: ", [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")
+
+
+x2, l2 = closest_x, closest_y
+pos2 = robot.get_cartesian_pos()
+
+print "Closest 2: ", [x2, l2]
+print "Position 2: ", pos2
+
+
+#Updates the tool
+t=robot.get_tool_trsf(TOOL_DEFAULT)
+
+xoff = (x1-x2)/2
+yoff = (l2 - l1)/2 
+xrot = math.degrees(math.atan(yoff/t[2]))
+yrot = math.degrees(math.atan(xoff/t[2]))
+
+print "Former tool: " + str(t)
+t[0]=xoff 
+t[1]=-yoff 
+print "Offset: ", [t[0], t[1]]
+
+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/data/samples.py

@@ -134,7 +134,6 @@ def update_puck_table():
 #Sample mount/unmount
 
 def update_samples_info_sample_mount(puck_address, sample_position, sample_detected, sample_id):
-    print ""
     try:
         if (samples_info is not None) and (puck_address is not None):
             for si in samples_info:
@@ -169,6 +168,24 @@ def update_samples_info_sample_unmount(puck_address, sample_position):
     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 str(si["puckAddress"]) == str(puck_address) and str(si["samplePosition"]) == str(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 = [    \

script/devices/LaserDistance.py

@@ -14,4 +14,4 @@ listenerAI = ListenerAI()
 ue.addListener(listenerAI)
         
 laser_distance=LaserDistance()   
-add_device(laser_distance, True)
+add_device(laser_distance, True)

script/devices/RobotSC.py

@@ -57,7 +57,8 @@ class RobotSC(RobotTCP):
 
     def put_gonio(self):    
         pin_offset = get_pin_offset()
-        self.start_task('putGonio', pin_offset) 
+        pin_angle_offset = get_pin_angle_offset()
+        self.start_task('putGonio', pin_offset, pin_angle_offset) 
         self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
         self.assert_gonio()
 

script/devices/Wago.py

@@ -124,9 +124,7 @@ def release_psys():
 ###################################################################################################
 #  Drier
 ###################################################################################################
-
-MAX_HEATER_TIME = 45000
-
+MAX_HEATER_TIME = 60000
 
 def set_air_stream(state):
     """
@@ -138,6 +136,16 @@ def set_air_stream(state):
 set_heater_chrono = None
 
 def monitor_heater_time():
+    """
+    """
+    return valve_1.read()
+
+
+def get_heater():
+    """
+    """
+    return gripper_dryer.read()
+
     time.sleep(0.5)
     try:
         while get_heater():
@@ -165,8 +173,3 @@ def get_air_stream():
     """
     return valve_1.read()
 
-
-def get_heater():
-    """
-    """
-    return gripper_dryer.read()

script/local.py

@@ -95,6 +95,7 @@ run("motion/move_cold")
 run("motion/move_scanner")
 run("motion/dry")
 run("motion/homing_hexiposi")
+run("motion/scan_pin")
 run("motion/robot_recover")
 run("motion/recover")
 run("imgproc/Utils")
@@ -240,7 +241,7 @@ def is_puck_loading():
 
 def set_pin_offset(val):
     if abs(val) >5:
-        raise Exception("Invlid pin offset: " + str(val)) 
+        raise Exception("Invalid pin offset: " + str(val)) 
     try:
         set_setting("pin_offset",float(val))
     except:
@@ -250,12 +251,31 @@ def get_pin_offset():
     try:
         ret =  float(get_setting("pin_offset"))
         if abs(ret) >5:
-            raise Exception("Invlid configured pin offset: " + str(ret))         
+            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"))

script/motion/dry.py

@@ -1,6 +1,6 @@
 
 
-def dry(heat_time=30.0, speed=0.5, wait_cold = 30.0):    
+def dry(heat_time=30.0, speed=0.4, wait_cold = 30.0):    
     """
         heat_time (float): in seconds
         speed (float): % of nominal speed
@@ -9,7 +9,7 @@ def dry(heat_time=30.0, speed=0.5, wait_cold = 30.0):
     """
     print "dry"
     
-    #Initial checks
+    #Initial chec
     robot.assert_no_task()
     robot.reset_motion()
     robot.wait_ready()
@@ -40,4 +40,4 @@ def dry(heat_time=30.0, speed=0.5, wait_cold = 30.0):
         robot.move_cold()  
         time.sleep(wait_cold)
     else:
-        robot.move_dewar()
+        robot.move_park()

script/motion/mount.py

@@ -56,7 +56,7 @@ def mount(segment, puck, sample, force=False, read_dm=False):
         if read_dm:
             barcode_reader.start_read(10.0)
             robot.move_scanner()     
-            time.sleep(1.0)           
+            #time.sleep(1.0)           
             
         robot.move_gonio()    
         

script/motion/move_scanner.py

@@ -1,3 +1,4 @@
+
 def move_scanner():
     """
     """
@@ -13,14 +14,17 @@ def move_scanner():
     #Enabling
     enable_motion()    
 
-    #barcode_reader.start_read(20.0)
-
+    barcode_reader.start_read(10.0)
     if not robot.is_scanner():
         robot.move_scanner()
 
-    #robot.do_scan()
-    #dm = barcode_reader.get_readout()
-    dm = barcode_reader.read(0.5)
+    time.sleep(0.25)
+    dm = barcode_reader.get_readout()    
+    if dm is None:
+        detected =  is_pin_detected_in_scanner()
+    else: 
+        detected = True
 
-    print "Datamatrix: " , dm
-    #print "Datamatrix: " , barcode_reader.wait_readout()
+    print ("Detected: " + str( detected) +  " - Datamatrix: " + str(dm))
+
+    return (detected, dm)

script/motion/scan_pin.py

@@ -1,48 +1,42 @@
-segment, puck, sample, force = "B",1,1, True
-pin_name = get_sample_name(segment, puck, sample)
+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)
+    
+    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()    
+    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)      
+    
+    robot.move_dewar()        
+    robot.put_dewar(segment, puck, sample)
 
+    return (detected, dm)
 
-print "scan pin", pin_name
-#Initial checks
-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()    
-set_hexiposi(segment)
-if not force:        
-    visual_check_hexiposi(segment)           
-
-
-robot.move_park()
-
-gripper_cam.update()
-background_img = gripper_cam.getImage()
-
-
-if not robot.is_dewar():
-    robot.move_dewar()      
-        
-robot.get_dewar(segment, puck, sample)
-
-robot.move_park()
-gripper_cam.update()
-sample_img = gripper_cam.getImage()
-
-barcode_reader.start_read(10.0)
-robot.move_scanner()     
-time.sleep(1.0)           
-
-mount_sample_id = barcode_reader.get_readout()
-print "Datamatrix: " , mount_sample_id
-
-robot.move_dewar()        
-robot.put_dewar(segment, puck, sample)
-
-        
+    
+def scan_puck(segment, puck, force=False):
+    ret = []
+    for i in range(16):
+        ret.append(scan_pin (segment, puck, i+1, force))
+    return ret

script/motion/tools.py

@@ -155,4 +155,16 @@ 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