script/devices/Hexiposi.py

@@ -82,6 +82,9 @@ class Hexiposi(DiscretePositionerBase):
             except:
                 pass
 
+    def assert_in_position(self, pos):
+        return take() == pos
+
     #def isReady(self):
     #    self.get_status()
     #    return self.moving == False
@@ -102,4 +105,15 @@ dev = Hexiposi("hexiposi", "myriotell:8002/hexiposi")
 add_device(dev, True)
 hexiposi.polling=500
 #print dev.url
-#print dev.get_status()
+#print dev.get_status()
+
+class hexiposi_position(ReadonlyRegisterBase):
+    def doRead(self):
+        try:
+            return float(hexiposi.pos)
+        except:
+            return float("nan")
+            
+add_device(hexiposi_position(), True)
+hexiposi_position.polling = 1000
+

script/devices/RobotSC.py

@@ -24,17 +24,49 @@ class RobotSC(RobotTCP):
         RobotTCP.__init__(self, name, server, timeout, retries)   
         self.setPolling(DEFAULT_ROBOT_POLLING)      
         
-    def mount(self, segment, puck, sample):
-        return self.execute('mount',segment, puck, sample)   
+    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.assert_dewar()
 
-    def firstmount(self, segment, puck, sample):
-        return self.execute('firstmount', segment, puck, sample)
+    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.assert_dewar()
 
-    def unmount(self, segment, puck, sample):
-        return self.execute('unmount',segment, puck, sample)
+    def put_gonio(self):    
+        self.start_task('putGonio',segment, puck, sample) 
+        self.wait_task_finished()    
+        #TODO
 
-    def robotRecover(self):
-        return self.execute('robotRecover')
+    def get_gonio(self):
+        self.start_task('getGonio') 
+        self.wait_task_finished()    
+        #TODO
+
+    def robot_recover(self):
+        self.start_task('robotRecover')
+        self.wait_task_finished()    
+
+    def move_dewar(self):
+        self.start_task('moveDewar')  
+        self.wait_task_finished()    
+        self.assert_dewar()
+
+    def move_park(self):
+        self.start_task('movePark')   
+        self.wait_task_finished()    
+        self.assert_park()
+
+    def toSegmentNumber(self, segment):
+        if type(segment) == str:
+            segment = ord(segment.upper()) - ord('A') +1
+        return segment
+    
        
     def on_event(self,ev):
         #print "EVT: " + ev
@@ -65,7 +97,49 @@ class RobotSC(RobotTCP):
                 
     def set_local(self):
         robot.set_profile("default")
+
+    def is_home(self):
+        return  self.is_in_point("pHome")       
+    
+    def is_park(self):
+        return  self.is_in_point("pPark")   
+    
+    def is_dewarHome(self):
+        return  self.is_in_point("pDewarHome")
+
+    def is_dewarWait(self):
+        return  self.is_in_point("pDewarWait")   
+
+    def is_gonio(self):
+        return  self.is_in_point("pGonioHome")   
                 
+    def is_cleared(self):
+        return self.is_home() or self.is_park() or self.is_dewarHome() or self.is_dewarWait()
+
+    def assert_home(self):
+        self.assert_in_point("pHome")       
+    
+    def assert_park(self):
+        self.assert_in_point("pPark")   
+    
+    def assert_dewarHome(self):
+        self.assert_in_point("pDewarHome")
+
+    def assert_dewarWait(self):
+        self.assert_in_point("pDewarWait")   
+
+    def assert_gonio(self):
+        self.assert_in_point("pGonioHome")   
+
+    def assert_cleared(self):
+        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
+        return None
         
 
 if simulation:

script/devices/RobotTCP.py

@@ -25,7 +25,7 @@ class RobotTCP(TcpDevice, Stoppable):
          self.status = None
          self.lock = threading.Lock()
          self.joint_forces = None
-         self.current_task = None
+         self.current_task = None         
          self.high_level_tasks = []         
          self.cartesian_destination = None  
          #self.flange_pos = [None] * 6
@@ -340,9 +340,9 @@ class RobotTCP(TcpDevice, Stoppable):
         time.sleep(0.05)
         self.update()  
         #time.sleep(0.01)
-        #robot.waitCacheChange(-1)              
+        #self.waitCacheChange(-1)              
         self.waitReady(-1)
-        #robot.waitState(State.Ready, -1)
+        #self.waitState(State.Ready, -1)
             
 
     #Tool
@@ -455,8 +455,10 @@ class RobotTCP(TcpDevice, Stoppable):
             self.speed = int(sts[3])
             self.empty = sts[4] == "1"
             self.settled = sts[5] == "1"                                    
-            if int(sts[6]) < 0:
-                self.current_task = None
+            if int(sts[6]) < 0:         
+                if self.current_task is not None:     
+                    log("Task finished: " + str(self.current_task), False)
+                self.current_task = None                
 
             for i in range(6):
                 self.joint_pos[i] = float(sts[7 + i])
@@ -513,6 +515,17 @@ 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
+        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         
+        if not self.is_in_point(p, tolerance):
+            raise Exception ("Not in position " + p)
+
+    
     #Cartesian peudo-motors
     def set_motors_enabled(self, value):
         if value !=self.cartesian_motors_enabled:
@@ -567,21 +580,25 @@ class RobotTCP(TcpDevice, Stoppable):
     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(pro)
+            tasks.append(self.current_task)
         if not program in tasks: 
-            tasks.append(pro)
+            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 > 5000:
-                raise Exception("Cannot start task " + task)
-            time.sleep(0.1)        
-        self.update()               
+        #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)        
+
+        self.update()               
         self._update_state()
+        return code
         
     def stop_task(self):
         tasks = [t for t in self.high_level_tasks]        
@@ -597,10 +614,19 @@ class RobotTCP(TcpDevice, Stoppable):
         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
+    
+    def wait_task_finished (self):
+        while get_task() != None:
+            time.sleep(0.01)        
 
-
+    def assert_no_task(self):
+        task = self.get_task()
+        if task != None:
+            raise Exception("Ongoing task: " + task)
+        
     def on_event(self,ev):
         pass