Fixed the error of MyArmM pid setting range

Author: K-Shuffler

pymycobot/common.py

@@ -263,6 +263,7 @@ class ProtocolCode(object):
     GET_SERVOS_ENCODER_DRAG = 0xEF
     RESTORE_SERVO_SYSTEM_PARAM = 0x0a
     GET_SERVO_D = 0xE8
+    SET_SERVO_P = 0x70
     # IIC
     # SET_IIC_STATE = 0xA4
     # GET_IIS_BYTE = 0xA5

pymycobot/error.py

@@ -312,7 +312,7 @@ def calibration_parameters(**kwargs):
         "joint_id": [1, 2, 3, 4, 5, 6, 7],
         "servo_id": [1, 2, 3, 4, 5, 6, 7, 8],
         "angles_min": [-168, -77, -86, -159, -95, -161, -118],
-        "angles_max": [172, 90, 91, 148, 84, 146, 0],
+        "angles_max": [172, 90, 91, 148, 84, 146, 2],
         "encoders_min": [137, 1163, 1035, 1013, 248, 979, 220, 706],
         "encoders_max": [4004, 2945, 3079, 3026, 3724, 2994, 3704, 2048],
     }
@@ -453,13 +453,13 @@ def calibration_parameters(**kwargs):
                 min_encoder = limit_info["encoders_min"][i]
                 if value < min_encoder or value > max_encoder:
                     raise MyArmDataException(
-                        "angle value not right, should be {min_encoder} ~ {max_encoder}, but received {value}"
+                        f"angle value not right, should be {min_encoder} ~ {max_encoder}, but received {value}"
                     )
             elif parameter == 'encoders':
                 for i, v in enumerate(value):
                     max_encoder = limit_info["encoders_max"][i]
                     min_encoder = limit_info["encoders_min"][i]
-                    if value < min_encoder or value > max_encoder:
+                    if v < min_encoder or v > max_encoder:
                         raise MyArmDataException(
                             f"encoder value not right, should be {min_encoder} ~ {max_encoder}, but received {v}"
                         )

pymycobot/myarm_api.py

@@ -96,6 +96,8 @@ def _mesg(self, genre, *args, **kwargs):
                     result = self._int2angle(result)
                 return result
             if genre in (ProtocolCode.GET_ATOM_PRESS_STATUS, ):
+                if self.__class__.__name__ == 'MyArmM':
+                    return res[0]
                 return res
             elif genre in [
                 ProtocolCode.GET_ANGLES, ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE,
@@ -115,7 +117,7 @@ def _mesg(self, genre, *args, **kwargs):
                     rank = [i for i, e in enumerate(reverse_bins) if e != '0']
                     result.append(rank)
                 return result
-            elif genre == ProtocolCode.GET_ROBOT_FIRMWARE_VERSION:
+            elif genre in (ProtocolCode.GET_ROBOT_FIRMWARE_VERSION, ProtocolCode.GET_ROBOT_TOOL_FIRMWARE_VERSION):
                 return self._int2coord(res[0])
             else:
                 return res
@@ -144,10 +146,6 @@ def get_robot_tool_firmware_version(self):
         """Get the Robot Tool Firmware Version (End Atom)"""
         return self._mesg(ProtocolCode.GET_ROBOT_TOOL_FIRMWARE_VERSION, has_reply=True)
 
-    # def get_robot_serial_number(self):
-    #     """Get the bot number"""
-    #     return self._mesg(ProtocolCode.GET_ROBOT_SERIAL_NUMBER, has_reply=True)
-
     def set_robot_err_check_state(self, status):
         """Set Error Detection Status You can turn off error detection, but do not turn it off unless necessary
 
@@ -175,7 +173,8 @@ def get_recv_queue_max_len(self):
 
     def set_recv_queue_max_len(self, max_len):
         """Set the total length of the receiving command queue"""
-        self._mesg(ProtocolCode.SET_RECV_QUEUE_SIZE, max_len, has_reply=True)
+        assert 0 < max_len <= 100, "queue size must be in range 1 - 100"
+        self._mesg(ProtocolCode.SET_RECV_QUEUE_SIZE, max_len)
 
     def get_recv_queue_len(self):
         """The current length of the read receive queue"""
@@ -275,7 +274,9 @@ def set_servo_p(self, servo_id, data):
             data (int): 0-254
 
         """
-        self._mesg(ProtocolCode.MERCURY_DRAG_TECH_SAVE, servo_id, data)
+        assert 0 <= data <= 254, "data must be between 0 and 254"
+        self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
+        self._mesg(ProtocolCode.SET_SERVO_P, servo_id, data)
 
     def get_servo_p(self, servo_id):
         """Reads the position loop P scale factor of the specified servo motor
@@ -292,6 +293,8 @@ def set_servo_d(self, servo_id, data):
             servo_id (int): 0-254
             data (int): 0-254
         """
+        assert 0 <= data <= 254, "data must be between 0 and 254"
+        self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
         self._mesg(ProtocolCode.MERCURY_DRAG_TECH_EXECUTE, servo_id, data)
 
     def get_servo_d(self, servo_id):
@@ -309,6 +312,8 @@ def set_servo_i(self, servo_id, data):
             servo_id (int): 0 - 254
             data (int): 0 - 254
         """
+        assert 0 <= data <= 254, "data must be between 0 and 254"
+        self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
         self._mesg(ProtocolCode.MERCURY_DRAG_TECH_PAUSE, servo_id, data)
 
     def get_servo_i(self, servo_id):
@@ -322,6 +327,8 @@ def set_servo_cw(self, servo_id, data):
             servo_id (int): 0 - 254
             data (int): 0 - 32
         """
+        assert 0 <= data <= 32, "data must be between 0 and 32"
+        self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
         self._mesg(ProtocolCode.SET_SERVO_MOTOR_CLOCKWISE, servo_id, data)
 
     def get_servo_cw(self, servo_id):
@@ -339,6 +346,8 @@ def set_servo_cww(self, servo_id, data):
             servo_id (int): 0 - 254
             data (int): 0 - 32
         """
+        assert 0 <= data <= 32, "data must be between 0 and 32"
+        self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
         return self._mesg(ProtocolCode.SET_SERVO_MOTOR_COUNTER_CLOCKWISE, servo_id, data)
 
     def get_servo_cww(self, servo_id):