632 lines
22 KiB
Python
632 lines
22 KiB
Python
#!/usr/bin/env python
|
||
# -*- coding: utf-8 -*-
|
||
'''
|
||
# Description：电机库
|
||
'''
|
||
|
||
from time import sleep
|
||
import numpy as np
|
||
from enum import IntEnum
|
||
from struct import unpack
|
||
from struct import pack
|
||
|
||
|
||
class Motor:
|
||
def __init__(self, MotorType, SlaveID, MasterID):
|
||
"""
|
||
define Motor object 定义电机对象
|
||
:param MotorType: Motor type 电机类型
|
||
:param SlaveID: CANID 电机ID
|
||
:param MasterID: MasterID 主机ID 建议不要设为0
|
||
"""
|
||
self.Pd = float(0)
|
||
self.Vd = float(0)
|
||
self.state_q = float(0)
|
||
self.state_dq = float(0)
|
||
self.state_tau = float(0)
|
||
self.SlaveID = SlaveID
|
||
self.MasterID = MasterID
|
||
self.MotorType = MotorType
|
||
self.isEnable = False
|
||
self.NowControlMode = Control_Type.MIT
|
||
self.temp_param_dict = {}
|
||
|
||
def recv_data(self, q: float, dq: float, tau: float):
|
||
self.state_q = q
|
||
self.state_dq = dq
|
||
self.state_tau = tau
|
||
|
||
def getPosition(self):
|
||
"""
|
||
get the position of the motor 获取电机位置
|
||
:return: the position of the motor 电机位置
|
||
"""
|
||
return self.state_q
|
||
|
||
def getVelocity(self):
|
||
"""
|
||
get the velocity of the motor 获取电机速度
|
||
:return: the velocity of the motor 电机速度
|
||
"""
|
||
return self.state_dq
|
||
|
||
def getTorque(self):
|
||
"""
|
||
get the torque of the motor 获取电机力矩
|
||
:return: the torque of the motor 电机力矩
|
||
"""
|
||
return self.state_tau
|
||
|
||
def getParam(self, RID):
|
||
"""
|
||
get the parameter of the motor 获取电机内部的参数，需要提前读取
|
||
:param RID: DM_variable 电机参数
|
||
:return: the parameter of the motor 电机参数
|
||
"""
|
||
if RID in self.temp_param_dict:
|
||
return self.temp_param_dict[RID]
|
||
else:
|
||
return None
|
||
|
||
|
||
class MotorControl:
|
||
send_data_frame = np.array(
|
||
[0x55, 0xAA, 0x1e, 0x03, 0x01, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0, 0, 0, 0, 0x00, 0x08, 0x00,
|
||
0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0x00], np.uint8)
|
||
# 4310 4310_48 4340 4340_48
|
||
Limit_Param = [[300, 30, 10], [12.5, 50, 10], [12.5, 8, 28], [12.5, 10, 28],
|
||
# 6006 8006 8009 10010L 10010
|
||
[12.5, 45, 20], [12.5, 45, 40], [12.5, 45, 54], [12.5, 25, 200], [12.5, 20, 200],
|
||
# H3510 DMG6215 DMH6220
|
||
[12.5 , 280 , 1],[12.5 , 45 , 10],[12.5 , 45 , 10]]
|
||
|
||
def __init__(self, serial_device):
|
||
"""
|
||
define MotorControl object 定义电机控制对象
|
||
:param serial_device: serial object 串口对象
|
||
"""
|
||
self.serial_ = serial_device
|
||
self.motors_map = dict()
|
||
self.data_save = bytes() # save data
|
||
if self.serial_.is_open: # open the serial port
|
||
print("Serial port is open")
|
||
serial_device.close()
|
||
self.serial_.open()
|
||
|
||
def controlMIT(self, DM_Motor, kp: float, kd: float, q: float, dq: float, tau: float):
|
||
"""
|
||
MIT Control Mode Function 达妙电机MIT控制模式函数
|
||
:param DM_Motor: Motor object 电机对象
|
||
:param kp: kp
|
||
:param kd: kd
|
||
:param q: position 期望位置
|
||
:param dq: velocity 期望速度
|
||
:param tau: torque 期望力矩
|
||
:return: None
|
||
"""
|
||
if DM_Motor.SlaveID not in self.motors_map:
|
||
print("controlMIT ERROR : Motor ID not found")
|
||
return
|
||
kp_uint = float_to_uint(kp, 0, 500, 12)
|
||
kd_uint = float_to_uint(kd, 0, 5, 12)
|
||
MotorType = DM_Motor.MotorType
|
||
Q_MAX = self.Limit_Param[MotorType][0]
|
||
DQ_MAX = self.Limit_Param[MotorType][1]
|
||
TAU_MAX = self.Limit_Param[MotorType][2]
|
||
q_uint = float_to_uint(q, -Q_MAX, Q_MAX, 16)
|
||
dq_uint = float_to_uint(dq, -DQ_MAX, DQ_MAX, 12)
|
||
tau_uint = float_to_uint(tau, -TAU_MAX, TAU_MAX, 12)
|
||
data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8)
|
||
data_buf[0] = (q_uint >> 8) & 0xff
|
||
data_buf[1] = q_uint & 0xff
|
||
data_buf[2] = dq_uint >> 4
|
||
data_buf[3] = ((dq_uint & 0xf) << 4) | ((kp_uint >> 8) & 0xf)
|
||
data_buf[4] = kp_uint & 0xff
|
||
data_buf[5] = kd_uint >> 4
|
||
data_buf[6] = ((kd_uint & 0xf) << 4) | ((tau_uint >> 8) & 0xf)
|
||
data_buf[7] = tau_uint & 0xff
|
||
self.__send_data(DM_Motor.SlaveID, data_buf)
|
||
self.recv() # receive the data from serial port
|
||
|
||
def control_delay(self, DM_Motor, kp: float, kd: float, q: float, dq: float, tau: float, delay: float):
|
||
"""
|
||
MIT Control Mode Function with delay 达妙电机MIT控制模式函数带延迟
|
||
:param DM_Motor: Motor object 电机对象
|
||
:param kp: kp
|
||
:param kd: kd
|
||
:param q: position 期望位置
|
||
:param dq: velocity 期望速度
|
||
:param tau: torque 期望力矩
|
||
:param delay: delay time 延迟时间 单位秒
|
||
"""
|
||
self.controlMIT(DM_Motor, kp, kd, q, dq, tau)
|
||
sleep(delay)
|
||
|
||
def control_Pos_Vel(self, Motor, P_desired: float, V_desired: float):
|
||
"""
|
||
control the motor in position and velocity control mode 电机位置速度控制模式
|
||
:param Motor: Motor object 电机对象
|
||
:param P_desired: desired position 期望位置
|
||
:param V_desired: desired velocity 期望速度
|
||
:return: None
|
||
"""
|
||
if Motor.SlaveID not in self.motors_map:
|
||
print("Control Pos_Vel Error : Motor ID not found")
|
||
return
|
||
motorid = 0x100 + Motor.SlaveID
|
||
data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8)
|
||
P_desired_uint8s = float_to_uint8s(P_desired)
|
||
V_desired_uint8s = float_to_uint8s(V_desired)
|
||
data_buf[0:4] = P_desired_uint8s
|
||
data_buf[4:8] = V_desired_uint8s
|
||
self.__send_data(motorid, data_buf)
|
||
# time.sleep(0.001)
|
||
self.recv() # receive the data from serial port
|
||
|
||
def control_Vel(self, Motor, Vel_desired):
|
||
"""
|
||
control the motor in velocity control mode 电机速度控制模式
|
||
:param Motor: Motor object 电机对象
|
||
:param Vel_desired: desired velocity 期望速度
|
||
"""
|
||
if Motor.SlaveID not in self.motors_map:
|
||
print("control_VEL ERROR : Motor ID not found")
|
||
return
|
||
motorid = 0x200 + Motor.SlaveID
|
||
data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8)
|
||
Vel_desired_uint8s = float_to_uint8s(Vel_desired)
|
||
data_buf[0:4] = Vel_desired_uint8s
|
||
self.__send_data(motorid, data_buf)
|
||
self.recv() # receive the data from serial port
|
||
|
||
def control_pos_force(self, Motor, Pos_des: float, Vel_des, i_des):
|
||
"""
|
||
control the motor in EMIT control mode 电机力位混合模式
|
||
:param Pos_des: desired position rad 期望位置 单位为rad
|
||
:param Vel_des: desired velocity rad/s 期望速度 为放大100倍
|
||
:param i_des: desired current rang 0-10000 期望电流标幺值放大10000倍
|
||
电流标幺值：实际电流值除以最大电流值，最大电流见上电打印
|
||
"""
|
||
if Motor.SlaveID not in self.motors_map:
|
||
print("control_pos_vel ERROR : Motor ID not found")
|
||
return
|
||
motorid = 0x300 + Motor.SlaveID
|
||
data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8)
|
||
Pos_desired_uint8s = float_to_uint8s(Pos_des)
|
||
data_buf[0:4] = Pos_desired_uint8s
|
||
Vel_uint = np.uint16(Vel_des)
|
||
ides_uint = np.uint16(i_des)
|
||
data_buf[4] = Vel_uint & 0xff
|
||
data_buf[5] = Vel_uint >> 8
|
||
data_buf[6] = ides_uint & 0xff
|
||
data_buf[7] = ides_uint >> 8
|
||
self.__send_data(motorid, data_buf)
|
||
self.recv() # receive the data from serial port
|
||
|
||
def enable(self, Motor):
|
||
"""
|
||
enable motor 使能电机
|
||
最好在上电后几秒后再使能电机
|
||
:param Motor: Motor object 电机对象
|
||
"""
|
||
self.__control_cmd(Motor, np.uint8(0xFC))
|
||
sleep(0.1)
|
||
self.recv() # receive the data from serial port
|
||
|
||
def enable_old(self, Motor ,ControlMode):
|
||
"""
|
||
enable motor old firmware 使能电机旧版本固件，这个是为了旧版本电机固件的兼容性
|
||
可恶的旧版本固件使能需要加上偏移量
|
||
最好在上电后几秒后再使能电机
|
||
:param Motor: Motor object 电机对象
|
||
"""
|
||
data_buf = np.array([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc], np.uint8)
|
||
enable_id = ((int(ControlMode)-1) << 2) + Motor.SlaveID
|
||
self.__send_data(enable_id, data_buf)
|
||
sleep(0.1)
|
||
self.recv() # receive the data from serial port
|
||
|
||
def disable(self, Motor):
|
||
"""
|
||
disable motor 失能电机
|
||
:param Motor: Motor object 电机对象
|
||
"""
|
||
self.__control_cmd(Motor, np.uint8(0xFD))
|
||
sleep(0.01)
|
||
|
||
def set_zero_position(self, Motor):
|
||
"""
|
||
set the zero position of the motor 设置电机0位
|
||
:param Motor: Motor object 电机对象
|
||
"""
|
||
self.__control_cmd(Motor, np.uint8(0xFE))
|
||
sleep(0.1)
|
||
self.recv() # receive the data from serial port
|
||
|
||
def recv(self):
|
||
# 把上次没有解析完的剩下的也放进来
|
||
data_recv = b''.join([self.data_save, self.serial_.read_all()])
|
||
packets = self.__extract_packets(data_recv)
|
||
for packet in packets:
|
||
data = packet[7:15]
|
||
CANID = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3]
|
||
CMD = packet[1]
|
||
self.__process_packet(data, CANID, CMD)
|
||
|
||
def recv_set_param_data(self):
|
||
data_recv = self.serial_.read_all()
|
||
packets = self.__extract_packets(data_recv)
|
||
for packet in packets:
|
||
data = packet[7:15]
|
||
CANID = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3]
|
||
CMD = packet[1]
|
||
self.__process_set_param_packet(data, CANID, CMD)
|
||
|
||
def __process_packet(self, data, CANID, CMD):
|
||
if CMD == 0x11:
|
||
if CANID != 0x00:
|
||
if CANID in self.motors_map:
|
||
q_uint = np.uint16((np.uint16(data[1]) << 8) | data[2])
|
||
dq_uint = np.uint16((np.uint16(data[3]) << 4) | (data[4] >> 4))
|
||
tau_uint = np.uint16(((data[4] & 0xf) << 8) | data[5])
|
||
MotorType_recv = self.motors_map[CANID].MotorType
|
||
Q_MAX = self.Limit_Param[MotorType_recv][0]
|
||
DQ_MAX = self.Limit_Param[MotorType_recv][1]
|
||
TAU_MAX = self.Limit_Param[MotorType_recv][2]
|
||
recv_q = uint_to_float(q_uint, -Q_MAX, Q_MAX, 16)
|
||
recv_dq = uint_to_float(dq_uint, -DQ_MAX, DQ_MAX, 12)
|
||
recv_tau = uint_to_float(tau_uint, -TAU_MAX, TAU_MAX, 12)
|
||
self.motors_map[CANID].recv_data(recv_q, recv_dq, recv_tau)
|
||
else:
|
||
MasterID=data[0] & 0x0f
|
||
if MasterID in self.motors_map:
|
||
q_uint = np.uint16((np.uint16(data[1]) << 8) | data[2])
|
||
dq_uint = np.uint16((np.uint16(data[3]) << 4) | (data[4] >> 4))
|
||
tau_uint = np.uint16(((data[4] & 0xf) << 8) | data[5])
|
||
MotorType_recv = self.motors_map[MasterID].MotorType
|
||
Q_MAX = self.Limit_Param[MotorType_recv][0]
|
||
DQ_MAX = self.Limit_Param[MotorType_recv][1]
|
||
TAU_MAX = self.Limit_Param[MotorType_recv][2]
|
||
recv_q = uint_to_float(q_uint, -Q_MAX, Q_MAX, 16)
|
||
recv_dq = uint_to_float(dq_uint, -DQ_MAX, DQ_MAX, 12)
|
||
recv_tau = uint_to_float(tau_uint, -TAU_MAX, TAU_MAX, 12)
|
||
self.motors_map[MasterID].recv_data(recv_q, recv_dq, recv_tau)
|
||
|
||
|
||
def __process_set_param_packet(self, data, CANID, CMD):
|
||
if CMD == 0x11 and (data[2] == 0x33 or data[2] == 0x55):
|
||
masterid=CANID
|
||
slaveId = ((data[1] << 8) | data[0])
|
||
if CANID==0x00: #防止有人把MasterID设为0稳一手
|
||
masterid=slaveId
|
||
|
||
if masterid not in self.motors_map:
|
||
if slaveId not in self.motors_map:
|
||
return
|
||
else:
|
||
masterid=slaveId
|
||
|
||
RID = data[3]
|
||
# 读取参数得到的数据
|
||
if is_in_ranges(RID):
|
||
#uint32类型
|
||
num = uint8s_to_uint32(data[4], data[5], data[6], data[7])
|
||
self.motors_map[masterid].temp_param_dict[RID] = num
|
||
|
||
else:
|
||
#float类型
|
||
num = uint8s_to_float(data[4], data[5], data[6], data[7])
|
||
self.motors_map[masterid].temp_param_dict[RID] = num
|
||
|
||
|
||
def addMotor(self, Motor):
|
||
"""
|
||
add motor to the motor control object 添加电机到电机控制对象
|
||
:param Motor: Motor object 电机对象
|
||
"""
|
||
self.motors_map[Motor.SlaveID] = Motor
|
||
if Motor.MasterID != 0:
|
||
self.motors_map[Motor.MasterID] = Motor
|
||
return True
|
||
|
||
def __control_cmd(self, Motor, cmd: np.uint8):
|
||
data_buf = np.array([0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, cmd], np.uint8)
|
||
self.__send_data(Motor.SlaveID, data_buf)
|
||
|
||
def __send_data(self, motor_id, data):
|
||
"""
|
||
send data to the motor 发送数据到电机
|
||
:param motor_id:
|
||
:param data:
|
||
:return:
|
||
"""
|
||
self.send_data_frame[13] = motor_id & 0xff
|
||
self.send_data_frame[14] = (motor_id >> 8)& 0xff #id high 8 bits
|
||
self.send_data_frame[21:29] = data
|
||
self.serial_.write(bytes(self.send_data_frame.T))
|
||
|
||
def __read_RID_param(self, Motor, RID):
|
||
can_id_l = Motor.SlaveID & 0xff #id low 8 bits
|
||
can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits
|
||
data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0x33, np.uint8(RID), 0x00, 0x00, 0x00, 0x00], np.uint8)
|
||
self.__send_data(0x7FF, data_buf)
|
||
|
||
def __write_motor_param(self, Motor, RID, data):
|
||
can_id_l = Motor.SlaveID & 0xff #id low 8 bits
|
||
can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits
|
||
data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0x55, np.uint8(RID), 0x00, 0x00, 0x00, 0x00], np.uint8)
|
||
if not is_in_ranges(RID):
|
||
# data is float
|
||
data_buf[4:8] = float_to_uint8s(data)
|
||
else:
|
||
# data is int
|
||
data_buf[4:8] = data_to_uint8s(int(data))
|
||
self.__send_data(0x7FF, data_buf)
|
||
|
||
def switchControlMode(self, Motor, ControlMode):
|
||
"""
|
||
switch the control mode of the motor 切换电机控制模式
|
||
:param Motor: Motor object 电机对象
|
||
:param ControlMode: Control_Type 电机控制模式 example:MIT:Control_Type.MIT MIT模式
|
||
"""
|
||
max_retries = 10
|
||
retry_interval = 0.05 #retry times
|
||
RID = 10
|
||
self.__write_motor_param(Motor, RID, np.uint8(ControlMode))
|
||
for _ in range(max_retries):
|
||
sleep(retry_interval)
|
||
self.recv_set_param_data()
|
||
if Motor.SlaveID in self.motors_map:
|
||
if RID in self.motors_map[Motor.SlaveID].temp_param_dict:
|
||
if self.motors_map[Motor.SlaveID].temp_param_dict[RID] == ControlMode:
|
||
return True
|
||
else:
|
||
return False
|
||
return False
|
||
|
||
def save_motor_param(self, Motor):
|
||
"""
|
||
save the all parameter to flash 保存所有电机参数
|
||
:param Motor: Motor object 电机对象
|
||
:return:
|
||
"""
|
||
can_id_l = Motor.SlaveID & 0xff #id low 8 bits
|
||
can_id_h = (Motor.SlaveID >> 8)& 0xff #id high 8 bits
|
||
data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0xAA, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8)
|
||
self.disable(Motor) # before save disable the motor
|
||
self.__send_data(0x7FF, data_buf)
|
||
sleep(0.001)
|
||
|
||
def change_limit_param(self, Motor_Type, PMAX, VMAX, TMAX):
|
||
"""
|
||
change the PMAX VMAX TMAX of the motor 改变电机的PMAX VMAX TMAX
|
||
:param Motor_Type:
|
||
:param PMAX: 电机的PMAX
|
||
:param VMAX: 电机的VMAX
|
||
:param TMAX: 电机的TMAX
|
||
:return:
|
||
"""
|
||
self.Limit_Param[Motor_Type][0] = PMAX
|
||
self.Limit_Param[Motor_Type][1] = VMAX
|
||
self.Limit_Param[Motor_Type][2] = TMAX
|
||
|
||
def refresh_motor_status(self, Motor):
|
||
"""
|
||
get the motor status 获得电机状态
|
||
"""
|
||
can_id_l = Motor.SlaveID & 0xff #id low 8 bits
|
||
can_id_h = (Motor.SlaveID >> 8) & 0xff #id high 8 bits
|
||
data_buf = np.array([np.uint8(can_id_l), np.uint8(can_id_h), 0xCC, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8)
|
||
self.__send_data(0x7FF, data_buf)
|
||
self.recv() # receive the data from serial port
|
||
|
||
def change_motor_param(self, Motor, RID, data):
|
||
"""
|
||
change the RID of the motor 改变电机的参数
|
||
:param Motor: Motor object 电机对象
|
||
:param RID: DM_variable 电机参数
|
||
:param data: 电机参数的值
|
||
:return: True or False ,True means success, False means fail
|
||
"""
|
||
max_retries = 20
|
||
retry_interval = 0.05 #retry times
|
||
|
||
self.__write_motor_param(Motor, RID, data)
|
||
for _ in range(max_retries):
|
||
self.recv_set_param_data()
|
||
if Motor.SlaveID in self.motors_map and RID in self.motors_map[Motor.SlaveID].temp_param_dict:
|
||
if abs(self.motors_map[Motor.SlaveID].temp_param_dict[RID] - data) < 0.1:
|
||
return True
|
||
else:
|
||
return False
|
||
sleep(retry_interval)
|
||
return False
|
||
|
||
def read_motor_param(self, Motor, RID):
|
||
"""
|
||
read only the RID of the motor 读取电机的内部信息例如 版本号等
|
||
:param Motor: Motor object 电机对象
|
||
:param RID: DM_variable 电机参数
|
||
:return: 电机参数的值
|
||
"""
|
||
max_retries = 20
|
||
retry_interval = 0.05 #retry times
|
||
self.__read_RID_param(Motor, RID)
|
||
for _ in range(max_retries):
|
||
sleep(retry_interval)
|
||
self.recv_set_param_data()
|
||
if Motor.SlaveID in self.motors_map:
|
||
if RID in self.motors_map[Motor.SlaveID].temp_param_dict:
|
||
return self.motors_map[Motor.SlaveID].temp_param_dict[RID]
|
||
else:
|
||
return None
|
||
return None
|
||
|
||
# -------------------------------------------------
|
||
# Extract packets from the serial data
|
||
def __extract_packets(self, data):
|
||
frames = []
|
||
header = 0xAA
|
||
tail = 0x55
|
||
frame_length = 16
|
||
i = 0
|
||
remainder_pos = 0
|
||
|
||
while i <= len(data) - frame_length:
|
||
if data[i] == header and data[i + frame_length - 1] == tail:
|
||
frame = data[i:i + frame_length]
|
||
frames.append(frame)
|
||
i += frame_length
|
||
remainder_pos = i
|
||
else:
|
||
i += 1
|
||
self.data_save = data[remainder_pos:]
|
||
return frames
|
||
|
||
|
||
def LIMIT_MIN_MAX(x, min, max):
|
||
if x <= min:
|
||
x = min
|
||
elif x > max:
|
||
x = max
|
||
|
||
|
||
def float_to_uint(x: float, x_min: float, x_max: float, bits):
|
||
LIMIT_MIN_MAX(x, x_min, x_max)
|
||
span = x_max - x_min
|
||
data_norm = (x - x_min) / span
|
||
return np.uint16(data_norm * ((1 << bits) - 1))
|
||
|
||
|
||
def uint_to_float(x: np.uint16, min: float, max: float, bits):
|
||
span = max - min
|
||
data_norm = float(x) / ((1 << bits) - 1)
|
||
temp = data_norm * span + min
|
||
return np.float32(temp)
|
||
|
||
|
||
def float_to_uint8s(value):
|
||
# Pack the float into 4 bytes
|
||
packed = pack('f', value)
|
||
# Unpack the bytes into four uint8 values
|
||
return unpack('4B', packed)
|
||
|
||
|
||
def data_to_uint8s(value):
|
||
# Check if the value is within the range of uint32
|
||
if isinstance(value, int) and (0 <= value <= 0xFFFFFFFF):
|
||
# Pack the uint32 into 4 bytes
|
||
packed = pack('I', value)
|
||
else:
|
||
raise ValueError("Value must be an integer within the range of uint32")
|
||
|
||
# Unpack the bytes into four uint8 values
|
||
return unpack('4B', packed)
|
||
|
||
|
||
def is_in_ranges(number):
|
||
"""
|
||
check if the number is in the range of uint32
|
||
:param number:
|
||
:return:
|
||
"""
|
||
if (7 <= number <= 10) or (13 <= number <= 16) or (35 <= number <= 36):
|
||
return True
|
||
return False
|
||
|
||
|
||
def uint8s_to_uint32(byte1, byte2, byte3, byte4):
|
||
# Pack the four uint8 values into a single uint32 value in little-endian order
|
||
packed = pack('<4B', byte1, byte2, byte3, byte4)
|
||
# Unpack the packed bytes into a uint32 value
|
||
return unpack('<I', packed)[0]
|
||
|
||
|
||
def uint8s_to_float(byte1, byte2, byte3, byte4):
|
||
# Pack the four uint8 values into a single float value in little-endian order
|
||
packed = pack('<4B', byte1, byte2, byte3, byte4)
|
||
# Unpack the packed bytes into a float value
|
||
return unpack('<f', packed)[0]
|
||
|
||
|
||
def print_hex(data):
|
||
hex_values = [f'{byte:02X}' for byte in data]
|
||
print(' '.join(hex_values))
|
||
|
||
|
||
def get_enum_by_index(index, enum_class):
|
||
try:
|
||
return enum_class(index)
|
||
except ValueError:
|
||
return None
|
||
|
||
|
||
class DM_Motor_Type(IntEnum):
|
||
DM4310 = 0
|
||
DM4310_48V = 1
|
||
DM4340 = 2
|
||
DM4340_48V = 3
|
||
DM6006 = 4
|
||
DM8006 = 5
|
||
DM8009 = 6
|
||
DM10010L = 7
|
||
DM10010 = 8
|
||
DMH3510 = 9
|
||
DMH6215 = 10
|
||
DMG6220 = 11
|
||
|
||
|
||
class DM_variable(IntEnum):
|
||
UV_Value = 0
|
||
KT_Value = 1
|
||
OT_Value = 2
|
||
OC_Value = 3
|
||
ACC = 4
|
||
DEC = 5
|
||
MAX_SPD = 6
|
||
MST_ID = 7
|
||
ESC_ID = 8
|
||
TIMEOUT = 9
|
||
CTRL_MODE = 10
|
||
Damp = 11
|
||
Inertia = 12
|
||
hw_ver = 13
|
||
sw_ver = 14
|
||
SN = 15
|
||
NPP = 16
|
||
Rs = 17
|
||
LS = 18
|
||
Flux = 19
|
||
Gr = 20 # 齿轮比
|
||
PMAX = 21
|
||
VMAX = 22
|
||
TMAX = 23
|
||
I_BW = 24
|
||
KP_ASR = 25
|
||
KI_ASR = 26
|
||
KP_APR = 27
|
||
KI_APR = 28
|
||
OV_Value = 29
|
||
GREF = 30
|
||
Deta = 31
|
||
V_BW = 32
|
||
IQ_c1 = 33
|
||
VL_c1 = 34
|
||
can_br = 35
|
||
sub_ver = 36 # 固件版本号
|
||
u_off = 50
|
||
v_off = 51
|
||
k1 = 52
|
||
k2 = 53
|
||
m_off = 54
|
||
dir = 55
|
||
p_m = 80
|
||
xout = 81
|
||
|
||
|
||
class Control_Type(IntEnum):
|
||
MIT = 1
|
||
POS_VEL = 2
|
||
VEL = 3
|
||
Torque_Pos = 4
|