850 lines
27 KiB
Python
850 lines
27 KiB
Python
# -*- 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])
|
||
|
||
# for f in format_list:
|
||
# s_f = []
|
||
# if f == 'f':
|
||
# s_f = [4, 'f']
|
||
# elif f == 'u16':
|
||
# s_f = [2, 'H']
|
||
# elif f == 's16':
|
||
# s_f = [2, 'h']
|
||
# elif f == 'u32':
|
||
# s_f = [4, 'I']
|
||
# elif f == 's32':
|
||
# s_f = [4, 'i']
|
||
# elif f == 'u8':
|
||
# s_f = [1, 'B']
|
||
# elif f == 's8':
|
||
# s_f = [1, 'b']
|
||
# ba = bytearray()
|
||
# if len(s_f) == 2:
|
||
# for i in range(s_f[0]):
|
||
# ba.append(data[p])
|
||
# p = p + 1
|
||
# rdata.append(struct.unpack(s_f[1], ba)[0])
|
||
# else:
|
||
# print('unkown format in format_data(): ' + f)
|
||
# return []
|
||
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])]
|
||
|
||
# for i in range(len(format_list)):
|
||
# f = format_list[i]
|
||
# s_f = []
|
||
# if f == 'f':
|
||
# s_f = [4, 'f']
|
||
# elif f == 'u16':
|
||
# s_f = [2, 'H']
|
||
# elif f == 's16':
|
||
# s_f = [2, 'h']
|
||
# elif f == 'u32':
|
||
# s_f = [4, 'I']
|
||
# elif f == 's32':
|
||
# s_f = [4, 'i']
|
||
# elif f == 'u8':
|
||
# s_f = [1, 'B']
|
||
# elif f == 's8':
|
||
# s_f = [1, 'b']
|
||
# if f != 'f':
|
||
# data[i] = int(data[i])
|
||
# if len(s_f) == 2:
|
||
# bs = struct.pack(s_f[1], data[i])
|
||
# for j in range(s_f[0]):
|
||
# rdata.append(bs[j])
|
||
# else:
|
||
# print('unkown format in format_data(): ' + f)
|
||
# return []
|
||
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
|