Closedown

script/RobotSC.py

@@ -2,18 +2,17 @@ run("RobotTCP")
 
 simulation = True
 
+
 class RobotSC(RobotTCP):    
     def mount(self, puck, sample):
-        return self.execute('mount', puck, sample)   
+        return self.execute('mount',segment, puck, sample)   
 
     def firstmount(self, puck, sample):
-        return self.execute('firstmount', puck, sample)
+        return self.execute('firstmount', segment, puck, sample)
 
     def unmount(self, puck, sample):
-        return self.execute('unmount', puck, sample)
-
-        
-
+        return self.execute('unmount',segment, puck, sample)
+       
     def on_event(self,ev):
         #print "EVT: " + ev
         pass
@@ -26,10 +25,12 @@ class RobotSC(RobotTCP):
             if self.state != State.Offline:
                 self.get_joint_forces()
                 for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: 
-                    dev.update()
+                    dev.update()        
+                
         #print time.time() -start
 
-
+    
+        
     
 
 
@@ -38,10 +39,12 @@ if simulation:
 else:
     add_device(RobotSC("robot", "129.129.126.100:1000"), force = True) 
 
+robot.high_level_tasks = ["mount", "firstmount"]
 robot.setPolling(20)
 #robot.set_monitor_speed(20)
 
 
+
 class jf1(ReadonlyRegisterBase):
     def doRead(self):
         return  None if robot.joint_forces == None else robot.joint_forces[0]

script/RobotTCP.py

@@ -19,6 +19,8 @@ class RobotTCP(TcpDevice, Stoppable):
          self.status = None
          self.lock = threading.Lock()
          self.joint_forces = None
+         self.running_task = False
+         self.high_level_tasks = []
          
     def _sendReceive(self, msg_id, msg = "", timeout =  None):  
         tx = self.header if (self.header != None) else  ""
@@ -342,7 +344,7 @@ class RobotTCP(TcpDevice, Stoppable):
         #self.setState(State.Busy if self.status=="move" else State.Ready)
         if self.state==State.Offline:
             print "Communication resumed"
-        if not self.settled: self.setState(State.Busy)
+        if (not self.settled) or (self.running_task is not None): self.setState(State.Busy)
         if not self.empty: self.setState(State.Paused)
         else: self.setState(State.Ready)                
 
@@ -359,18 +361,30 @@ class RobotTCP(TcpDevice, Stoppable):
             ev = sts[6] if len(sts)>6 else ""            
             if len(ev.strip()) >0:
                 self.getLogger().info(ev)
-                self.on_event(ev)        
-            self._update_state()  
+                self.on_event(ev) 
+            if ((self.running_task is not None) and  (self.get_task_status(self.running_task)<=0)):
+                self.running_task = None
+            self._update_state()              
             self.setCache({"powered": self.powered,
                            "speed": self.speed,
                            "empty": self.empty,
-                           "settled": self.settled }, None)
+                           "settled": self.settled,
+                           "task": self.running_task }, None)
+
             #print time.time() - start                        
         except:
             if self.state != State.Offline:
                 print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
             self.setState(State.Offline)            
 
+    #High-level, exclusive motion task.
+    def start_task(self, program, *args,  **kwargs):
+        for task in HIGH_LEVEL_TASKS:
+            if self.get_task_status(task)>=0:
+                raise Exception("Cannot start program - ongoing high-level task: " + task)
+        self.current_task = program
+        self.task_create(program, args, kwargs)
+
 
     def on_event(self,ev):
         pass