script/motion/tools.py

@@ -1,5 +1,23 @@
 POSITION_TOLERANCE = 50
 
+def enable_power():
+    """
+    Check safety and enable arm power
+    """
+    release_psys()  
+    time.sleep(0.1)   
+    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")
+    time.sleep(0.25)    
+    if not robot.state.isNormal():
+        raise Exception("Cannot enable power: robot state is " + str(robot.state))
+    robot.enable()
+    
+
+
 def wait_end_of_move():
     robot.update()
     while (not robot.settled) or (not robot.empty) or (not robot.isReady()) :