Startup

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)