script/devices/RobotSC.py

@@ -224,7 +224,7 @@ robot.default_speed = 20
 robot.set_tool(TOOL_DEFAULT)
 robot.setPolling(DEFAULT_ROBOT_POLLING)          
    
-
+robot.get_current_point()   #TODO: REMOVE WHEN CURRENT POINT REPORTED BY POLLING MESSAGE
 
 class jf1(ReadonlyRegisterBase):
     def doRead(self):

script/devices/RobotTCP.py

@@ -30,6 +30,7 @@ class RobotTCP(TcpDevice, Stoppable):
          self.current_task_ret = None
          self.high_level_tasks = []    
          self.known_points = []     
+         self.current_points = []
          self.cartesian_destination = None  
          #self.flange_pos = [None] * 6
          self.cartesian_pos = [None] * 6
@@ -94,13 +95,28 @@ class RobotTCP(TcpDevice, Stoppable):
     def get_known_points(self):    
         return self.known_points
 
-    def get_current_point(self, tolerance = None):      
+    def get_current_points(self, tolerance = None):  
         ret = self.is_in_points(*self.known_points, tolerance = tolerance)
+        current_points = []
         for i in range(len(ret)):
             if ret[i] == True:
-                return self.known_points[i]
+                current_points.append(self.known_points[i])
+        return  current_points 
+            
+    def get_current_point(self, tolerance = None): 
+        current_points = self.get_current_points(tolerance)
+        if (current_points is not None) and ( len(current_points) >0):
+            return current_points[0] 
         return None
 
+    def get_current_points_cached(self):
+        return self.current_points
+    
+    def get_current_point_cached(self):
+        if (self.current_points is not None) and (len (self.current_points) >0):
+            return  self.current_points[0] 
+        return None
+        
     def assert_in_known_point(self, tolerance = None):
         if self.get_current_point(tolerance) is None:
             raise Exception ("Robot not in known point")        
@@ -585,7 +601,8 @@ class RobotTCP(TcpDevice, Stoppable):
                            "task": cur_task,
                            "mode": self.working_mode,
                            "status": self.status,
-                           "open": self.tool_open
+                           "open": self.tool_open,
+                           "pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by possiung funtion 
                            }, None)
             if self.cartesian_motors_enabled:
                 for m in self.cartesian_motors:
@@ -620,6 +637,9 @@ class RobotTCP(TcpDevice, Stoppable):
         return  self.distance_p("tcp_p", name)
 
     def is_in_point(self, p, tolerance = None): #Tolerance in mm
+        if (tolerance is None) and p in self.known_points:
+            #If checking a known point with default tolerance, updates the position cache
+            return p in self.get_current_points()
         tolerance = self.default_tolerance if tolerance == None else tolerance
         d = self.get_distance_to_pnt(p)
         if d<0:
@@ -644,6 +664,12 @@ class RobotTCP(TcpDevice, Stoppable):
                 ret[i] = None
             else:
                 ret[i] = ret[i]<tolerance
+        if (tolerance == self.default_tolerance) and (set(self.known_points).issubset(set(pars))): #Only update cache if tolerance is default
+            current_points = []
+            for i in range(len(ret)):
+                if ret[i] == True:
+                    current_points.append(self.known_points[i])        
+            self.current_points = current_points                       
         return ret
              
     def assert_in_point(self, p, tolerance = None): #Tolerance in mm         

script/local.py

@@ -207,7 +207,7 @@ dewar_level_listener.onValueChanged(dewar_level, dewar_level.take(), None)
 
 
 ###################################################################################################
-#  Global variables
+#  Global variables & application state
 ###################################################################################################
 
 
@@ -216,6 +216,9 @@ context = get_context()
 cover_detection_debug = False
 
 
+def is_puck_loading():
+    return robot.state == State.Ready and robot.take()["pos"] == 'pPark' and  feedback_psys_safety.take() == False
+
 
 update()