Merge branch 'mycobot_280_refactor' of github.com:elephantrobotics/pymycobot into myhand_gripper

Author: wangWking

pymycobot/common.py

@@ -801,7 +801,7 @@ def write(self, command, method=None):
         self._serial_port.flush()
 
 
-def read(self, genre, method=None, command=None, _class=None, timeout=None):
+def read(self, genre, method=None, command=None, _class=None, timeout=None, real_command=None):
     datas = b""
     data_len = -1
     k = 0
@@ -833,9 +833,16 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
         wait_time = 90
     elif genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
         wait_time = 0.05
+    if real_command:
+        if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+            if real_command[6] == 13:
+                wait_time = 3
     data = b""
-
     if method is not None:
+        if real_command:
+            if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+                if real_command[6] == 13:
+                    wait_time = 3
         if genre == 177:
             while True:
                 data = self.sock.recv(1024)

pymycobot/error.py

@@ -349,6 +349,14 @@ def calibration_parameters(**kwargs):
             "coords_min": [-310, -310, -140, -180, -180, -180],
             "coords_max": [310, 310, 480, 180, 180, 180]
         },
+        "MyArmM": {
+            "joint_id": [1, 2, 3, 4, 5, 6, 7],
+            "servo_id": [1, 2, 3, 4, 5, 6, 7, 8],
+            "angles_min": [-170, -83, -90, -155, -91, -153, -118],
+            "angles_max": [170, 83, 84, 153, 88, 153, 2],
+            "encoders_min": [137, 1163, 1035, 1013, 248, 979, 220, 706],
+            "encoders_max": [4004, 2945, 3079, 3026, 3724, 2994, 3704, 2048]
+        },
         "MyArmSocket": {
             "id": [1, 2, 3, 4, 5, 6, 7, 8],
             "angles_min": [-160, -70, -170, -113, -170, -115, -180],
@@ -787,7 +795,7 @@ def calibration_parameters(**kwargs):
                     raise MyCobot320DataException(
                         "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
                                                                                                           gripper_id))
-                if torque_value < 100 or torque_value > 300:
+                if torque_value < 0 or torque_value > 100:
                     raise MyCobot320DataException(
                         "The range of 'torque_value' in {} is 100 ~ 300, but the received value is {}".format(parameter,
                                                                                                               torque_value))
@@ -1284,52 +1292,40 @@ def calibration_parameters(**kwargs):
             value = kwargs[parameter]
             value_type = type(value)
             if parameter in ("servo_id", "joint_id") and value not in limit_info[parameter]:
-                raise MyArmDataException(
-                    "The id not right, should be in {0}, but received {1}.".format(limit_info[parameter], value)
-                )
+                raise ValueError(f"The {parameter} not right, should be in {limit_info[parameter]}, but received {value}.")
             elif parameter == 'angle':
                 i = kwargs['joint_id'] - 1
                 min_angle = limit_info["angles_min"][i]
                 max_angle = limit_info["angles_max"][i]
                 if value < min_angle or value > max_angle:
-                    raise MyArmDataException(
-                        f"angle value not right, should be {min_angle} ~ {max_angle}, but received {value}"
-                    )
+                    raise ValueError(f"angle value not right, should be {min_angle} ~ {max_angle}, but received {value}")
             elif parameter == 'angles':
                 for i, v in enumerate(value):
                     min_angle = limit_info["angles_min"][i]
                     max_angle = limit_info["angles_max"][i]
                     if v < min_angle or v > max_angle:
-                        raise MyArmDataException(
-                            f"angle value not right, should be {min_angle} ~ {max_angle}, but received {v}"
-                        )
+                        raise ValueError(f"angle value not right, should be {min_angle} ~ {max_angle}, but received {v}")
             elif parameter == 'encoder':
                 i = kwargs['servo_id'] - 1
                 max_encoder = limit_info["encoders_max"][i]
                 min_encoder = limit_info["encoders_min"][i]
                 if value < min_encoder or value > max_encoder:
-                    raise MyArmDataException(
-                        f"angle value not right, should be {min_encoder} ~ {max_encoder}, but received {value}"
-                    )
+                    raise ValueError(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 v < min_encoder or v > max_encoder:
-                        raise MyArmDataException(
-                            f"encoder value not right, should be {min_encoder} ~ {max_encoder}, but received {v}"
-                        )
+                        raise ValueError(f"encoder value not right, should be {min_encoder} ~ {max_encoder}, but received {v}")
             elif parameter == "speed":
-                check_value_type(parameter, value_type, MyArmDataException, int)
+                check_value_type(parameter, value_type, TypeError, int)
                 if not 1 <= value <= 100:
-                    raise MyArmDataException(f"speed value not right, should be 1 ~ 100, the received speed is {value}")
+                    raise ValueError(f"speed value not right, should be 1 ~ 100, the received speed is {value}")
             elif parameter == "speeds":
                 assert len(value) == 8, "The length of `speeds` must be 8."
                 for i, s in enumerate(value):
                     if not 1 <= s <= 100:
-                        raise MyArmDataException(
-                            f"speed value not right, should be 1 ~ 100, the received speed is {value}"
-                        )
+                        raise ValueError(f"speed value not right, should be 1 ~ 100, the received speed is {value}")
             elif parameter == "servo_addr":
                 if value in (0, 1, 2, 3, 4):
-                    raise MyArmDataException("addr 0-4 cannot be modified")
+                    raise ValueError("addr 0-4 cannot be modified")

pymycobot/myarm_api.py

@@ -67,7 +67,7 @@ def _mesg(self, genre, *args, **kwargs):
             data = self._read(genre, command=command, timeout=timeout, _class=self.__class__.__name__)
             res = self._process_received(data, genre, arm=8)
 
-            if len(res) == 0:
+            if not res:
                 return None
 
             return_single_genres = [

pymycobot/myarmm.py

@@ -108,15 +108,11 @@ def get_robot_power_status(self):
         return self._mesg(ProtocolCode.IS_POWER_ON, has_reply=True)
 
     def set_robot_power_on(self):
-        """Set the robot to power on
-        Returns: (int) 1
-        """
+        """Set the robot to power on"""
         return self._mesg(ProtocolCode.POWER_ON, has_reply=True)
 
     def set_robot_power_off(self):
-        """Set the robot to power off
-        Returns: (int) 1
-        """
+        """Set the robot to power off"""
         return self._mesg(ProtocolCode.POWER_OFF, has_reply=True)
 
     def clear_robot_err(self):

pymycobot/mycobot320.py

@@ -154,7 +154,7 @@ def _res(self, real_command, has_reply, genre):
             try_count = 0
             while try_count < 3:
                 self._write(self._flatten(real_command))
-                data = self._read(genre)
+                data = self._read(genre, real_command=real_command)
                 if data is not None and data != b'':
                     break
                 try_count += 1
@@ -165,6 +165,11 @@ def _res(self, real_command, has_reply, genre):
         res = self._process_received(data, genre)
         if res is None:
             return None
+        if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+            if res == [0]:
+                self._write(self._flatten(real_command))
+                data = self._read(genre, real_command=real_command)
+                res = self._process_received(data, genre)
         if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [ProtocolCode.GET_BASIC_VERSION,
                                                                                          ProtocolCode.GET_JOINT_MIN_ANGLE,
                                                                                          ProtocolCode.GET_JOINT_MAX_ANGLE,
@@ -201,6 +206,10 @@ def _res(self, real_command, has_reply, genre):
         ]:
             return self._process_single(res)
         elif genre in [ProtocolCode.GET_TOQUE_GRIPPER]:
+            if res[-1] == 255 and res[-2] == 255:
+                self._write(self._flatten(real_command))
+                data = self._read(genre)
+                res = self._process_received(data, genre)
             return self._process_high_low_bytes(res)
         elif genre in [ProtocolCode.GET_ANGLES]:
             return [self._int2angle(angle) for angle in res]

pymycobot/mycobot320socket.py

@@ -145,7 +145,7 @@ def _mesg(self, genre, *args, **kwargs):
                 try_count = 0
                 while try_count < 3:
                     self._write(self._flatten(real_command), "socket")
-                    data = self._read(genre, method='socket')
+                    data = self._read(genre, method='socket', real_command=real_command)
                     if data is not None and data != b'':
                         break
                     try_count += 1
@@ -156,7 +156,12 @@ def _mesg(self, genre, *args, **kwargs):
             res = self._process_received(data, genre)
             if res is None:
                 return None
-            elif res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
+            if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+                if res == [0]:
+                    self._write(self._flatten(real_command))
+                    data = self._read(genre, real_command=real_command)
+                    res = self._process_received(data, genre)
+            if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
                 ProtocolCode.GET_BASIC_VERSION,
                 ProtocolCode.GET_JOINT_MIN_ANGLE,
                 ProtocolCode.GET_JOINT_MAX_ANGLE,