# -*- coding=utf-8 -*- """ 大然机器人-小米CyberGear微电机python控制库 适用平台：windows或linux平台 库版本号：v1.0 测试主控版本：windows 10 python 3.7 测试人员：唐昭 测试时间：2023.08.31 备注：该库函数目前仅适用于大然科技的USB转CAN模块 淘宝链接： https://item.taobao.com/item.htm?spm=a1z10.5-c-s.w4002-22325405943.12.31aac3b4jKqqpc&id=705379289168 """ import time import serial import struct import numpy from math import pi from typing import Union P_MIN = -12.5 P_MAX = 12.5 V_MIN = -30.0 V_MAX = 30.0 KP_MIN = 0.0 KP_MAX = 500.0 KD_MIN = 0.0 KD_MAX = 5.0 T_MIN = -12.0 T_MAX = 12.0 I_MAX = 27 TORQUE_CONSTANT = T_MAX / I_MAX RAD_DEG = 180 / pi DEG_RAD = pi / 180 RAD_S_R_MIN = 30 / pi R_MIN_RAD_S = pi / 30 uart = serial.Serial('COM325', 115200) # 在 windows 下控制电机测试，相应的输入连接的COM口和波特率 # uart = serial.Serial('/dev/ttyAMA0', 115200) # 在树莓派（raspbian）下控制电机测试，相应的输入连接的串口和波特率 # linux系统下 # result = os.popen("sudo ls -l /dev/ttyACM*").read() # 在 jetson nano（ubuntu） 下控制电机测试，相应的输入连接的串口和波特率 # com = result.split()[-1] # sudo chmod 666 /dev/ttyACM* 有时会报错，显示无曲线打开串口，这时需运行左侧命令 # os.system("sudo chmod 777 " + com) # os.system("sudo fuser -k " + com) # uart = serial.Serial(com, 115200, timeout=0.5) ERROR_FLAG = "状态正常" READ_FLAG = 0 # 读取结果标志位 MOTOR_NUM = 64 motor_state = numpy.zeros((MOTOR_NUM, 6)) MCU_ID = [] # 电机主控MCU芯片序列号 """ 电机状态二维数组 通过motor_state[id_num-1] 获取电机id_num的实时返回状态 [angle, speed, torque, motor_temp, axis_error, mode_status] 单位分别为degree，r/min,Nm，三个变量值均指的是电机输出轴 其中 motor_state[id_num-1][0] 表示id_num号电机的角度 motor_state[id_num-1][1] 表示id_num号电机的速度 motor_state[id_num-1][2] 表示id_num号电机的输出扭矩 """ """ 内部辅助函数，用户无需使用 """ # 串口发送函数 def write_data(data=[]): global uart if uart.inWaiting() > 0: uart.read(uart.inWaiting()) try: result = uart.write(data) # 写数据 return result except Exception as e: print("---error in write_data--：", e) print("重启串口") uart.close() uart.open() result = uart.write(data) # 写数据 return result # 串口接收函数 def read_data(num=16): global READ_FLAG READ_FLAG = -1 byte_list = [] i = 1000 # 经过测试，发现正常接收16位耗时大概为500 # 该函数中的两处time.sleep(0.001)会影响CAN接收速度，如果要提高CAN读取速度，可以将这两行注释掉，并将上面的i改成一个较大的值，比如5000 while uart.inWaiting() == 0 and i > 0: # To do: i -= 1 # time.sleep(0.001) # time.sleep(0.001) while uart.inWaiting() > 0: byte_list.append(list(uart.read(1))[0]) if len(byte_list) == num: READ_FLAG = 1 return byte_list else: print("Received data error in read_data(): " + str(byte_list)) READ_FLAG = -1 return # USB转CAN模块包模式：CAN报文->串行帧 def can_to_uart(data=[], rtr=0): udata = [0xAA, 1, 0, 0x08, 0, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00] if len(data) == 13 and data[0] == 0x08: for i in range(12): udata[4 + i] = data[i + 1] return udata else: return [] # USB转CAN模块包模式：串行帧->CAN报文 def uart_to_can(data=[]): global READ_FLAG cdata = [0x08, 0, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00] if len(data) == 16 and data[3] == 0x08: for i in range(12): cdata[1 + i] = data[i + 4] return cdata else: READ_FLAG = -1 return [] # CAN发送函数 def send_command(id_num=1, cmd_mode=0, cmd_data=None, data=None, rtr=0): if data is None: data = [] if cmd_data is None: cmd_data = [] cdata = [0x08, 0, 0, 0, 0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00] cdata[1] = cmd_mode cdata[2] = cmd_data[1] cdata[3] = cmd_data[0] # master_ID cdata[4] = int(id_num) for i in range(8): cdata[5 + i] = data[i] write_data(data=can_to_uart(data=cdata, rtr=rtr)) # CAN接收函数 def receive_data(): udata = read_data(16) if READ_FLAG == 1: cdata = uart_to_can(data=udata) if cdata[1] == 21: dump_error(cdata, True) return cdata[:] data_map_dict = { 'f': [4, 'f'], 'u16': [2, 'H'], 's16': [2, 'h'], 'u32': [4, 'I'], 's32': [4, 'i'], 'u8': [1, 'B'], 's8': [1, 'b'] } # 数据格式转换函数，decode是将二进制(bytes)转化成人看的懂得数据，encode反之 def format_data(data=[], format="f f", type='decode'): # print(data) format_list = format.split() rdata = [] if type == 'decode': p = 0 for value_type in format_list: s_f = data_map_dict.get(value_type, None) if s_f is None: print('unkown format in format_data(): ' + value_type) return [] ba = bytearray() for i in range(s_f[0]): ba.append(data[p]) p = p + 1 rdata.append(struct.unpack(s_f[1], ba)[0]) return rdata elif type == 'encode' and len(format_list) == len(data): for value_type in format_list: s_f = data_map_dict.get(value_type, None) if s_f is None: print('unkown format in format_data(): ' + value_type) return [] bs = struct.pack(s_f[1], data[0]) rdata += [bs[i] for i in range(s_f[0])] if len(rdata) < 4: for i in range(4 - len(rdata)): rdata.append(0x00) return rdata def float_to_uint(x, x_min, x_max, bits): span = x_max - x_min offset = x_min if x > x_max: x = x_max elif x < x_min: x = x_min return int(((x - offset) * ((1 << bits) - 1) / span)) def uint_to_float(x, x_min, x_max, bits): span = (1 << bits) - 1 offset = x_max - x_min if x > span: x = span elif x < 0: x = 0 return offset * x / span + x_min # 故障反馈帧解析 def dump_error(rdata=[]): """打印电机错误编号(电机自动返回错误信息) 读取电机错误信息编码，如果错误编码为0，表示无异常。如果错误编码不为0，则表示存在故障。 Args: rdata: 故障反馈帧内容 Returns: 无 Raises: 无 """ # [0x08 mode cmd_data[1] cmd_data[0] id_num data0 data1 data2 data3 data4 data5 data6 data7] global ERROR_FLAG mode = rdata[1] cmd_data = rdata[2:4] id_num = rdata[4] data = rdata[5:] if READ_FLAG == 1 and mode == 21: print("电机CAN_ID" + str(id_num)) print("主CAN_ID" + str(cmd_data[0])) if data[0] & 0x3F or data[5] & (0x01 << 0): ERROR_FLAG = "状态异常：" if data[0] & (0x01 << 0): ERROR_FLAG = ERROR_FLAG + '\n' + "电机过温故障，默认80度" if data[0] & (0x01 << 1): ERROR_FLAG = ERROR_FLAG + '\n' + "驱动芯⽚故障" if data[0] & (0x01 << 2): ERROR_FLAG = ERROR_FLAG + '\n' + "⽋压故障" if data[0] & (0x01 << 3): ERROR_FLAG = ERROR_FLAG + '\n' + "过压故障" if data[0] & (0x01 << 4): ERROR_FLAG = ERROR_FLAG + '\n' + "B相电流采样过流" if data[0] & (0x01 << 5): ERROR_FLAG = ERROR_FLAG + '\n' + "C相电流采样过流" if data[0] & (0x01 << 7): ERROR_FLAG = ERROR_FLAG + '\n' + "编码器未标定" if data[1] & 0xFF: ERROR_FLAG = ERROR_FLAG + '\n' + "过载故障" if data[2] & (0x01 << 0): ERROR_FLAG = ERROR_FLAG + '\n' + "A相电流采样过流" if data[4] & (0x01 << 0): ERROR_FLAG = ERROR_FLAG + '\n' + "电机过温预警，默认75度" else: ERROR_FLAG = "状态正常" print(ERROR_FLAG) return ERROR_FLAG # 电机应答反馈帧 def reply_state(id_num=1): """ @brief 电机运动控制指令状态实时返回参数 通过该函数读取电机运动控制指令实时返回的电机状态参数[angle,speed,torque,temp,error_flag,mode_status]，单位分别为degree，r/min,Nm，三个变量值均指的是电机输出轴 其中motor_state[id_num-1][0]表示id_num号电机的角度，motor_state[id_num-1][1]表示id_num号电机的速度，motor_state[id_num-1][2]表示id_num号电机的输出扭矩 @param id_num 需要读取的电机编号 注意该指令id_num不能为0 """ global READ_FLAG, ERROR_FLAG, motor_state try: if id_num <= MOTOR_NUM: READ_FLAG = 0 rdata = receive_data() # [0x08 mode cmd_data[1] cmd_data[0] id_num data0 data1 data2 data3 data4 data5 data6 data7] if READ_FLAG == 1 and rdata[1] == 2: cmd_data = [rdata[3], rdata[2]] id_num = rdata[3] # 电机返回的数据帧中电机CAN_ID位于Bit8~Bit15 data = rdata[5:] motor_state[id_num - 1][0] = uint_to_float((data[0] << 8) + data[1], P_MIN, P_MAX, 16) * RAD_DEG motor_state[id_num - 1][1] = uint_to_float((data[2] << 8) + data[3], V_MIN, V_MAX, 16) * RAD_S_R_MIN motor_state[id_num - 1][2] = uint_to_float((data[4] << 8) + data[5], T_MIN, T_MAX, 16) motor_state[id_num - 1][3] = ((data[6] << 8) + data[7]) * 0.1 if cmd_data[1] & 0x3F: motor_state[id_num - 1][4] = 1 ERROR_FLAG = '状态异常：' if cmd_data[1] & (0x01 << 0): ERROR_FLAG = ERROR_FLAG + '\n' + "⽋压故障" if cmd_data[1] & (0x01 << 1): ERROR_FLAG = ERROR_FLAG + '\n' + "过流" if cmd_data[1] & (0x01 << 2): ERROR_FLAG = ERROR_FLAG + '\n' + "过温" if cmd_data[1] & (0x01 << 3): ERROR_FLAG = ERROR_FLAG + '\n' + "磁编码故障" if cmd_data[1] & (0x01 << 4): ERROR_FLAG = ERROR_FLAG + '\n' + "HALL编码故障" if cmd_data[1] & (0x01 << 5): ERROR_FLAG = ERROR_FLAG + '\n' + "未标定" print(ERROR_FLAG) else: ERROR_FLAG = '状态正常' motor_state[id_num - 1][4] = 0 mode_status = (cmd_data[1] >> 6) & 0x03 # if mode_status == 0: # print("当前模式状态为 Reset 模式") # elif mode_status == 1: # print("当前模式状态为 Cali 模式") # elif mode_status == 2: # print("当前模式状态为 Motor 模式") motor_state[id_num - 1][5] = mode_status except Exception as e: print("---error in reply_state--：", e) # 设置运动模式 def set_mode(id_num=1, mode=0): """设置电机模式。 设置电机进入不同的控制模式。 Args: id_num: 需要设置的电机ID编号。 mode: 电机模式编号 mode = 0: 运控模式 mode = 1: 位置模式 mode = 2: 速度模式 mode = 3: 电流模式 Returns: 无 Raises: 无 """ write_property(id_num=id_num, index=0x7005, value=mode, data_type='u8') # 电机使能 def motor_enable(id_num=1): """电机使能运行函数 电机使能运行。 Args: id_num: 需要使能运行的电机ID编号 Returns: 无 Raises: 无 """ master_id = 0 cmd_data = [0] * 2 cmd_data[0] = master_id & 0xFF tx_data = [0] * 8 for i in range(8): tx_data[i] = 0x00 send_command(id_num=id_num, cmd_mode=3, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 reply_state(id_num=id_num) """ 电机控制函数，用户使用 """ # 位置控制 def set_angle(id_num=1, angle=0, speed=0, limit_cur=27): """电机角度控制函数。 控制指定电机编号的电机按照指定的速度转动到指定的角度。 Args: id_num: 需要设置的电机ID编号 angle: 电机转动角度(度) speed: 最大速度限制或前馈速度（0~300r/min） limit_cur:电流限制（0-27A） """ motor_enable(id_num=id_num) set_mode(id_num=id_num, mode=1) write_property(id_num=id_num, index=0x7018, value=limit_cur, data_type='f') write_property(id_num=id_num, index=0x7017, value=speed * R_MIN_RAD_S, data_type='f') write_property(id_num=id_num, index=0x7016, value=angle * DEG_RAD, data_type='f') # 速度控制 def set_speed(id_num=1, speed=10, limit_cur=27): """电机速度控制函数。 控制指定电机编号的电机按照指定的速度连续整周转动。 Args: id_num: 需要设置的电机ID编号 speed: 目标速度（-300~300r/min） limit_cur:电流限制（0-27A） """ motor_enable(id_num=id_num) set_mode(id_num=id_num, mode=2) write_property(id_num=id_num, index=0x7018, value=limit_cur, data_type='f') write_property(id_num=id_num, index=0x700A, value=speed * R_MIN_RAD_S, data_type='f') # 扭矩（电流）控制 def set_torque(id_num=1, torque=0.1): """电机力矩（电流）闭环控制函数。 控制指定电机编号的电机输出指定的扭矩（Nm） Args: id_num: 需要设置的电机ID编号 torque: 电机输出（0~12Nm） """ motor_enable(id_num=id_num) set_mode(id_num=id_num, mode=3) write_property(id_num=id_num, index=0x7006, value=torque / TORQUE_CONSTANT, data_type='f') # 阻抗控制 def impedance_control(id_num=1, pos=0, vel=0, tff=0, kp=0, kd=0): """运控模式电机控制指令函数。 运控模式电机控制指令， ⽤来向电机发送控制指令 Args: id_num: 需要设置的电机ID编号 pos: 电机目标角度（度） vel: 电机目标速度（r/min） tff: 前馈扭矩（Nm) kp: 刚度系数(rad/Nm) kd: 阻尼系数(rad/s/Nm) Returns: 无 Raises: "---error in impedance_control---" """ try: motor_enable(id_num=id_num) set_mode(id_num=id_num, mode=0) cmd_data = [0] * 2 cmd_data[0] = (float_to_uint(tff, T_MIN, T_MAX, 16)) & 0xFF # 阻抗控制里都是高位在前，低位在后，不满足小端排序 cmd_data[1] = ((float_to_uint(tff, T_MIN, T_MAX, 16)) >> 8) & 0xFF tx_data = [0] * 8 tx_data[0] = (float_to_uint(pos * DEG_RAD, P_MIN, P_MAX, 16) >> 8) & 0xFF tx_data[1] = (float_to_uint(pos * DEG_RAD, P_MIN, P_MAX, 16)) & 0xFF tx_data[2] = (float_to_uint(vel * R_MIN_RAD_S, V_MIN, V_MAX, 16) >> 8) & 0xFF tx_data[3] = (float_to_uint(vel * R_MIN_RAD_S, V_MIN, V_MAX, 16)) & 0xFF tx_data[4] = (float_to_uint(kp, KP_MIN, KP_MAX, 16) >> 8) & 0xFF tx_data[5] = (float_to_uint(kp, KP_MIN, KP_MAX, 16)) & 0xFF tx_data[6] = (float_to_uint(kd, KD_MIN, KD_MAX, 16) >> 8) & 0xFF tx_data[7] = (float_to_uint(kd, KD_MIN, KD_MAX, 16)) & 0xFF send_command(id_num=id_num, cmd_mode=1, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 reply_state(id_num=id_num) except Exception as e: print("---error in impedance_control--：", e) # 电机停止运行（急停） def motor_estop(id_num=1): """停止运行函数 控制电机停止运行。 Args: id_num: 需要停止运行的电机ID编号 Returns: 无 Raises: 无 """ master_id = 0 cmd_data = [0] * 2 cmd_data[0] = master_id & 0xFF tx_data = [0] * 8 for i in range(8): tx_data[i] = 0x00 send_command(id_num=id_num, cmd_mode=4, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 reply_state(id_num=id_num) # 设置零点 def set_zero_position(id_num=1): """设置电机零点位置函数 设置电机机械零点位置，会把当前电机位置设为机械零位 Args: id_num: 需要设置的电机ID编号 Returns: 无 Raises: 无 """ mode_status = motor_state[id_num - 1][5] motor_estop(id_num=id_num) master_id = 0 cmd_data = [0] * 2 cmd_data[0] = master_id & 0xFF tx_data = [0] * 8 tx_data[0] = 0x01 send_command(id_num=id_num, cmd_mode=6, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送，不能用远程帧 reply_state(id_num=id_num) if mode_status == 2: motor_enable(id_num=id_num) # 清除错误 def clear_error(id_num=1): """清除错误函数 一旦电机运行过程中出现任何错误, 如果要恢复正常控制模式，需要用clear_error清除错误 Args: id_num: 需要清除错误标志的电机ID编号 Returns: 无 Raises: 无 """ global ERROR_FLAG ERROR_FLAG = '状态正常' master_id = 0 cmd_data = [0] * 2 cmd_data[0] = master_id & 0xFF tx_data = [0] * 8 tx_data[0] = 0x01 send_command(id_num=id_num, cmd_mode=4, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 reply_state(id_num=id_num) # 设置电机CAN ID def set_id(id_num=1, new_id=1): """设置电机ID号。 改变电机CAN_ID号（掉电保存）,更改当前电机CAN_ID , ⽴即⽣效。 Args: id_num: 需要重新设置编号的电机编号,如果不知道当前电机编号，可以用0广播，但是这时总线上只能连一个电机，否则多个电机会被设置成相同编号 new_id: 预设ID Returns: True: 设置成功 False:设置失败 Raises: 无 """ global MCU_ID motor_estop(id_num=id_num) # 修改ID号必须在电机模式下进行 否则无法设置成功 time.sleep(0.1) # 等待一会，需要保存 get_id(id_num=id_num) master_id = 0 cmd_data = [0] * 2 cmd_data[1] = new_id & 0xFF cmd_data[0] = master_id & 0xFF # 预设置CAN_ID if len(MCU_ID) == 8: tx_data = MCU_ID send_command(id_num=id_num, cmd_mode=7, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 time.sleep(0.1) # 等待一会，需要保存 reply_state(id_num=id_num) return True else: print("set_id to " + str(new_id) + " 失败!") return False # 恢复出厂设置 def init_config(id_num=1): """将电机参数恢复至出厂设置函数 如果想将所有配置参数恢复至出厂设置，可以调用该函数（默认情况下恢复出厂设置后CAN_ID会恢复成127，为了方便使用，该函数自动将其改回原来的id_num） Args: id_num: 需要修改的电机ID编号 Returns: 无 Raises: 无 """ master_id = 0 cmd_data = [0] * 2 cmd_data[1] = 0x03 cmd_data[0] = master_id & 0xFF tx_data = [0] * 8 for i in range(8): tx_data[i] = 0x00 send_command(id_num=id_num, cmd_mode=8, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 print("恢复出厂设置中。。。。。，请稍等3s之后再继续操作") time.sleep(3.0) set_id(127, id_num) print("恢复出厂设置成功！") # 设置参数 def write_property(id_num=1, index=0, data_type='f', value: Union[int, float] = 0): """修改电机属性参数 修改电机属性参数，这里的属性参数为电机控制参数 Args: id_num: 需要修改的电机ID编号 index: 需要读取的参数地址 data_type: 需要写入的参数数据类型：‘f’:float,'u16':uint16,'s16':int16,'u32':uint32,'s32':int32,'u8':uint8,'s8':'int8' value: 对应参数数据。 Returns: 无 Raises: 无 """ master_id = 0 cmd_data = [0] * 2 cmd_data[0] = master_id & 0xFF tx_data = [0] * 8 tx_data[0] = index & 0xFF tx_data[1] = (index >> 8) & 0xFF value = value cmd_mode = 18 if index < 0x7000: cmd_mode = 8 type_list = ['u8', 's8', 'u16', 's16', 'u32', 's32', 'f'] tx_data[2] = type_list.index(data_type) tx_data[4:] = format_data(data=[value], format=data_type, type="encode") send_command(id_num=id_num, cmd_mode=cmd_mode, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 reply_state(id_num=id_num) if cmd_mode == 8: # 永久保存 motor_estop(id_num=id_num) # 永久保存时，也需要先进入待机模式 cmd_data[1] = 0x02 tx_data = [0] * 8 send_command(id_num=id_num, cmd_mode=cmd_mode, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 time.sleep(0.1) # 等待一会，需要保存 reply_state(id_num=id_num) # 读取参数 def read_property(id_num=1, index=0, data_type='f'): """读取电机属性参数 读取电机属性参数，这里的属性参数包括电机状态量及电机控制参数 Args: id_num: 需要修改的电机ID编号 index: 需要读取的参数地址 data_type: 需要读取的参数数据类型：‘f’:float,'u16':uint16,'s16':int16,'u32':uint32,'s32':int32,'u8':uint8,'s8':'int8' Returns: value: 返回对应属性参数的值 Raises: 无 """ master_id = 0 cmd_data = [0] * 2 cmd_data[0] = master_id & 0xFF tx_data = [0] * 8 tx_data[0] = index & 0xFF tx_data[1] = (index >> 8) & 0xFF cmd_mode = 17 if index < 0x7000: cmd_mode = 9 type_list = ['u8', 's8', 'u16', 's16', 'u32', 's32', 'f'] tx_data[2] = type_list.index(data_type) send_command(id_num=id_num, cmd_mode=cmd_mode, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 data = receive_data() if READ_FLAG == 1 and (data[1] == 17 or data[1] == 9): value = format_data(data=data[9:], format=data_type, type="decode") return value[0] # 获取电机ID号（同时会读取电机MCU_ID） def get_id(id_num=1): global MCU_ID master_id = 0xFD cmd_data = [0] * 2 cmd_data[0] = master_id & 0xFF tx_data = [0] * 8 send_command(id_num=id_num, cmd_mode=0, cmd_data=cmd_data, data=tx_data, rtr=0) # 需要用扩展帧（数据帧）进行发送 data = receive_data() if READ_FLAG == 1 and data[1] == 0: MCU_ID = data[5:] return id_num # 读取电机当前角度及速度 def get_state(id_num=1): """读取电机的当前位置和速度 读取电机当前位置和速度列表，单位分别为度（°）和转每分钟(r/min) Args: id_num: 需要读取的电机编号,如果不知道当前电机编号，可以用0广播，但是这时总线上只能连一个电机，否则将报错。 Returns: [pos, vel]: 位置和速度列表 Raises: 无 """ global READ_FLAG, motor_state pos_vel = [0, 0] try: # 调用master_ID写入接口，通过电机应答反馈帧获取实时位置和速度 write_property(id_num=id_num, index=0x7018, value=27, data_type='f') if READ_FLAG == 1 and id_num != 0: pos_vel[0] = round(motor_state[id_num - 1][0], 1) pos_vel[1] = round(motor_state[id_num - 1][1], 1) return pos_vel[:] else: return except Exception as e: print("---error in get_state---：", e) return False # 读取电机当前电压及电流 def get_volcur(id_num=1): """读取电机的当前电压和电流 读取电机当前电压和q轴电流列表，单位分别为伏（V）和安(A) Args: id_num: 需要读取的电机编号,如果不知道当前电机编号，可以用0广播，但是这时总线上只能连一个电机，否则将报错。 Returns: [vol, cur]: 电压和电流列表 Raises: 无 """ global READ_FLAG vol_cur = [0, 0] try: vol_cur[0] = read_property(id_num=id_num, index=0x302b, data_type='f') vol_cur[1] = read_property(id_num=id_num, index=0x301e, data_type='f') if READ_FLAG == 1: vol_cur[0] = round(vol_cur[0], 1) # 将单位从mV换算成V vol_cur[1] = round(vol_cur[1], 2) return vol_cur else: return except Exception as e: print("---error in get_volcur--：", e) return False