Script execution

script/RobotTCP.py

@@ -11,6 +11,8 @@ class RobotTCP(TcpDevice):
          self.cmd_separator = ' '
          self.msg_id = 0    
          self.working_mode = "invalid"     
+         self.status = "invalid" 
+         self.powered = False    
     
     def _sendReceive(self, msg_id, msg = ""):        
         tx = self.header if (self.header != None) else  ""
@@ -62,6 +64,9 @@ class RobotTCP(TcpDevice):
     def get_varb(self, name):
         return True if (self.execute('get_bool', name).strip() == '1') else False        
 
+    def get_str(self, name):
+        return self.execute('get_str', name)
+
     def get_arr(self, name, size):
         return self.execute('get_arr', name, size)
 
@@ -112,13 +117,25 @@ class RobotTCP(TcpDevice):
     def get_joint_forces(self):
         robot_tcp.evaluate("getJointForce(arr)")        
         return robot_tcp.get_float_arr(6)
+
+    def get_move_id(self):
+        return self.eval_int("getMoveId()")
+
+    def get_emergency_stop_sts(self):
+        st = self.eval_int("esStatus()")        
+        if (st== 1): return "active"
+        if (st== 2): return "activated"
+        return "off"
+
+    def get_safety_fault_signal(self):
+        fault = eval_bool("safetyFault(s)")
+        if (fault):
+            return get_str("s")            
+
         
-    def mount(self, puck, sample):
-        return self.execute('mount', puck, sample)
-
-
     def is_powered(self):
-        return self.eval_bool("isPowered()")
+        self.powered = self.eval_bool("isPowered()")
+        return self.powered
 
     def enable(self):
         return self.evaluate("enablePower()")
@@ -131,28 +148,43 @@ class RobotTCP(TcpDevice):
 
     def read_status(self):
         try:
-            mode =  self.eval_int("workingMode(arr)")
-            self.status = int(get_var("arr[0]"))
-            if mode==1: 
+            mode =  self.eval_int("workingMode(arr)")          
+            status = int(self.get_var("arr[0]"))            
+            if mode==1:                 
                 self.working_mode = "manual"
-            if mode==2: 
+                self.status = "hold" if status==6 else "move"
+            elif mode==2: 
                 self.working_mode = "test"
-            if mode==3: 
+                self.status = "hold" if status==3 else "move"
+            elif mode==3: 
                 self.working_mode = "local"
-            if mode==4: 
+                self.status = "hold" if status==2 else "move"
+            elif mode==4: 
                 self.working_mode = "remote"
+                self.status = "hold" if status==2 else "move"
+            else:
+                self.working_mode = "invalid"
+                self.status = "invalid"
         except:
             self.working_mode = "invalid"
-        return self.mode
+        self.is_powered()
+        return self.working_mode
 
     def doUpdate(self):
         try:
-            ev = self.read_event()
+            read_status()
+            ev = self.read_event()            
             self.setState(State.Ready)
             self.getLogger().info(ev)
         except:
             self.setState(State.Offline)            
+
+        
+    def mount(self, puck, sample):
+        return self.execute('mount', puck, sample)
+            
             
 
 add_device(RobotTCP("robot_tcp", "129.129.126.100:1000"), force = True)   
 robot_tcp.setPolling(500)
+robot_tcp.get_emergency_stop_sts()