script/motion/tools.py

@@ -1,10 +1,21 @@
 POSITION_TOLERANCE = 50
 
-def enable_motion():
+
+def is_manual_mode():
     """
-    Check safety and enable arm power if in remote 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():
     """
-    auto = robot.working_mode != "manual"
+    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()  
@@ -18,6 +29,12 @@ def enable_motion():
         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()    
+    auto = not is_manual_mode()        
     if auto:
         if not robot.state.isNormal():
             raise Exception("Cannot enable power: robot state is " + str(robot.state))
@@ -31,7 +48,7 @@ def set_hexiposi(pos, force = False):
         if hexiposi.position == pos:
             return 
         
-    if robot.working_mode == "manual":
+    if is_manual_mode():
         set_status("Move Hexiposi to position " + str(pos) + " ...")
         try:
             hexiposi.waitInPosition(pos, -1)
@@ -48,7 +65,7 @@ def _set_hexiposi(pos):
             
 
 def visual_check_hexiposi(segment):
-    #if robot.working_mode == "manual" ?
+    #if is_manual_mode(): ?
     if hexiposi.moved:
         #Clearing for image processing
         if not robot.is_park():