Merge pull request #83 from Mrkun5018/main

The packet loss issue is fixed

Author: anla-xu

pymycobot/common.py

@@ -264,6 +264,7 @@ class ProtocolCode(object):
     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
@@ -729,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:
@@ -743,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/myarm_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:
@@ -284,7 +286,7 @@ def get_servo_p(self, servo_id):
         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