script/devices/RobotSC.py

@@ -9,6 +9,7 @@ DESC_DEFAULT = DESC_FAST
 
 
 DEFAULT_ROBOT_POLLING = 500
+TASK_WAIT_ROBOT_POLLING = 50
 
 
 run("devices/RobotTCP")
@@ -22,44 +23,45 @@ joint_forces = False
 class RobotSC(RobotTCP):    
     def __init__(self, name, server, timeout = 1000, retries = 1):
         RobotTCP.__init__(self, name, server, timeout, retries)   
+        self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "movePark"])
         self.setPolling(DEFAULT_ROBOT_POLLING)      
         
     def get_dewar(self, segment, puck, sample):
         segment = self.toSegmentNumber(segment)
         self.assert_dewar()
         self.start_task('getDewar',segment, puck, sample) 
-        self.wait_task_finished()    
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
         self.assert_dewar()
 
     def put_dewar(self, segment, puck, sample):
         segment = self.toSegmentNumber(segment)
         self.assert_dewar()
         self.start_task('putDewar',segment, puck, sample) 
-        self.wait_task_finished()    
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
         self.assert_dewar()
 
     def put_gonio(self):    
         self.start_task('putGonio',segment, puck, sample) 
-        self.wait_task_finished()    
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
         #TODO
 
     def get_gonio(self):
         self.start_task('getGonio') 
-        self.wait_task_finished()    
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
         #TODO
 
     def robot_recover(self):
         self.start_task('robotRecover')
-        self.wait_task_finished()    
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
 
-    def move_dewar(self):
+    def move_dewar_wait(self):
         self.start_task('moveDewar')  
-        self.wait_task_finished()    
-        self.assert_dewar()
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        self.assert_dewar_wait()
 
     def move_park(self):
         self.start_task('movePark')   
-        self.wait_task_finished()    
+        self.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
         self.assert_park()
 
     def toSegmentNumber(self, segment):
@@ -86,11 +88,12 @@ class RobotSC(RobotTCP):
 
     def start_task(self, program, *args,  **kwargs):
         #TODO: Check safe position
-        RobotTCP.start_task(self, program, *args,  **kwargs)
+        return RobotTCP.start_task(self, program, *args,  **kwargs)
 
     def stop_task(self):
         RobotTCP.stop_task(self)
-        #TODO: Restore safe position            
+        #TODO: Restore safe position          
+          
 
     def set_remote_mode(self):
         robot.set_profile("remote")
@@ -104,10 +107,10 @@ class RobotSC(RobotTCP):
     def is_park(self):
         return  self.is_in_point("pPark")   
     
-    def is_dewarHome(self):
+    def is_dewar_home(self):
         return  self.is_in_point("pDewarHome")
 
-    def is_dewarWait(self):
+    def is_dewar_wait(self):
         return  self.is_in_point("pDewarWait")   
 
     def is_gonio(self):
@@ -122,10 +125,10 @@ class RobotSC(RobotTCP):
     def assert_park(self):
         self.assert_in_point("pPark")   
     
-    def assert_dewarHome(self):
+    def assert_dewar_home(self):
         self.assert_in_point("pDewarHome")
 
-    def assert_dewarWait(self):
+    def assert_dewar_wait(self):
         self.assert_in_point("pDewarWait")   
 
     def assert_gonio(self):
@@ -149,7 +152,7 @@ else:
     add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) 
 
 #robot.set_monitor_speed(20)
-
+robot.set_default_desc(DESC_DEFAULT)
 
 
 class jf1(ReadonlyRegisterBase):

script/devices/RobotTCP.py

@@ -36,31 +36,44 @@ class RobotTCP(TcpDevice, Stoppable):
          self.joint_motors_enabled = False
          self.joint_motors = []
          self.tool = None
+         self.default_desc = None
          #self.tool_trsf = [0.0] * 6
          self.frame = FRAME_DEFAULT
-                
+         self.polling_interval = 0.01
+         self.reset = True                    
 
     def doInitialize(self):           
         super(RobotTCP, self).doInitialize()
         self.reset = True      
 
-
-    def set_tool(self,tool):
+    def set_tool(self, tool):
         self.tool = tool
         #self.tool_trsf = self.get_tool_trsf()
         self.evaluate("tcp_curtool=" + tool)         
         if self.cartesian_motors_enabled:            
             self.update()
-            self.set_motors_enabled(True)        
-    
+            self.set_motors_enabled(True)            
     
     def get_tool(self):    
         return self.tool
 
+
     def assert_tool(self, tool):
         if self.tool != tool:
             raise Exception("Invalid tool: " + self.tool)        
-         
+
+    def set_default_desc(self,default_desc):
+        self.default_desc=default_desc
+    
+    def get_default_desc(self):    
+        return self.default_desc
+
+    def set_tasks(self,tasks):
+        self.high_level_tasks=tasks
+    
+    def get_tasks(self):    
+        return self.high_level_tasks
+
     def _sendReceive(self, msg_id, msg = "", timeout =  None):  
         tx = self.header if (self.header != None) else  ""
         tx = tx + msg_id + " " + msg        
@@ -318,19 +331,25 @@ class RobotTCP(TcpDevice, Stoppable):
             self.joint_forces = None
             raise
 
-    def movej(self, joint_or_point, tool, desc, sync=False):
+    def movej(self, joint_or_point, tool=None, desc=None, sync=False):
+        if desc is None: desc = self.default_desc
+        if tool is None: tool = self.tool
         ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
         if sync:
             self.wait_end_of_move()        
         return ret
         
-    def movel(self, point, tool, desc, sync=False):
+    def movel(self, point, tool=None, desc=None, sync=False):
+        if desc is None: desc = self.default_desc
+        if tool is None: tool = self.tool
         ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
         if sync:
             self.wait_end_of_move()        
         return ret
 
-    def movec(self, point_interm, point_target, tool, desc, sync=False):
+    def movec(self, point_interm, point_target, tool=None, desc=None, sync=False):
+        if desc is None: desc = self.default_desc
+        if tool is None: tool = self.tool
         ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")
         if sync:
             self.wait_end_of_move()        
@@ -371,17 +390,20 @@ class RobotTCP(TcpDevice, Stoppable):
     def compose(self, pnt, frame = FRAME_DEFAULT, trsf = "tcp_t"):    
         return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")
 
-    def here(self, tool, frame):
+    def here(self, tool=None, frame=FRAME_DEFAULT):
+        if tool is None: tool = self.tool    
         return self.eval_pnt("here(" + tool + ", " + frame + ")")
 
-    def joint_to_point(self, tool, frame, joint="tcp_j"):
+    def joint_to_point(self, tool=None, frame=FRAME_DEFAULT, joint="tcp_j"):
+        if tool is None: tool = self.tool    
         return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")
 
-    def point_to_joint(self, tool, initial_joint="tcp_j", point="tcp_p"):
+    def point_to_joint(self, tool=None, initial_joint="tcp_j", point="tcp_p"):
+        if tool is None: tool = self.tool    
         if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"):
             return self.get_jnt()
 
-    def position(self, point, frame):
+    def position(self, point, frame=FRAME_DEFAULT):
         return self.eval_trf("position(" + point + ", " + frame + ")")
     
     #Profile
@@ -434,9 +456,9 @@ class RobotTCP(TcpDevice, Stoppable):
     def _update_state(self):
         #self.setState(State.Busy if self.status=="move" else State.Ready)
         if self.state==State.Offline:
-            print "Communication resumed"
+            print "Communication resumed"        
         if self.reset or (self.state==State.Offline):
-            self.get_task()
+            self.check_task()
             if self.current_task is not None:
                 print "Ongoing task: " + self.current_task            
 
@@ -564,7 +586,8 @@ class RobotTCP(TcpDevice, Stoppable):
                 for m in self.joint_motors:
                     m.initialize()                
     #Alignment
-    def align(self, point = None, frame = FRAME_DEFAULT, desc = DESC_FAST):
+    def align(self, point = None, frame = FRAME_DEFAULT, desc = None):
+        if desc is None: desc = self.default_desc
         if point is None:
             self.get_cartesian_pos()
         else:
@@ -609,21 +632,29 @@ class RobotTCP(TcpDevice, Stoppable):
                 self.task_kill(task)                
 
     def get_task(self):
-        if self.current_task is not None:             
-            return self.current_task
-        for task in self.high_level_tasks:
-            if self.get_task_status(task)[0]>=0:                
-                 self.current_task = task
-                 log("Task detected: " + str(self.current_task), False)        
-                 return task
-        return None
+        return self.current_task
+        
+    def check_task(self):
+        if self.current_task is None:        
+            for task in self.high_level_tasks:
+                if self.get_task_status(task)[0]>=0:                
+                     self.current_task = task
+                     log("Task detected: " + str(self.current_task), False)        
+        return self.current_task
     
-    def wait_task_finished (self):
-        while get_task() != None:
-            time.sleep(0.01)        
+    def wait_task_finished (self, polling = None):
+        cur_polling = self.polling
+        if polling is not None:
+            self.polling = polling
+        try:
+            while self.get_task() != None:
+                time.sleep(self.polling_interval)        
+        finally:
+            if polling is not None:
+                self.polling = cur_polling
 
     def assert_no_task(self):
-        task = self.get_task()
+        task = self.check_task()
         if task != None:
             raise Exception("Ongoing task: " + task)
         

script/local.py

@@ -64,8 +64,7 @@ except:
    print  >> sys.stderr, traceback.format_exc()    
 
 try:
-    robot.setPolling(DEFAULT_ROBOT_POLLING)      
-    robot.high_level_tasks = ["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "movePark"]
+    robot.setPolling(DEFAULT_ROBOT_POLLING)          
     robot.set_tool(TOOL_CALIBRATION)
     robot.set_motors_enabled(True)
     robot.set_joint_motors_enabled(True)

script/motion/mount.py

@@ -26,7 +26,7 @@ def mount(segment, puck, sample, force=False):
     location = get_current_point()
     print "Location: " + location
 
-    robot.move_dewar()        
+    robot.move_dewar_wait()        
     try:
         robot.get_dewar(segment, puck, sample)
     except:

script/motion/unmount.py

@@ -32,13 +32,13 @@ def unmount(segment, puck, sample, force=False):
         raise
 
     try:
-        robot.move_dewar()        
+        robot.move_dewar_wait()        
     except:
         #TODO: recover
         raise
 
     try:
-        robot.pet_dewar(segment, puck, sample)
+        robot.put_dewar(segment, puck, sample)
     except:
         #robot.move_dewar()        
         raise

script/test/TestCmdSynchronization.py

@@ -0,0 +1,5 @@
+enable_motion()
+while True:
+
+    robot.move_dewar_wait()
+    robot.move_park()