Merge branches 'main' and 'main' of https://github.com/elephantrobotics/pymycobot

Author: anla-xu

Makefile

@@ -263,6 +263,8 @@ class ProtocolCode(object):
     GET_SERVOS_ENCODER_DRAG = 0xEF
     RESTORE_SERVO_SYSTEM_PARAM = 0x0a
     GET_SERVO_D = 0xE8
+    SET_SERVO_P = 0x70
+    GET_SERVO_P = 0xE7
     # IIC
     # SET_IIC_STATE = 0xA4
     # GET_IIS_BYTE = 0xA5
@@ -728,6 +730,7 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
             wait_time = 1
         while True and time.time() - t < wait_time:
             data = self._serial_port.read()
+            self.log.debug("data: {}".format(data))
             k += 1
             if _class in ["Mercury", "MercurySocket"]:
                 if data_len == 3:
@@ -742,7 +745,8 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
                     if genre in (
                         ProtocolCode.GET_ATOM_PRESS_STATUS,
                         ProtocolCode.GET_SERVO_MOTOR_COUNTER_CLOCKWISE,
-                        ProtocolCode.GET_SERVO_MOTOR_CLOCKWISE
+                        ProtocolCode.GET_SERVO_MOTOR_CLOCKWISE,
+                        ProtocolCode.GET_SERVO_P,
                     ) and _class in ["MyArmM", "MyArmC", "MyArmAPI"]:
                         break
                     datas = b''

pymycobot/common.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/error.py

@@ -162,11 +162,11 @@ def get_base_version(self):
     def go_straight(self, speed=0.2):
         """
         Forward control
-        :param speed: speed (float, optional): Movement speed. Defaults to 0.2. range 0 ~ 1
+        :param speed: speed (float, optional): Movement speed. Defaults to 0.2. range 0 ~ 0.5 m/s
         :return: None
         """
-        if speed < 0 or speed > 1:
-            raise Exception("The movement speed range is 0~1, but the received value is {}".format(speed))
+        if speed < 0 or speed > 0.5:
+            raise Exception("The movement speed range is 0~0.5, but the received value is {}".format(speed))
         self.Send_Data[0] = ProtocolCode.header
         self.Send_Data[1] = 0
         self.Send_Data[2] = 0
@@ -195,11 +195,11 @@ def go_straight(self, speed=0.2):
     def go_back(self, speed=-0.2):
         """
         Back control
-        :param speed: speed (float, optional): Movement speed. Defaults to 0.25. range -1 ~ 0
+        :param speed: speed (float, optional): Movement speed. Defaults to -0.2. range -0.5 ~ 0 m/s
         :return: None
         """
-        if not -1 <= speed <= 0:
-            raise Exception("The movement speed range is -1~0, but the received value is {}".format(speed))
+        if not -0.5 <= speed <= 0:
+            raise Exception("The movement speed range is -0.5~0, but the received value is {}".format(speed))
         self.Send_Data = [0] * 11
         self.Send_Data[0] = ProtocolCode.header
         self.Send_Data[1] = 0
@@ -231,12 +231,12 @@ def go_back(self, speed=-0.2):
     def turn_left(self, speed=0.2):
         """
         Left turn control
-        :param speed: speed (float, optional): Movement speed. Defaults to 0.2. range 0 ~ 1
+        :param speed: speed (float, optional): Movement speed. Defaults to 0.2. range 0 ~ 0.5 m/s
         :return: None
         """
 
-        if speed < 0 or speed > 1:
-            raise Exception("The movement speed range is 0~1, but the received value is {}".format(speed))
+        if speed < 0 or speed > 0.5:
+            raise Exception("The movement speed range is 0~5, but the received value is {}".format(speed))
         self.Send_Data = [0] * 11
         self.Send_Data[0] = ProtocolCode.header
         self.Send_Data[1] = 0
@@ -266,11 +266,11 @@ def turn_left(self, speed=0.2):
     def turn_right(self, speed=-0.2):
         """
         Right turn control
-        :param speed: speed (float, optional): Movement speed. Defaults to -0.2. range -1 ~ 0
+        :param speed: speed (float, optional): Movement speed. Defaults to -0.2. range -0.5 ~ 0 m/s
         :return: None
         """
-        if not -1 <= speed <= 0:
-            raise Exception("The movement speed range is -1~0, but the received value is {}".format(speed))
+        if not -0.5 <= speed <= 0:
+            raise Exception("The movement speed range is -0.5~0, but the received value is {}".format(speed))
         self.Send_Data = [0] * 11
 
         self.Send_Data[0] = ProtocolCode.header

pymycobot/mercurychassis_api.py

@@ -5,6 +5,8 @@
 import sys
 import logging
 import threading
+import time
+
 from pymycobot.common import ProtocolCode, write, read, DataProcessor
 from pymycobot.error import calibration_parameters
 from pymycobot.log import setup_logging
@@ -52,7 +54,7 @@ def _mesg(self, genre, *args, **kwargs):
             **kwargs: support `has_reply`
                 has_reply: Whether there is a return value to accept.
         """
-
+        time.sleep(0.01)
         real_command, has_reply = super(MyArmAPI, self)._mesg(genre, *args, **kwargs)
         command = self._flatten(real_command)
         with self.lock:
@@ -96,6 +98,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 +119,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 +148,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 +175,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,15 +276,17 @@ 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
 
         Args:
             servo_id (int): 0-254
         """
-        return self._mesg(ProtocolCode.SERVO_RESTORE, servo_id, has_reply=True)
+        return self._mesg(ProtocolCode.GET_SERVO_P, servo_id, has_reply=True)
 
     def set_servo_d(self, servo_id, data):
         """Sets the proportional factor for the position ring D of the specified servo motor
@@ -292,6 +295,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 +314,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 +329,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 +348,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):

pymycobot/myarm_api.py

@@ -744,8 +744,9 @@ def start_robot(self, power_on_only=False):
             return False
 
         for i in range(6):
-            self._servo_init_can_output(Joint(i))
-            self._servo_clear_encoder_error(Joint(i))
+            if self.get_joint_error_mask(Joint(i)) & (1 << 16):
+                self._servo_init_can_output(Joint(i))
+                self._servo_clear_encoder_error(Joint(i))
 
         os.system("halcmd setp or2.0.in1 1")
         return True