script/devices/RobotTCP.py

@@ -40,6 +40,7 @@ class RobotTCP(TcpDevice, Stoppable):
          self.joint_motors = []
          self.tool = None
          self.default_desc = None
+         self.tool_open = None
          #self.tool_trsf = [0.0] * 6
          self.frame = FRAME_DEFAULT
          self.polling_interval = 0.01
@@ -52,7 +53,7 @@ class RobotTCP(TcpDevice, Stoppable):
 
     def doInitialize(self):           
         super(RobotTCP, self).doInitialize()
-        self.reset = True      
+        self.reset = True        
 
     def set_tool(self, tool):
         self.tool = tool
@@ -60,7 +61,8 @@ class RobotTCP(TcpDevice, Stoppable):
         self.evaluate("tcp_curtool=" + tool)         
         if self.cartesian_motors_enabled:            
             self.update()
-            self.set_motors_enabled(True)            
+            self.set_motors_enabled(True)       
+        self.is_tool_open()     
     
     def get_tool(self):    
         return self.tool
@@ -395,18 +397,36 @@ class RobotTCP(TcpDevice, Stoppable):
         #self.waitState(State.Ready, -1)
             
 
-    #Tool
-    #This function can timeout since it synchronizes move. 
-    #Better check state before otherwise it can freeze the communication
+    #Tool - synchronized as can freeze communication
+    """
+    def open_tool(self, tool=None, timeout=3000):
+        #This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
+        self.waitState(State.Ready, -1)
+        if tool is None: tool = self.tool
+        return self.evaluate("open(" + tool + " )", timeout=timeout)
+
+    def close_tool(self, tool=None, timeout=3000):
+        #This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
+        self.waitState(State.Ready, -1)
+        if tool is None: tool = self.tool
+        return self.evaluate("close(" + tool + " )", timeout=timeout)
+    """
+    #Tool - Not synchronized calls: atention to open/close only when state is Ready
     def open_tool(self, tool=None):
         if tool is None: tool = self.tool
-        return self.evaluate("open(" + tool + " )", timeout=3000)
+        self.evaluate(tool + ".gripper=true")
+        self.tool_open = True
 
-    #This function can timeout since it synchronizes move. Better check state before
-    #Better check state before otherwise it can freeze the communication
     def close_tool(self, tool=None):
         if tool is None: tool = self.tool
-        return self.evaluate("close(" + tool + " )", timeout=3000)
+        self.evaluate(tool + ".gripper=false")
+        self.tool_open = False
+
+    def is_tool_open(self, tool=None):
+        if tool is None: tool = self.tool
+        self.tool_open =  robot.eval_bool(tool + ".gripper")        
+        return self.tool_open
+
 
     #Arm position
     def herej(self):
@@ -521,6 +541,7 @@ class RobotTCP(TcpDevice, Stoppable):
             self.speed = int(sts[3])
             self.empty = sts[4] == "1"
             self.settled = sts[5] == "1"
+            #TODO: add tool open
             if cur_task is not None:                                    
                 if int(sts[6]) < 0:         
                     log("Task "+ str(cur_task) + " finished with code: " + str(sts[7]), False)
@@ -547,7 +568,8 @@ class RobotTCP(TcpDevice, Stoppable):
                            "settled": self.settled,
                            "task": cur_task,
                            "mode": self.working_mode,
-                           "status": self.status
+                           "status": self.status,
+                           "open": self.tool_open
                            }, None)
             if self.cartesian_motors_enabled:
                 for m in self.cartesian_motors:

script/local.py

@@ -4,6 +4,7 @@
 import traceback
 from ch.psi.pshell.serial import TcpDevice
 from ch.psi.pshell.modbus import ModbusTCP
+import ch.psi.mxsc.Controller as Controller
 
 
 run("setup/Layout")
@@ -60,6 +61,16 @@ def system_check(robot_move=True):
         if not guiding_tool_park().read():
             raise Exception("Guiding tool not parked")
         
+def get_puck_elect_detection(segment, puck):
+    return str(Controller.getInstance().getPuck(str(segment).upper() + str(puck)).detection)
+
+def get_puck_img_detection(segment, puck):
+    return str(Controller.getInstance().getPuck(str(segment).upper() + str(puck)).imageDetection)
+
+def assert_puck_detected(segment, puck):
+    if get_puck_detection(segment, puck) != "Present":
+        raise Exception ("Puck " + str(segment).upper() + str(puck) + " not present")
+
 
 def start_puck_detection():
     run("tools/RestartPuckDetection")

script/motion/get_dewar.py

@@ -5,6 +5,8 @@ def get_dewar(segment, puck, sample, force=False):
     
     #Initial checks
     assertValidAddress(segment, puck, sample)
+    assert_puck_detected(segment, puck)
+    
     robot.assert_no_task()
     robot.reset_motion()
     robot.wait_ready()

script/motion/mount.py

@@ -5,6 +5,8 @@ def mount(segment, puck, sample, force=False, read_dm=False):
     start = time.time()
     #Initial checks
     assertValidAddress(segment, puck, sample)
+    assert_puck_detected(segment, puck)
+    
     robot.assert_no_task()
     robot.reset_motion()
     robot.wait_ready()

script/motion/put_dewar.py

@@ -5,6 +5,8 @@ def put_dewar(segment, puck, sample, force=False):
     
     #Initial checks
     assertValidAddress(segment, puck, sample)
+    assert_puck_detected(segment, puck)
+    
     robot.assert_no_task()
     robot.reset_motion()
     robot.wait_ready()

script/motion/recover.py

@@ -0,0 +1,5 @@
+def recover():
+    """
+    """
+    print "recover"
+    

script/motion/robot_recover.py

@@ -11,6 +11,4 @@ def robot_recover():
     #Enabling
     enable_motion()    
 
-    #barcode_reader.start_read(20.0)
-
     robot.robot_recover()

script/motion/unmount.py

@@ -5,6 +5,8 @@ def unmount(segment, puck, sample, force=False):
     
     #Initial checks
     assertValidAddress(segment, puck, sample)
+    assert_puck_detected(segment, puck)
+    
     robot.assert_no_task()
     robot.reset_motion()
     robot.wait_ready()

script/test/TestBugPcAPI2.py

@@ -1,6 +1,6 @@
 import ch.psi.pshell.imaging.MjpegSource as MjpegSource
 MjpegSource2 = get_context().pluginManager.getDynamicClass("MjpegSource2")
-add_device(MjpegSource2("gripper_cam", "http://129.129.110.114/axis-cgi/mjpg/video.cgi"), True)
-gripper_cam.polling=100
-#gripper_cam.monitored = True
+add_device(MjpegSource("gripper_cam", "http://129.129.110.114/axis-cgi/mjpg/video.cgi"), True)
+#gripper_cam.polling=1000
+gripper_cam.monitored = True
 show_panel(gripper_cam)