Add UltraArm gripper control interface

Author: anla-xu

pymycobot/common.py

@@ -234,6 +234,9 @@ class ProtocolCode(object):
     GET_CURRENT_COORD_INFO = "M114"
     GET_BACK_ZERO_STATUS = "M119"
     IS_MOVING_END = "M9"
+    GET_GRIPPER_ANGLE = "M50"
+    SET_SYSTEM_VALUE = "M51"
+    GET_SYSTEM_VALUE = "M52"
 
 
 class DataProcessor(object):

pymycobot/ultraArm.py

@@ -130,6 +130,8 @@ def _request(self, flag=""):
                         return 1
                     else:
                         return 0
+                elif flag in ['gripper', 'system']:
+                    return int(data[data.find("[")+1:-1])
                 elif flag == None:
                     return 0
 
@@ -600,4 +602,43 @@ def is_moving_end(self):
 
     def sync(self):
         while self.is_moving_end() != 1:
-            pass
+            pass
+        
+    def get_gripper_angle(self):
+        """
+        
+        """
+        command = ProtocolCode.GET_GRIPPER_ANGLE + ProtocolCode.END
+        self._serial_port.write(command.encode())
+        self._serial_port.flush()
+        self._debug(command)
+        return self._request("gripper")
+    
+    def set_system_value(self, id, address, value):
+        """_summary_
+
+        Args:
+            id (int): 4 or 7
+            address (int): 0 ~ 69
+            value (int): 
+        """
+        command = ProtocolCode.SET_SYSTEM_VALUE +" X{} ".format(id) + "Y{} ".format(address) +"Z{} ".format(value)+ ProtocolCode.END
+        self._serial_port.write(command.encode())
+        self._serial_port.flush()
+        self._debug(command)
+        
+    def get_system_value(self, id, address):
+        """_summary_
+
+        Args:
+            id (int): 4 or 7
+            address (_type_): 0 ~ 69
+
+        Returns:
+            _type_: _description_
+        """
+        command = ProtocolCode.GET_SYSTEM_VALUE + " J{} ".format(id) + "S{} ".format(address) +ProtocolCode.END
+        self._serial_port.write(command.encode())
+        self._serial_port.flush()
+        self._debug(command)
+        return self._request("system")