script/devices/RobotSC.py

@@ -25,6 +25,7 @@ class RobotSC(RobotTCP):
         RobotTCP.__init__(self, name, server, timeout, retries)   
         self.set_tasks(["getDewar", "putDewar", "putGonio", "getGonio", "robotRecover", "moveDewar", "movePark"])
         self.setPolling(DEFAULT_ROBOT_POLLING)      
+        self.known_points = ["pHome", "pPark", "pDewarHome", "pGonioHome", "pDewarWait"]
         
     def get_dewar(self, segment, puck, sample):
         segment = self.toSegmentNumber(segment)
@@ -138,10 +139,11 @@ class RobotSC(RobotTCP):
         if not self.is_cleared():
             raise Exception("Robot not in cleared position")
 
-    def get_current_point(self):
-        for p in ["pHome", "pPark", "pDewarHome", "pGonioHome", "pDewarWait"]:
-            if self.is_in_point(p):
-                return p
+    def get_current_point(self):      
+        ret = robot.is_in_points(*self.known_points, tolerance = 20)
+        for i in range(len(ret)):
+            if ret[i] == True:
+                return self.known_points[i]
         return None
         
 

script/devices/RobotTCP.py

@@ -40,7 +40,11 @@ class RobotTCP(TcpDevice, Stoppable):
          #self.tool_trsf = [0.0] * 6
          self.frame = FRAME_DEFAULT
          self.polling_interval = 0.01
-         self.reset = True                    
+         self.reset = True           
+
+         self.task_start_retries = 3
+         self.exception_on_task_start_failure = False  #Tasks may start and be finished when checked
+                  
 
     def doInitialize(self):           
         super(RobotTCP, self).doInitialize()
@@ -453,6 +457,16 @@ class RobotTCP(TcpDevice, Stoppable):
         else: status = "Error"  
         return (code,status)
 
+    def get_tasks_status(self, *pars):
+        ret = self.execute("task_sts", *pars)
+        ret = ret[0:len(pars)]
+        for i in range(len(ret)):
+            try:
+                ret[i] = int(ret[i])
+            except:
+                ret[i] = None   
+        return ret
+
     def _update_state(self):
         #self.setState(State.Busy if self.status=="move" else State.Ready)
         if self.state==State.Offline:
@@ -469,8 +483,6 @@ class RobotTCP(TcpDevice, Stoppable):
     def doUpdate(self):        
         try:
             start = time.time()
-            #sts = self._sendReceive("EVT").strip().split(self.array_separator)        
-            #sts = self.execute("get_status", self.current_task, self.tool, self.frame)
             sts = self.execute("get_status", self.current_task)
             self._update_working_mode(int(sts[0]), int(sts[1]))
             self.powered = sts[2] == "1"
@@ -479,24 +491,20 @@ class RobotTCP(TcpDevice, Stoppable):
             self.settled = sts[5] == "1"                                    
             if int(sts[6]) < 0:         
                 if self.current_task is not None:     
-                    log("Task finished: " + str(self.current_task), False)
+                    log("Task "+ str(self.current_task) + " finished with code: " + str(sts[7]), False)
                 self.current_task = None                
 
             for i in range(6):
-                self.joint_pos[i] = float(sts[7 + i])
+                self.joint_pos[i] = float(sts[8 + i])
             for i in range(6):
-                #self.flange_pos[i] = float(sts[13 + i])
-                #self.tool_pos[i] =  (self.flange_pos[i] - self.tool_trsf[i] ) if i<3 else (self.flange_pos[i] + self.tool_trsf[i] )
-                self.cartesian_pos[i] = float(sts[13 + i])
+                self.cartesian_pos[i] = float(sts[14 + i])
             
-            ev_index = 19 #7
+            ev_index = 20 #7
             ev = sts[ev_index] if len(sts)>ev_index else ""            
             if len(ev.strip()) >0:
                 self.getLogger().info(ev)
                 self.on_event(ev) 
-
                 
-            #if (self.current_task is not None) and (self.get_task_status(self.current_task)[0]<0):
             self._update_state()              
             self.reset = False
             self.setCache({"powered": self.powered,
@@ -537,13 +545,33 @@ class RobotTCP(TcpDevice, Stoppable):
         self.get_cartesian_pos() 
         return  self.distance_p("tcp_p", name)
 
-    def is_in_point(self, p, tolerance = 50): #Tolerance in mm
+    def is_in_point(self, p, tolerance = 20): #Tolerance in mm
         d = self.get_distance_to_pnt(p)
         if d<0:
             raise Exception ("Error calculating distance to " + p + " : " + str(d))
         return d<tolerance
-         
-    def assert_in_point(self, p, tolerance = 50): #Tolerance in mm         
+
+    def get_distance_to_pnts(self, *pars):
+        ret = self.execute("dist_pnt", *pars)
+        ret = ret[0:len(pars)]
+        for i in range(len(ret)):
+            try:
+                ret[i] = float(ret[i])
+            except:
+                ret[i] = None         
+        return ret
+        
+    def is_in_points(self, *pars, **kwargs): #Tolerance in mm
+        tolerance = 20 if (kwargs is None) or (not kwargs.has_key("tolerance")) else kwargs["tolerance"]                
+        ret = self.get_distance_to_pnts(*pars)
+        for i in range(len(ret)):        
+            if ret[i]<0:
+                ret[i] = None
+            else:
+                ret[i] = ret[i]<tolerance
+        return ret
+             
+    def assert_in_point(self, p, tolerance = 20): #Tolerance in mm         
         if not self.is_in_point(p, tolerance):
             raise Exception ("Not in position " + p)
 
@@ -600,27 +628,43 @@ class RobotTCP(TcpDevice, Stoppable):
         return np
         
     #High-level, exclusive motion task.
-    def start_task(self, program, *args,  **kwargs):
+    def start_task(self, program, *args,  **kwargs):        
         tasks = [t for t in self.high_level_tasks]
         if (self.current_task is not None) and (not self.current_task in tasks): 
             tasks.append(self.current_task)
         if not program in tasks: 
             tasks.append(program)
-        for task in tasks:
-            if self.get_task_status(task)[0]>=0:
-                raise Exception("Ongoing high-level task: " + task)        
-        self.task_create(program, *args, **kwargs)
-        start = time.time()
-        #while self.get_task_status(program)[0] < 0:
-        #    if time.time() - start > 5.0:
-        #        raise Exception("Cannot start task " + task)
-        #    time.sleep(0.1)                        
-        self.current_task = program
-        (code, status) = self.get_task_status(task)
-        log("Task started: " + str(self.current_task) + " - status: " + str(status) + " (" + str(code) + ")", False)        
+        #for task in tasks:
+        #    if self.get_task_status(task)[0]>=0:
+        #        raise Exception("Ongoing high-level task: " + task)        
+        ts = robot.get_tasks_status(*tasks)
+        for i in range(len(ts)):
+            if ts[i] > 0:
+                raise Exception("Ongoing high-level task: " + tasks[i])        
+        
+        self.clear_task_ret()
 
+        for i in range(self.task_start_retries):
+            self.task_create(program, *args, **kwargs)                                    
+            (code, status) = self.get_task_status(program)                                           
+            if code>=0: break
+            if i < retries-1:
+                ret = self.get_task_ret()
+                if ret == 0: #Did't start
+                    log("Retrying starting : " + str(program) + " - status: " + str(status) + " (" + str(code) + ")", False)        
+                    print "Retrying starting : " + str(program) + " - status: " + str(status) + " (" + str(code) + ")"                
+                else :
+                    break
+            else:
+                log("Failed starting : " + str(program), False)       
+                print "Failed starting : " + str(program) 
+                if self.exception_on_task_start_failure:
+                    raise Exception("Cannot start task: " + task)                                
+                              
+        log("Task started: " + str(program) + " - status: " + str(status) + " (" + str(code) + ")", False)        
+        self.current_task = program
         self.update()               
-        self._update_state()
+        #self._update_state()
         return code
         
     def stop_task(self):
@@ -631,6 +675,13 @@ class RobotTCP(TcpDevice, Stoppable):
             #if self.get_task_status(task)[0]>=0:                
                 self.task_kill(task)                
 
+
+    def get_task_ret(self):
+        return self.eval_int("tcp_ret")
+
+    def clear_task_ret(self):
+        return self.evaluate("tcp_ret=0")
+        
     def get_task(self):
         return self.current_task
         
@@ -648,7 +699,7 @@ class RobotTCP(TcpDevice, Stoppable):
             self.polling = polling
         try:
             while self.get_task() != None:
-                time.sleep(self.polling_interval)        
+                time.sleep(self.polling_interval)  
         finally:
             if polling is not None:
                 self.polling = cur_polling

script/motion/mount.py

@@ -23,7 +23,7 @@ def mount(segment, puck, sample, force=False):
         robot.move_park()        
         assert_img_in_cover_pos(segment)
     
-    location = get_current_point()
+    location = robot.get_current_point()
     print "Location: " + location
 
     robot.move_dewar_wait()        

script/motion/unmount.py

@@ -22,7 +22,7 @@ def unmount(segment, puck, sample, force=False):
         robot.move_park()
         assert_img_in_cover_pos(segment)
 
-    location = get_current_point()
+    location = robot.get_current_point()
 
     
     try:

script/test/TestCmdSynchronization.py

@@ -1,5 +1,32 @@
-enable_motion()
-while True:
+import java.util.logging.Level
 
-    robot.move_dewar_wait()
-    robot.move_park()
+def get_pos_str():
+    return "point: " + str(robot.get_current_point()) + " - here: " + str(robot.here()) + " - herej: " + str(robot.herej())
+
+enable_motion()
+
+get_context().setLogLevel(java.util.logging.Level.FINER)
+try:
+    while True:
+    #robot.move_dewar_wait()
+    #robot.move_park()
+
+        log("Moving dewar", False)
+        flag = robot.start_task('moveDewar')  
+        print "moveDewar: ", flag        
+        log("Waiting", False)
+        robot.wait_task_finished(TASK_WAIT_ROBOT_POLLING)            
+        log("Moving dewar finished - " + get_pos_str(), False)        
+        robot.assert_dewar_wait()
+
+
+        log("Moving park", False)
+        flag = robot.start_task('movePark')  
+        print "movePark: ", flag
+        log("Waiting", False)
+        robot.wait_task_finished(TASK_WAIT_ROBOT_POLLING)    
+        log("Moving park finished - " + get_pos_str(), False)     
+        robot.assert_park()    
+
+finally:
+    get_context().setLogLevel(java.util.logging.Level.INFO)