Merge pull request #56 from Mrkun5018/main

add myarmm and myarmc class api

Author: anla-xu

pymycobot/__init__.py

@@ -26,7 +26,8 @@
 from pymycobot.mercurychassis import MercuryChassis
 from pymycobot.mercurysocket import MercurySocket
 from pymycobot.mycobotpro630 import Phoenix
-
+from pymycobot.myarmm import MyArmM
+from pymycobot.myarmc import MyArmC
 __all__ = [
     "MyCobot",
     "CommandGenerator",
@@ -50,7 +51,9 @@
     "MyArmSocket",
     "MercuryChassis",
     "MercurySocket",
-    "Phoenix"
+    "Phoenix",
+    "MyArmM",
+    "MyArmC"
 ]
 
 

pymycobot/common.py

@@ -229,16 +229,18 @@ class ProtocolCode(object):
     GET_ATOM_LED_COLOR = 0x6a
     SET_ATOM_PIN_STATUS = 0x61
     GET_ATOM_PIN_STATUS = 0x62
-    SET_MASTER_PIN_STATUS = 0x61
-    GET_MASTER_PIN_STATUS = 0x62
-    SET_AUXILIARY_PIN_STATUS = 0x63
-    GET_AUXILIARY_PIN_STATUS = 0x64
+    SET_MASTER_PIN_STATUS = 0x65
+    GET_MASTER_PIN_STATUS = 0x66
+    SET_AUXILIARY_PIN_STATUS = 0xa0
+    GET_AUXILIARY_PIN_STATUS = 0xa1
     SET_SERVO_MOTOR_CLOCKWISE = 0x73
     GET_SERVO_MOTOR_CLOCKWISE = 0Xea
     SET_SERVO_MOTOR_COUNTER_CLOCKWISE = 0x74
     GET_SERVO_MOTOR_COUNTER_CLOCKWISE = 0xeb
     SET_SERVO_MOTOR_CONFIG = 0x52
     GET_SERVO_MOTOR_CONFIG = 0x53
+    CLEAR_RECV_QUEUE = 0x19
+    GET_RECV_QUEUE_LENGTH = 0x08
     GET_BASE_COORDS = 0xF0
     BASE_TO_SINGLE_COORDS = 0xF1
     COLLISION = 0xF2
@@ -249,6 +251,9 @@ class ProtocolCode(object):
     JOG_INC_COORD = 0xF7
     COLLISION_SWITCH = 0xF8
     IS_COLLISION_ON = 0xF9
+    CLEAR_ROBOT_ERROR = 0x16
+    GET_RECV_QUEUE_SIZE = 0x17
+    SET_RECV_QUEUE_SIZE = 0x18
 
     # IIC
     # SET_IIC_STATE = 0xA4
@@ -300,7 +305,6 @@ def _mesg(self, genre, *args, **kwargs):
                 has_reply: Whether there is a return value to accept.
         """
         command_data = self._process_data_command(genre, self.__class__.__name__, args)
-
         if genre == 178:
             # 修改wifi端口
             command_data = self._encode_int16(command_data)
@@ -328,7 +332,7 @@ def _mesg(self, genre, *args, **kwargs):
         real_command = self._flatten(command)
         has_reply = kwargs.get("has_reply", False)
         return real_command, has_reply
-    
+
     # Functional approach
     def _encode_int8(self, data):
         return struct.pack("b", data)
@@ -410,7 +414,7 @@ def _process_data_command(self, genre, _class, args):
                         byte_value = data.to_bytes(4, byteorder='big', signed=True)
                         res = []
                         for i in range(len(byte_value)):
-                           res.append(byte_value[i])
+                            res.append(byte_value[i])
                         processed_args.extend(res)
                 else:
                     processed_args.extend(self._encode_int16(args[index]))
@@ -422,7 +426,7 @@ def _process_data_command(self, genre, _class, args):
                         byte_value = args[index].to_bytes(2, byteorder='big', signed=True)
                         res = []
                         for i in range(len(byte_value)):
-                           res.append(byte_value[i])
+                            res.append(byte_value[i])
                         processed_args.extend(res)
                     else:
                         processed_args.append(args[index])
@@ -446,7 +450,7 @@ def _process_received(self, data, genre, arm=6):
         header_i, header_j = 0, 1
         while header_j < data_len - 4:
             if self._is_frame_header(data, header_i, header_j):
-                if arm in [6, 7, 14]:
+                if arm in [6, 7, 14, 8]:
                     cmd_id = data[header_i + 3]
                 elif arm == 12:
                     cmd_id = data[header_i + 4]
@@ -457,7 +461,7 @@ def _process_received(self, data, genre, arm=6):
             header_j += 1
         else:
             return []
-        if arm in [6, 7]:
+        if arm in [6, 7, 8]:
             data_len = data[header_i + 2] - 2
         elif arm == 12:
             data_len = data[header_i + 3] - 2
@@ -474,7 +478,7 @@ def _process_received(self, data, genre, arm=6):
                 data_pos = header_i + 5
             data_len -= 1
         else:
-            if arm in [6, 7, 14]:
+            if arm in [6, 7, 14, 8]:
                 data_pos = header_i + 4
             elif arm == 12:
                 data_pos = header_i + 5
@@ -487,7 +491,17 @@ def _process_received(self, data, genre, arm=6):
         #         res.append(i)
         #     return res    
         if data_len in [6, 8, 12, 14, 16, 24, 26, 60]:
-            if data_len == 8 and arm == 14 and cmd_id == ProtocolCode.IS_INIT_CALIBRATION:
+            ignor_t = (
+                ProtocolCode.GET_SERVO_CURRENTS,
+                ProtocolCode.GET_SERVO_TEMPS,
+                ProtocolCode.GET_SERVO_VOLTAGES,
+                ProtocolCode.GET_SERVO_STATUS,
+                ProtocolCode.IS_ALL_SERVO_ENABLE
+            )
+            if data_len == 8 and (
+                    (arm == 14 and cmd_id == ProtocolCode.IS_INIT_CALIBRATION) or
+                    (arm == 8 and cmd_id in ignor_t)
+            ):
                 for v in valid_data:
                     res.append(v)
                 return res
@@ -650,7 +664,8 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
     if _class in ["Mercury", "MercurySocket"]:
         if genre == ProtocolCode.POWER_ON:
             wait_time = 8
-        elif genre in [ProtocolCode.POWER_OFF, ProtocolCode.RELEASE_ALL_SERVOS, ProtocolCode.FOCUS_ALL_SERVOS, ProtocolCode.RELEASE_SERVO, ProtocolCode.FOCUS_SERVO, ProtocolCode.STOP]:
+        elif genre in [ProtocolCode.POWER_OFF, ProtocolCode.RELEASE_ALL_SERVOS, ProtocolCode.FOCUS_ALL_SERVOS,
+                       ProtocolCode.RELEASE_SERVO, ProtocolCode.FOCUS_SERVO, ProtocolCode.STOP]:
             wait_time = 3
     if method is not None:
         if genre == 177:

pymycobot/generate.py

@@ -103,15 +103,15 @@ def get_robot_version(self):  # TODO: test method <2021-03-11, yourname> #
             mycobotPro: 101
         """
         return self._mesg(ProtocolCode.ROBOT_VERSION, has_reply=True)
-    
+
     def get_system_version(self):
         """get system version"""
-        return self._mesg(ProtocolCode.SOFTWARE_VERSION, has_reply = True)
+        return self._mesg(ProtocolCode.SOFTWARE_VERSION, has_reply=True)
 
     def get_robot_id(self):
         """get robot id"""
-        return self._mesg(ProtocolCode.GET_ROBOT_ID, has_reply = True)
-    
+        return self._mesg(ProtocolCode.GET_ROBOT_ID, has_reply=True)
+
     def set_robot_id(self, id):
         """set robot id
         
@@ -408,7 +408,8 @@ def set_encoder(self, joint_id, encoder, speed):
             encoder: The value of the set encoder.
             speed : 1 - 100
         """
-        self.calibration_parameters(class_name = self.__class__.__name__, encode_id = joint_id, encoder = encoder, speed=speed)
+        self.calibration_parameters(class_name=self.__class__.__name__, encode_id=joint_id, encoder=encoder,
+                                    speed=speed)
         return self._mesg(ProtocolCode.SET_ENCODER, joint_id, [encoder], speed)
 
     def get_encoder(self, joint_id):
@@ -544,7 +545,8 @@ def set_servo_data(self, servo_id, data_id, value, mode=None):
             self.calibration_parameters(class_name = self.__class__.__name__, id=servo_id, address=data_id, value=value)
             return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, value)
         else:
-            self.calibration_parameters(class_name = self.__class__.__name__, id=servo_id, address=data_id, value=value, mode=mode)
+            self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id, value=value,
+                                        mode=mode)
             return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, [value], mode)
 
     def get_servo_data(self, servo_id, data_id, mode=None):
@@ -729,7 +731,8 @@ def set_gripper_value(self, gripper_value, speed, gripper_type=None):
                 4: Flexible gripper
         """
         if gripper_type is not None:
-            self.calibration_parameters(class_name = self.__class__.__name__, gripper_value=gripper_value, speed=speed, gripper_type=gripper_type)
+            self.calibration_parameters(class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed,
+                                        gripper_type=gripper_type)
             return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, gripper_value, speed, gripper_type)
         else:
             return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, gripper_value, speed)