add mercury debug api

Author: anla-xu

pymycobot/common.py

@@ -180,6 +180,9 @@ class ProtocolCode(object):
     SET_JOINT_CURRENT = 0x90
     GET_JOINT_CURRENT = 0x91
     SET_CURRENT_STATE = 0x92
+    GET_POS_OVER = 0x94
+    CLEAR_ENCODERS_ERROR = 0x95
+    GET_DOWN_ENCODERS = 0x96
 
     # planning speed
     GET_PLAN_SPEED = 0xD0
@@ -424,6 +427,13 @@ def _process_received(self, data, genre, arm=6):
                 for v in valid_data:
                     res.append(v)
                 return res
+            elif data_len == 8 and arm == 14 and cmd_id == ProtocolCode.GET_DOWN_ENCODERS:
+                i = 0
+                while i < data_len:
+                    byte_value = int.from_bytes(valid_data[i:i+4], byteorder='big', signed=True)
+                    i+=4
+                    res.append(byte_value)
+                return res
             for header_i in range(0, len(valid_data), 2):
                 one = valid_data[header_i : header_i + 2]
                 res.append(self._decode_int16(one))

pymycobot/mercury_api.py

@@ -469,4 +469,17 @@ def pause(self, deceleration=False):
         if deceleration:
             return self._mesg(ProtocolCode.PAUSE, has_reply=True)
         else:
-            return self._mesg(ProtocolCode.PAUSE, 1, has_reply=True)
+            return self._mesg(ProtocolCode.PAUSE, 1, has_reply=True)
+        
+    def get_modified_version(self):
+        return self._mesg(ProtocolCode.ROBOT_VERSION, has_reply=True)
+    
+    def get_pos_over(self):
+        return self._mesg(ProtocolCode.GET_POS_OVER, has_reply=True)
+    
+    def clear_encoders_error(self):
+        return self._mesg(ProtocolCode.CLEAR_ENCODERS_ERROR)
+    
+    def get_down_encoders(self):
+        return self._mesg(ProtocolCode.GET_DOWN_ENCODERS, has_reply=True)
+