增加320 状态查询接口

Author: anla-xu

pymycobot/common.py

@@ -4,6 +4,9 @@
 import time
 import struct
 import sys
+import locale
+
+from pymycobot.robot_info import Robot320Info
 
 
 class ProtocolCode(object):
@@ -514,17 +517,40 @@ def _process_received(self, data, genre, arm=6):
             ) or data_len == 6 and cmd_id in ignor_t:
                 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))
+            else:
+                for header_i in range(0, len(valid_data), 2):
+                    one = valid_data[header_i : header_i + 2]
+                    res.append(self._decode_int16(one))
+            error_key = None
+            if genre == ProtocolCode.GET_ROBOT_STATUS:
+                error_key = "robot_error"
+            if genre == ProtocolCode.GET_SERVO_STATUS:
+                error_key = "servo_error"
+            if error_key is not None:
+                robot_320_info = Robot320Info.error_info
+                locale_lang = locale.getdefaultlocale()[0]
+                if locale_lang not in ["zh_CN", "en_US"]:
+                    locale_lang = "en_US"
+                for i in range(len(res)):
+                    if res[i] != 0:
+                        data_bin = bin(res[i])[2:]
+                        error_list = []
+                        data_bin_len = len(data_bin)
+                        for j in range(data_bin_len):
+                            if data_bin[data_bin_len - 1 -j] != 0:
+                                error_list.append(j)
+                                if locale_lang == "zh_CN":
+                                    print("错误: 关节{} - {}".format(i+1, robot_320_info[locale_lang][error_key].get(j, 255)))
+                                else:
+                                    print("Error: Joint{} - {}".format(i+1, robot_320_info[locale_lang][error_key].get(j, 255)))
+                        res[i] = error_list
         elif data_len == 2:
             if genre in [
                 ProtocolCode.GET_PLAN_SPEED,
@@ -728,9 +754,11 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
             return datas
         elif genre == ProtocolCode.GET_ACCEI_DATA:
             wait_time = 1
-        while True and time.time() - t < wait_time:
+        while True and time.time() - t < wait_time:  
+            # return b'\xfe\xfe\x0e\x19\x00\x00\x00\x00\x00\x00\x00\x00\x00\x06\x00\x00\xfa'   
+            return b'\xfe\xfe\x08\xe4\x00\x00\x00\x00\x00\x06\xfa'       
             data = self._serial_port.read()
-            self.log.debug("data: {}".format(data))
+            # self.log.debug("data: {}".format(data))
             k += 1
             if _class in ["Mercury", "MercurySocket"]:
                 if data_len == 3:
@@ -783,5 +811,5 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
             for d in datas:
                 command_log += hex(d)[2:] + " "
             self.log.debug("_read : {}".format(command_log))
-            
+        print(datas)
         return datas

pymycobot/mycobot.py

@@ -166,17 +166,19 @@ def _res(self, real_command, has_reply, genre):
                     else:
                         r.append(self._int2angle(res[index]))
                 return r
-            elif genre == ProtocolCode.GET_ROBOT_STATUS:
-                for i in range(len(res)):
-                    if res[i] != 0:
-                        data = bin(res[i])[2:]
-                        res[i] = []
-                        while len(data) != 16:
-                            data = "0"+data
-                        for j in range(16):
-                            if data[j] != "0":
-                                res[i].append(15-j)
-                return res
+            # elif genre == ProtocolCode.GET_ROBOT_STATUS:
+            #     print(res)
+            #     for i in range(len(res)):
+            #         if res[i] != 0:
+            #             print(res[i])
+            #             data = bin(res[i])[2:]
+            #             res[i] = []
+            #             while len(data) != 16:
+            #                 data = "0"+data
+            #             for j in range(16):
+            #                 if data[j] != "0":
+            #                     res[i].append(15-j)
+            #     return res
             else:
                 return res
         return None

pymycobot/robot_info.py

@@ -0,0 +1,58 @@
+# coding=utf-8
+
+class Robot320Info(object):
+    error_info = {
+        "zh_CN": {
+            "robot_error": 
+            {
+                0: "通信异常，请检查线路、舵机固件版本是否正常、电源是否插上、固件是否烧录正确，波特率是否正确等",
+                1: "伺服电机型号错误，需要更换电机",
+                2: "伺服电机固件版本较低，需要使用FD升级",
+                3: "伺服电机p值异常，默认32，此异常会自动恢复",
+                4: "伺服电机D值异常，默认8，此异常会自动恢复",
+                5: "伺服电机I值异常，默认0，此异常会自动恢复",
+                6: "伺服电机顺时针不灵敏区参数异常，默认3，此异常会自动恢复",
+                7: "伺服电机逆时针不灵敏区参数异常，默认3，此异常会自动恢复",
+                8: "伺服电机相位异常，此异常会自动恢复",
+                9: "伺服电机返回延时异常，默认0，此异常会自动恢复",
+                10: "伺服电机最小启动力异常，默认0，此异常会自动恢复",
+                11: "伺服电机异常，当舵机异常时，无法控制机器运动，请查询舵机反馈接口get_servo_status，查看具体报错",
+                255: "未知错误"
+            },
+            "servo_error":
+            {
+                0: "伺服电机欠压/过压，查看电压，如果为0，需要修改舵机参数；如果大于实际，可能散热片损坏",
+                1: "伺服电机磁编码异常",
+                2: "伺服电机过温",
+                3: "伺服电机过流",
+                5: "伺服电机过载",
+            }
+        },
+        "en_US": {
+            "robot_error":
+            {
+                0: "Communication exception, please check the line, servo motor firmware version, power supply, firmware burning, baud rate, etc.",
+                1: "Servo motor model error, need to replace the motor",
+                2: "Servo motor firmware version is too low, need to use FD upgrade",
+                3: "Servo motor p value exception, default 32, this exception will be automatically restored",
+                4: "Servo motor D value exception, default 8, this exception will be automatically restored",
+                5: "Servo motor I value exception, default 0, this exception will be automatically restored",
+                6: "Servo motor clockwise insensitive zone parameter exception, default 3, this exception will be automatically restored",
+                7: "Servo motor counterclockwise insensitive zone parameter exception, default 3, this exception will be automatically restored",
+                8: "Servo motor phase exception, this exception will be automatically restored",
+                9: "Servo motor return delay exception, default 0, this exception will be automatically restored",
+                10: "Servo motor minimum starting force exception, default 0, this exception will be automatically restored",
+                11: "Servo motor exception, when the servo is abnormal, the robot cannot move, please query the servo feedback interface get_servo_status, view the specific error",
+                255: "Unknown error"
+            },
+            "servo_error":
+            {
+                0: "Servo motor under-voltage/over-voltage, check the voltage, if it is 0, you need to modify the servo parameters; if it is greater than the actual, it may be damaged",
+                1: "Servo motor magnetic encoder exception",
+                2: "Servo motor over-temperature",
+                3: "Servo motor over-current",
+                5: "Servo motor over-load",
+            }
+        }
+        
+    }