script/motion/tools.py

@@ -4,28 +4,33 @@ def enable_motion():
     """
     Check safety and enable arm power if in remote mode
     """
-    if robot.working_mode != "manual":
-        release_psys()  
-        time.sleep(0.1)   
+    auto = robot.working_mode != "manual"
+    if auto:
         if feedback_psys_safety.read() == False:
-            raise Exception("Cannot enable power: check doors")
-            
-    release_local()
-    if feedback_local_safety.read() == False:
-        raise Exception("Cannot enable power: check sample changer emergency stop button")
+            release_psys()  
+            time.sleep(0.5)   
+            if feedback_psys_safety.read() == False:
+                raise Exception("Cannot enable power: check doors")
 
-    if robot.working_mode == "manual":
-        pass
-    else:
-        time.sleep(0.25)    
+    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") 
+            
+    if auto:
         if not robot.state.isNormal():
             raise Exception("Cannot enable power: robot state is " + str(robot.state))
         robot.enable()
 
-def set_hexiposi(pos):
+def set_hexiposi(pos, force = False):
     """
-    Set the hexiposi posiiton in remote mode, or wait for it to be set in manual mode
+    Set the hexiposi position in remote mode, or wait for it to be set in manual mode
     """
+    if force == False:
+        if hexiposi.position == pos:
+            return 
+        
     if robot.working_mode == "manual":
         set_status("Move Hexiposi to position " + str(pos) + " ...")
         try: