CAN2.0B 通信协议
帧格式说明
- 在串行帧转CAN报文的过程中，以固定字节（13字节）对齐的串行数据帧中，如果某段数据的格式不标准，将不会对该数据段进行转换，接着转换后面的数据。如果转换后发现少了某些CAN报文，请检查对应报文的固定字节长度串行数据格式是否与标准格式符合。
- 帧数据在CAN格式转换时，长度固定为8字节。有效长度由DLC3-DLC0的值决定，有效数据不足固定长度时，需要补0到固定长度
- 此模式下，要注意严格按照固定字节的格式串行数据格式才能转换成功，CAN模式转换可参考以下示例，转换时首先要确保帧信息无误，数据长度表示无误，否则不会进行转换
- 例子：
串行帧
Serial Frame88
17
65
43
21
99
88
77
66
55
44
33
22
↓
CAN报文
CAN Frame帧信息
Frame Info帧ID
Frame ID帧数据
Frame Data88
17
65
43
21
99
88
77
66
55
44
33
22
注意，此处报文的头字节为本公司的usb转can模块的需要，如果并未使用本公司的usb转can模块则可以忽略该字节，发送12个字节即可
控制命令表
- 控制帧由主控制器发送给执行器，用于模式选择，参数配置和运动控制
- 帧类型：DATA
- 帧格式：EXTENDED
- 长度:8
控制功能 | 默认帧ID | 数据 | |||||||
Data[0] | Data[1] | Data[2] | Data[3] | Data[4] | Data[5] | Data[6] | Data[7] | ||
复位 |
|
|
|
|
|
|
|
|
|
恢复出厂设置 |
|
|
|
|
|
|
|
|
|
选择电流模式 |
|
|
|
|
|
|
|
|
|
设置相电流（电流模式） |
| 16bits 相电流数据内容，采用大端模式 |
|
|
|
|
|
| |
选择轮廓速度模式 |
|
|
|
|
|
|
|
|
|
设置相电流&速度（轮廓速度模式） |
| 16bits 相电流（限制电流）数据内容，采用大端模式 | 16bits 速度数据内容，采用大端模式 |
|
|
|
| ||
选择轮廓位置模式 |
|
|
|
|
|
|
|
|
|
设置相电流&速度&位置（轮廓位置模式） |
| 16bits 相电流（限制电流）数据内容，采用大端模式 | 16bits 速度（限制速度）数据内容，采用大端模式 | 32bits 位置数据内容，采用大端模式 | |||||
选择位置模式 |
|
|
|
|
|
|
|
|
|
设置相电流&速度&位置（位置模式） |
| 16bits 相电流（限制电流）数据内容，采用大端模式 | 16bits 速度（限制速度）数据内容，采用大端模式 | 32bits 位置数据内容，采用大端模式 | |||||
选择MIT模式 |
|
|
|
|
|
|
|
|
|
设置相电流&速度&位置 (MIT模式） |
| 16bits 相电流数据内容，采用大端模式 | 16bits 速度数据内容，采用大端模式 | 32bits 位置数据内容，采用大端模式 | |||||
选择开环模式 |
|
|
|
|
|
|
|
|
|
设置电机转速（开环模式） |
| 16bits 速度数据内容，采用大端模式,单位：Pul |
|
|
|
|
|
| |
设置抱闸PWM和持续时间 |
| 16bits PWM 数据内容，采用大端模式 | 16bits 时间数据内容，采用大端模式 |
|
|
|
| ||
注意：以下为实时修改参数的指令，均为修改RAM的值，应该在进入操作模式后发送，如果不发送，则调用Flash里的值 | |||||||||
实时设置电流环PI（电流&轮廓速度&轮廓位置模式&位置模式） |
| 32bits浮点数 电流环系数Kp参数 | 32bits浮点数 电流环系数Ki参数 | ||||||
实时设置速度环PI(轮廓速度&轮廓位置模式&位置模式） |
| 32bits浮点数 速度环系数Kp参数 | 32bits浮点数 速度环系数Ki参数 | ||||||
实时设置位置环PD (轮廓位置模式&位置模式） |
| 32bits浮点数 位置环系数Kp参数 | 32bits浮点数 位置环系数Kd参数 | ||||||
实时设置Kp&Kd (MIT模式） |
| 32bits浮点数 位置系数Kp参数 | 32bits浮点数 速度系数Kd参数 | ||||||
实时设置加减速（开环模式） |
| 32bits 加速度数据内容，采用大端模式，单位：Pul/s | 32bits 减速度数据内容，采用大端模式，单位：Pul/s | ||||||
实时设置加减速（轮廓速度&轮廓位置模式&位置模式） |
| 32bits 加速度数据内容，采用大端模式，单位：rpm/s | 32bits 减速度数据内容，采用大端模式，单位：rpm/s | ||||||
（1）反馈帧
- 反馈帧由执行器发送给主控制器，用于反馈各种状态
- 帧类型：DATA
- 帧格式：EXTENDED
- 长度：8
反馈类型 | 默认帧ID | 数据 | |||||||
Data[0] | Data[1] | Data[2] | Data[3] | Data[4] | Data[5] | Data[6] | Data[7] | ||
反馈0 |
| 32bits 序列号 | 32bits 固件版本日期 | ||||||
反馈1 |
| 16-bit 电压 | 16-bit PWM | 16-bit 相电流 | 16-bit 速度 | ||||
反馈2 |
| 32-bit 电机位置 | 32-bit 减速机位置 | ||||||
反馈3 |
| Mosfet 温度 | 电机温度 | MCU温度 | 警告类型 | 错误类型 | 暂未定义 | 暂未定义 | 当前运行模式 |
错误类型包括：
错误类型 | |
|---|---|
| 欠压 |
| 过压 |
| 过流 |
| Mosfet过温 |
| 电机过温 |
| MCU过温 |
警告类型 | |
| CAN看门狗超时 |
读取与写入寄存器值
控制功能 | 默认帧ID | 数据 | |||||||
Data[0] | Data[1] | Data[2] | Data[3] | Data[4] | Data[5] | Data[6] | Data[7] | ||
选择EEPROM模式 |
|
|
|
|
|
|
|
|
|
读取寄存器数值 |
|
|
|
|
|
|
|
|
|
写入寄存器数值 |
|
|
| 32bits 数据内容，采用大端模式 |
|
| |||
电机参数识别 |
|
|
|
|
|
|
|
|
|
Definition | Register Adress | Default Value | Range | Unit |
|---|---|---|---|---|
电流环Kp值 |
| 0.0006 | ||
电流环Ki值 |
| 0.0001 | ||
速度环Kp值 |
| 50 | ||
速度环Ki值 |
| 1.2 | ||
位置环Kp值 |
| 0.008 | ||
位置环Kd值 |
| 0 | ||
MIT_Kp |
| 0.09 | ||
MIT_Kd |
| 13.1 | ||
CAN起始地址 |
| 05060000 | 0-256 | |
电机端旋转方向 |
| Auto-Set | +1/-1 | |
各位置模式下的编码器选择 |
| 1 | 1:Reducer | |
减速比大值 |
| 31/51/101 | ||
减速比小值 |
| 1 | ||
减速器端零点偏置 |
| 0 | ||
减速器端编码器模式自动回零 |
| 1 | 0:No | |
开环模式中的加速度 |
| 2000 | Pul/s | |
开环模式中的减速度 |
| 2000 | Pul/s | |
轮廓速度模式与轮廓位置模式中的加速度 |
| 2000 | rpm/s | |
轮廓速度模式与轮廓位置模式中的减速度 |
| 2000 | rpm/s | |
最大相电流限制 |
| 10000 | ma | |
最大速度限制 |
| 4000 | rpm | |
减速器端负向位置限制 |
| -2147480000 | ||
减速器端正向位置限制 |
| 2147480000 | ||
欠压保护电压阈值 |
| 20 | V | |
过压保护电压阈值 |
| 60 | V | |
Mosfet保护温度 |
| 90 | ℃ | |
电机保护温度 |
| 90 | ℃ | |
MCU保护温度 |
| 90 | ℃ | |
CAN看门狗保护周期 |
| 0 | ms |
数据解析示例（Python/C/C++）
（1）python
这里包括三个文件：
constants.py:
# Control Mode
ASYNC_MODE = 0x03
OPEN_LOOP_MODE = 0x04
CURRENT_MODE = 0x05
PROFILE_VELOCITY_MODE = 0x06
PROFILE_POSITION_MODE = 0x07
POSITION_MODE = 0x08
MIT_MODE = 0x09
BRAKE_CONTROL = 0x20
# RAM Modify Address
OPENLOOP_ACC_DEC_SET = 0x30
VELOCITY_ACC_DEC_SET = 0x31
CURRENT_PID_SET = 0x42
VELOCITY_PID_SET = 0x43
POSITION_PID_SET = 0x44
MIT_PID_SET = 0x45
# Tips
pattern_phrases = { 0x03:'ASYNC Mode',
0x04:'Openloop Mode',
0x05:'Current Mode',
0x06:'Profile Velocity Mode',
0x07:'Profile Position Mode',
0x08:'Position Mode',
0x09:'MIT Mode',
0x0E:'EEPROM MODE'}
motor_interface.py：
import time
import struct
import serial
from .constants import *
class MotorControl:
def __init__(self, serial_port: str, id_values, baudrate = 921600):
try:
self.ser = serial.Serial(serial_port, baudrate)
self.ser.timeout = 5
self.ids = id_values
self.is_connected = self.ser.is_open
except serial.SerialException as e:
print(f"Serial port connection failed: {e}")
self.ser = None
self.is_connected = False
def close_serial(self):
if self.ser:
self.ser.close()
self.is_connected = False
print("Serial port closed")
def Reset(self,flag):
results = []
self.ser.timeout = 1
for motor_id in self.ids:
id = 0x05060000 + (motor_id << 8)
value = [0x55] * 8
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
if flag:
time.sleep(0.2)
start_time = time.time()
while self.ser.in_waiting < 117 and time.time() - start_time <= 2:
time.sleep(0.01)
feedback = self.ser.read(self.ser.in_waiting)
header = bytes([0x88, 0x05, 0x06, motor_id, 0xB0])
start_feedback = feedback.find(header)
if start_feedback != -1 and len(feedback) >= start_feedback + 13:
packet = feedback[start_feedback: start_feedback + 13]
feedback1 = ''.join([f'{byte:02X}' for byte in packet[5:9]])
feedback2 = int.from_bytes(packet[9:13], byteorder='big', signed=True)
results.append([motor_id, feedback1, feedback2])
else: results.append([motor_id])
else:
feedback = self.ser.read(13)
if len(feedback) == 13 and feedback[4] == 0xB0:
feedback1 = ''.join([f'{byte:02X}' for byte in feedback[5:9]])
feedback2 = int.from_bytes(feedback[9:13], byteorder='big', signed=True)
results.append([motor_id, feedback1, feedback2])
else: results.append([motor_id])
self.ser.timeout = 5
return results
def mode_selection(self, serial_num, Feedback_cycle1, Feedback_cycle2, Feedback_cycle3):
for motor_id in self.ids:
serial_num = int(serial_num)
if serial_num < 3:
print('Please enter the correct parameters')
else:
id = 0x05060000 + (motor_id << 8) + 1
value = [0x55] * 8
value[0] = serial_num
value[1] = Feedback_cycle1
value[2] = Feedback_cycle2
value[3] = Feedback_cycle3
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
print(pattern_phrases[serial_num])
def write_async(self, pwm_values):
if len(pwm_values) != len(self.ids):
print("Error: The number of PWM values must match the number of motor IDs.")
return
for motor_id, pwm_value in zip(self.ids, pwm_values):
id = 0x05060000 + (motor_id << 8) + ASYNC_MODE
value = [0x55] * 8
value[0] = (int(pwm_value) >> 8) & 0xFF
value[1] = int(pwm_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def write_open_loop(self, pwm_values):
for motor_id, pwm_value in zip(self.ids, pwm_values):
id = 0x05060000 + (motor_id << 8) + OPEN_LOOP_MODE
value = [0x55] * 8
value[0] = (int(pwm_value) >> 8) & 0xFF
value[1] = int(pwm_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def write_current(self, current_values):
for motor_id, current_value in zip(self.ids, current_values):
id = 0x05060000 + (motor_id << 8) + CURRENT_MODE
value = [0x55] * 8
value[0] = (int(current_value) >> 8) & 0xFF
value[1] = int(current_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def write_profile_velocity(self, current_values, velocity_values):
for motor_id, current_value, velocity_value in zip(self.ids, current_values, velocity_values):
id = 0x05060000 + (motor_id << 8) + PROFILE_VELOCITY_MODE
value = [0x55] * 8
value[0] = (int(current_value) >> 8) & 0xFF
value[1] = int(current_value) & 0xFF
value[2] = (int(velocity_value) >> 8) & 0xFF
value[3] = int(velocity_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def write_profile_position(self, current_values, velocity_values, position_values):
for motor_id, current_value, velocity_value, position_value in zip(self.ids, current_values, velocity_values, position_values):
id = 0x05060000 + (motor_id << 8) + PROFILE_POSITION_MODE
value = [0x55] * 8
value[0] = (int(current_value) >> 8) & 0xFF
value[1] = int(current_value) & 0xFF
value[2] = (int(velocity_value) >> 8) & 0xFF
value[3] = int(velocity_value) & 0xFF
value[4] = (int(position_value) >> 24) & 0xFF
value[5] = (int(position_value) >> 16) & 0xFF
value[6] = (int(position_value) >> 8) & 0xFF
value[7] = int(position_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def write_position(self, current_values, velocity_values, position_values):
for motor_id, current_value, velocity_value, position_value in zip(self.ids, current_values, velocity_values, position_values):
id = 0x05060000 + (motor_id << 8) + POSITION_MODE
value = [0x00] * 8
value[0] = (int(current_value) >> 8) & 0xFF
value[1] = int(current_value) & 0xFF
value[2] = (int(velocity_value) >> 8) & 0xFF
value[3] = int(velocity_value) & 0xFF
value[4] = (int(position_value) >> 24) & 0xFF
value[5] = (int(position_value) >> 16) & 0xFF
value[6] = (int(position_value) >> 8) & 0xFF
value[7] = int(position_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def write_mit(self, current_values, velocity_values, position_values, kps, kis):
for motor_id, current_value, velocity_value, position_value, kp, ki in zip(self.ids, current_values, velocity_values, position_values, kps, kis):
id = 0x05060000 + (motor_id << 8) + MIT_MODE
value = [0x55] * 8
value[0] = (int(current_value) >> 8) & 0xFF
value[1] = int(current_value) & 0xFF
value[2] = (int(velocity_value) >> 8) & 0xFF
value[3] = int(velocity_value) & 0xFF
value[4] = (int(position_value) >> 24) & 0xFF
value[5] = (int(position_value) >> 16) & 0xFF
value[6] = (int(position_value) >> 8) & 0xFF
value[7] = int(position_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
kp_bytes = struct.pack('!f', kp)
ki_bytes = struct.pack('!f', ki)
id = 0x05060000 + (motor_id << 8) + MIT_PID_SET
value = [0x55] * 8
value[0] = kp_bytes[0]
value[1] = kp_bytes[1]
value[2] = kp_bytes[2]
value[3] = kp_bytes[3]
value[4] = ki_bytes[0]
value[5] = ki_bytes[1]
value[6] = ki_bytes[2]
value[7] = ki_bytes[3]
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def brake_control(self, pwm_values, time_values):
for motor_id, pwm_value, time_value in zip(self.ids, pwm_values, time_values):
id = 0x05060000 + (motor_id << 8) + BRAKE_CONTROL
value = [0x55] * 8
value[0] = (int(pwm_value) >> 8) & 0xFF
value[1] = int(pwm_value) & 0xFF
value[2] = (int(time_value) >> 8) & 0xFF
value[3] = int(time_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def openloop_acc_dec_set(self, acc_values, dec_values):
for motor_id, acc_value, dec_value in zip(self.ids, acc_values, dec_values):
id = 0x05060000 + (motor_id << 8) + OPENLOOP_ACC_DEC_SET
value = [0x55] * 8
value[0] = (int(acc_value) >> 24) & 0xFF
value[1] = (int(acc_value) >> 16) & 0xFF
value[2] = (int(acc_value) >> 8) & 0xFF
value[3] = int(acc_value) & 0xFF
value[4] = (int(dec_value) >> 24) & 0xFF
value[5] = (int(dec_value) >> 16) & 0xFF
value[6] = (int(dec_value) >> 8) & 0xFF
value[7] = int(dec_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def velocity_acc_dec_set(self, acc_values, dec_values):
for motor_id, acc_value, dec_value in zip(self.ids, acc_values, dec_values):
id = 0x05060000 + (motor_id << 8) + VELOCITY_ACC_DEC_SET
value = [0x55] * 8
value[0] = (int(acc_value) >> 24) & 0xFF
value[1] = (int(acc_value) >> 16) & 0xFF
value[2] = (int(acc_value) >> 8) & 0xFF
value[3] = int(acc_value) & 0xFF
value[4] = (int(dec_value) >> 24) & 0xFF
value[5] = (int(dec_value) >> 16) & 0xFF
value[6] = (int(dec_value) >> 8) & 0xFF
value[7] = int(dec_value) & 0xFF
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def current_pid_set(self, kps, kis):
for motor_id, kp, ki in zip(self.ids, kps, kis):
kp_bytes = struct.pack('!f', kp)
ki_bytes = struct.pack('!f', ki)
id = 0x05060000 + (motor_id << 8) + CURRENT_PID_SET
value = [0x55] * 8
value[0] = kp_bytes[0]
value[1] = kp_bytes[1]
value[2] = kp_bytes[2]
value[3] = kp_bytes[3]
value[4] = ki_bytes[0]
value[5] = ki_bytes[1]
value[6] = ki_bytes[2]
value[7] = ki_bytes[3]
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def velocity_pid_set(self, kps, kis):
for motor_id, kp, ki in zip(self.ids, kps, kis):
kp_bytes = struct.pack('!f', kp)
ki_bytes = struct.pack('!f', ki)
id = 0x05060000 + (motor_id << 8) + VELOCITY_PID_SET
value = [0x55] * 8
value[0] = kp_bytes[0]
value[1] = kp_bytes[1]
value[2] = kp_bytes[2]
value[3] = kp_bytes[3]
value[4] = ki_bytes[0]
value[5] = ki_bytes[1]
value[6] = ki_bytes[2]
value[7] = ki_bytes[3]
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def position_pid_set(self, kps, kds):
for motor_id, kp, kd in zip(self.ids, kps, kds):
kp_bytes = struct.pack('!f', kp)
kd_bytes = struct.pack('!f', kd)
id = 0x05060000 + (motor_id << 8) + POSITION_PID_SET
value = [0x55] * 8
value[0] = kp_bytes[0]
value[1] = kp_bytes[1]
value[2] = kp_bytes[2]
value[3] = kp_bytes[3]
value[4] = kd_bytes[0]
value[5] = kd_bytes[1]
value[6] = kd_bytes[2]
value[7] = kd_bytes[3]
data = [0x88] + [(id >> 24) & 0xFF, (id >> 16) & 0xFF, (id >> 8) & 0xFF, id & 0xFF] + value
byte_data = bytes(data)
self.ser.write(byte_data)
def read_v_p_c_v(self):
results = []
for motor_id in self.ids:
start_time = time.time()
self.ser.reset_input_buffer()
while self.ser.in_waiting < 117 and time.time() - start_time <= 3:
time.sleep(0.01)
feedback = self.ser.read(self.ser.in_waiting)
header = bytes([0x88, 0x05, 0x06, motor_id, 0xB1])
start_feedback = feedback.find(header)
if start_feedback != -1 and len(feedback) >= start_feedback + 13:
packet = feedback[start_feedback: start_feedback + 13]
voltage_value = int.from_bytes(packet[5:7], byteorder='big', signed=True)
pwm_value = int.from_bytes(packet[7:9], byteorder='big', signed=True)
current_value = int.from_bytes(packet[9:11], byteorder='big', signed=True)
velocity_value = int.from_bytes(packet[11:13], byteorder='big', signed=True)
results.append([motor_id, voltage_value, pwm_value, current_value, velocity_value])
else:
print(f"Motor ID: {motor_id :<2} | The complete message was not found or the message length was insufficient")
return results
def read_pos(self):
results = []
for motor_id in self.ids:
start_time = time.time()
self.ser.reset_input_buffer()
while self.ser.in_waiting < 117 and time.time() - start_time <= 3:
time.sleep(0.01)
feedback = self.ser.read(self.ser.in_waiting)
header = bytes([0x88, 0x05, 0x06, motor_id, 0xB2])
start_feedback = feedback.find(header)
if start_feedback != -1 and len(feedback) >= start_feedback + 13:
packet = feedback[start_feedback: start_feedback + 13]
motor_position_value = int.from_bytes(packet[5:9], byteorder='big', signed=True)
actuator_position_value = int.from_bytes(packet[9:13], byteorder='big', signed=True)
results.append([motor_id, motor_position_value, actuator_position_value])
else:
print(f"Motor ID: {motor_id :<2} | The complete message was not found or the message length was insufficient")
return results
def read_temp_error(self):
results = []
for motor_id in self.ids:
start_time = time.time()
self.ser.reset_input_buffer()
while self.ser.in_waiting < 117 and time.time() - start_time <= 3:
time.sleep(0.01)
feedback = self.ser.read(self.ser.in_waiting)
header = bytes([0x88, 0x05, 0x06, motor_id, 0xB3])
start_feedback = feedback.find(header)
if start_feedback != -1 and len(feedback) >= start_feedback + 13:
packet = feedback[start_feedback: start_feedback + 13]
mos_temp = int.from_bytes(packet[5:6], byteorder='big', signed=True)
mt_temp = int.from_bytes(packet[6:7], byteorder='big', signed=True)
g431_temp = int.from_bytes(packet[7:8], byteorder='big', signed=True)
warning_type = int.from_bytes(packet[8:9], byteorder='big', signed=True)
error_type = int.from_bytes(packet[9:10], byteorder='big', signed=True)
results.append([motor_id, mos_temp, mt_temp, g431_temp, warning_type, error_type])
else:
print(f"Motor ID: {motor_id :<2} | The complete message was not found or the message length was insufficient.")
return results
motor_control.py:
import time
from .motor_interface import MotorControl
from .constants import *
def reset(motor: MotorControl, flag = 0):
return motor.Reset(flag)
def mode_selection(motor: MotorControl, mode, Feedback_cycle1, Feedback_cycle2, Feedback_cycle3):
motor.mode_selection(mode, Feedback_cycle1, Feedback_cycle2, Feedback_cycle3)
def write_async(motor: MotorControl, pwm_values):
motor.write_async(pwm_values)
def write_open_loop(motor: MotorControl, pwm_values):
motor.write_open_loop(pwm_values)
def write_current(motor: MotorControl, current_values):
motor.write_current(current_values)
def write_profile_velocity(motor: MotorControl, current_values, velocity_values):
motor.write_profile_velocity(current_values, velocity_values)
def write_profile_position(motor: MotorControl, current_values, velocity_values, position_values):
motor.write_profile_position(current_values, velocity_values, position_values)
def write_position(motor: MotorControl, current_values, velocity_values, position_values):
motor.write_position(current_values, velocity_values, position_values)
def write_mit(motor: MotorControl, current_values, velocity_values, position_values, kps, kis):
motor.write_mit(current_values, velocity_values, position_values, kps, kis)
def brake_control(motor: MotorControl, pwm_values, time_values):
motor.brake_control(pwm_values, time_values)
def openloop_acc_dec_set(motor: MotorControl, acc_values, dec_values):
motor.openloop_acc_dec_set(acc_values, dec_values)
def velocity_acc_dec_set(motor: MotorControl, acc_values, dec_values):
motor.velocity_acc_dec_set(acc_values, dec_values)
def current_pid_set(motor: MotorControl, kps, kis):
motor.current_pid_set(kps, kis)
def velocity_pid_set(motor: MotorControl, kps, kis):
motor.velocity_pid_set(kps, kis)
def position_pid_set(motor: MotorControl, kps, kds):
motor.position_pid_set(kps, kds)
def get_v_p_c_v(motor: MotorControl):
return motor.read_v_p_c_v()
def get_pos(motor: MotorControl):
return motor.read_pos()
def get_temp_error(motor: MotorControl):
return motor.read_temp_error()
（2）c/c++
motor_control.h：
#ifndef MOTOR_CONTROL_H
#define MOTOR_CONTROL_H
#include <iostream>
#include <iomanip>
#ifdef _WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif
#include <vector>
#include <string>
#include <chrono>
#include <thread>
#include <iostream>
#include <stdexcept>
#include <serial/serial.h>
using namespace std;
enum class Mode
{
ASYNC_MODE = 0x03,
OPEN_LOOP_MODE = 0x04,
CURRENT_MODE = 0x05,
PROFILE_VELOCITY_MODE = 0x06,
PROFILE_POSITION_MODE = 0x07,
POSITION_MODE = 0x08,
MIT_MODE = 0x09,
BRAKE_CONTROL = 0x20,
OPENLOOP_ACC_DEC_SET = 0x30,
VELOCITY_ACC_DEC_SET = 0x31,
CURRENT_PID_SET = 0x42,
VELOCITY_PID_SET = 0x43,
POSITION_PID_SET = 0x44,
MIT_PID_SET = 0x45
};
struct MotorStatus
{
int16_t voltage_value = 0;
int16_t pwm_value = 0;
int16_t current_value = 0;
int16_t velocity_value = 0;
int32_t motor_position_value = 0;
int32_t actuator_position_value = 0;
uint8_t mos_temp = 0;
uint8_t mt_temp = 0;
uint8_t g431_temp = 0;
uint8_t warning_type = 0;
uint8_t error_type = 0;
};
class MotorControl
{
public:
MotorControl(const string& port, const uint32_t& baudrate, const uint8_t& motor_id);
//~MotorControl();
void closeport();
void set_object_id(const uint8_t& new_id);
void clearBuffer();
vector<int> reset();
void mode_selection(const Mode& mode, const uint8_t& Feedback_cycle1, const uint8_t& Feedback_cycle2, const uint8_t& Feedback_cycle3);
void write_async(const int16_t& PWM_Value);
void write_open_loop(const int16_t& PWM_Value);
void write_current(const int16_t& Current_Value);
void write_profile_velocity(const int16_t& Current_Value, const int16_t& Velocity_Value);
void write_profile_position(const int16_t& Current_Value, const int16_t& Velocity_Value, const int32_t& Positin_Value, Mode mode = Mode::PROFILE_POSITION_MODE);
void write_position(const int16_t& Current_Value, const int16_t& Velocity_Value, const int32_t& Positin_Value);
void write_mit(const int16_t& Current_Value, const int16_t& Velocity_Value, const int32_t& Positin_Value, const float& KP, const float& KI);
void write_openloop_acc_dec(const int16_t& Acc_Value, const int16_t& Dec_Value);
void write_velocity_acc_dec(const int16_t& Acc_Value, const int16_t& Dec_Value);
void write_current_pid(const float& KP, const float& KI);
void write_velocity_pid(const float& KP, const float& KI);
void write_position_pid(const float& KP, const float& KD);
void brake_control(const int16_t& PWM_Value, const uint16_t& Time_Value);
void insert_and_send_data();
void send_data(const uint8_t* data, size_t size);
MotorStatus get_motor_status(vector<uint8_t> header_types);
private:
uint8_t motor_id;
serial::Serial ser;
vector<uint8_t> Header = {0x88};
vector<uint8_t> ID = {0x05, 0x06, 0x00, 0x00};
vector<uint8_t> Data = {0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55};
size_t find_start_of_feedback(const vector<uint8_t>& feedback, const vector<uint8_t>& header);
};
#endif
motor_control.cpp:
#include <motor_control.h>
MotorControl::MotorControl(const string& port, const uint32_t& baudrate, const uint8_t& motor_id)
:motor_id(motor_id)
{
try
{
ser.setPort(port);
ser.setBaudrate(baudrate);
serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
ser.setTimeout(timeout);
if (ser.isOpen())
{
cout << port << " is opened" << endl;
}
else
{
ser.open();
cout << port << " open success" << endl;
}
}
catch (std::exception& e)
{
cout << "Error: " << e.what() << endl;
}
}
void MotorControl::closeport()
{
if (ser.isOpen())
{
ser.close();
cout << "Serial port closed" << endl;
}
else
{
cout << "Serial port was not open" << endl;
}
}
void MotorControl::set_object_id(const uint8_t& new_id)
{
motor_id = new_id;
}
void MotorControl::clearBuffer()
{
if (ser.isOpen())
{
ser.flush();
ser.read(ser.available());
}
}
vector<int> MotorControl::reset()
{
clearBuffer();
ID.back() = 0x00;
ID[2] = motor_id;
insert_and_send_data();
size_t response_length = 13;
vector<uint8_t> response_data(response_length);
vector<int> results(2, 0);
auto start_time = chrono::steady_clock::now();
while (true)
{
size_t bytes_read = ser.read(response_data.data(), response_length);
if (bytes_read > 0)
{
if (bytes_read >= 5 &&
response_data[0] == 0x88 &&
response_data[1] == 0x05 &&
response_data[2] == 0x06 &&
response_data[3] == motor_id &&
response_data[4] == 0xB0)
{
vector<int> b(response_data.begin() + 5, response_data.begin() + 9);
auto version = (b[0] << 24) + (b[1] << 16) + (b[2] << 8) + b[3];
results[0] = version;
vector<int> a(response_data.begin() + 9, response_data.begin() + 13);
auto date = (a[0] << 24) | (a[1] << 16) | (a[2] << 8) | a[3];
results[1] = date;
return results;
}
}
auto elapsed_time = chrono::steady_clock::now() - start_time;
if (chrono::duration_cast<chrono::seconds>(elapsed_time).count() >= 0.2)
{
// cout << "Timeout: No valid response received within 0.2 seconds." << endl;
return results;
}
this_thread::sleep_for(chrono::milliseconds(10));
}
}
void MotorControl::mode_selection(const Mode& mode, const uint8_t& Feedback_cycle1, const uint8_t& Feedback_cycle2, const uint8_t& Feedback_cycle3)
{
ID.back() = 0x01;
ID[2] = motor_id;
Data[0] = static_cast<int>(mode);
Data[1] = Feedback_cycle1;
Data[2] = Feedback_cycle2;
Data[3] = Feedback_cycle3;
insert_and_send_data();
}
void MotorControl::write_async(const int16_t& PWM_Value)
{
ID.back() = static_cast<int>(Mode::ASYNC_MODE);
ID[2] = motor_id;
Data[0] = PWM_Value >> 8 & 0xFF;
Data[1] = PWM_Value & 0xFF;
insert_and_send_data();
}
void MotorControl::write_open_loop(const int16_t& PWM_Value)
{
ID.back() = static_cast<int>(Mode::OPEN_LOOP_MODE);
ID[2] = motor_id;
Data[0] = PWM_Value >> 8 & 0xFF;
Data[1] = PWM_Value & 0xFF;
insert_and_send_data();
}
void MotorControl::write_current(const int16_t& Current_Value)
{
ID.back() = static_cast<int>(Mode::CURRENT_MODE);
ID[2] = motor_id;
Data[0] = Current_Value >> 8 & 0xFF;
Data[1] = Current_Value & 0xFF;
insert_and_send_data();
}
void MotorControl::write_profile_velocity(const int16_t& Current_Value, const int16_t& Velocity_Value)
{
ID.back() = static_cast<int>(Mode::PROFILE_VELOCITY_MODE);
ID[2] = motor_id;
Data[0] = Current_Value >> 8 & 0xFF;
Data[1] = Current_Value & 0xFF;
Data[2] = Velocity_Value >> 8 & 0xFF;
Data[3] = Velocity_Value & 0xFF;
insert_and_send_data();
}
void MotorControl::write_profile_position(const int16_t& Current_Value, const int16_t& Velocity_Value, const int32_t& Positin_Value, Mode mode)
{
ID.back() = static_cast<int>(mode);
ID[2] = motor_id;
Data[0] = Current_Value >> 8 & 0xFF;
Data[1] = Current_Value & 0xFF;
Data[2] = Velocity_Value >> 8 & 0xFF;
Data[3] = Velocity_Value & 0xFF;
Data[4] = Positin_Value >> 24 & 0xFF;
Data[5] = Positin_Value >> 16 & 0xFF;
Data[6] = Positin_Value >> 8 & 0xFF;
Data[7] = Positin_Value & 0xFF;
insert_and_send_data();
}
void MotorControl::write_position(const int16_t& Current_Value, const int16_t& Velocity_Value, const int32_t& Positin_Value)
{
ID.back() = static_cast<int>(Mode::POSITION_MODE);
ID[2] = motor_id;
Data[0] = Current_Value >> 8 & 0xFF;
Data[1] = Current_Value & 0xFF;
Data[2] = Velocity_Value >> 8 & 0xFF;
Data[3] = Velocity_Value & 0xFF;
Data[4] = Positin_Value >> 24 & 0xFF;
Data[5] = Positin_Value >> 16 & 0xFF;
Data[6] = Positin_Value >> 8 & 0xFF;
Data[7] = Positin_Value & 0xFF;
insert_and_send_data();
}
void MotorControl::write_mit(const int16_t& Current_Value, const int16_t& Velocity_Value, const int32_t& Positin_Value, const float& KP, const float& KI)
{
write_profile_position(Current_Value, Velocity_Value, Positin_Value, Mode:: MIT_MODE);
ID.back() = static_cast<int>(Mode::MIT_PID_SET);
union buf_kp_ki
{
float f[2];
uint8_t u8[8];
};
buf_kp_ki kp_ki;
kp_ki.f[0] = KP;
kp_ki.f[1] = KI;
Data[0] = kp_ki.u8[3];
Data[1] = kp_ki.u8[2];
Data[2] = kp_ki.u8[1];
Data[3] = kp_ki.u8[0];
Data[4] = kp_ki.u8[7];
Data[5] = kp_ki.u8[6];
Data[6] = kp_ki.u8[5];
Data[7] = kp_ki.u8[4];
insert_and_send_data();
}
void MotorControl::write_openloop_acc_dec(const int16_t& Acc_Value, const int16_t& Dec_Value)
{
ID.back() = static_cast<int>(Mode::OPENLOOP_ACC_DEC_SET);
ID[2] = motor_id;
Data[0] = Acc_Value >> 24 & 0xff;
Data[1] = Acc_Value >> 16 & 0xff;
Data[2] = Acc_Value >> 8 & 0xff;
Data[3] = Acc_Value & 0xff;
Data[4] = Dec_Value >> 24 & 0xff;
Data[5] = Dec_Value >> 16 & 0xff;
Data[6] = Dec_Value >> 8 & 0xff;
Data[7] = Dec_Value & 0xff;
insert_and_send_data();
}
void MotorControl::write_velocity_acc_dec(const int16_t& Acc_Value, const int16_t& Dec_Value)
{
ID.back() = static_cast<int>(Mode::VELOCITY_ACC_DEC_SET);
ID[2] = motor_id;
Data[0] = Acc_Value >> 24 & 0xff;
Data[1] = Acc_Value >> 16 & 0xff;
Data[2] = Acc_Value >> 8 & 0xff;
Data[3] = Acc_Value & 0xff;
Data[4] = Dec_Value >> 24 & 0xff;
Data[5] = Dec_Value >> 16 & 0xff;
Data[6] = Dec_Value >> 8 & 0xff;
Data[7] = Dec_Value & 0xff;
insert_and_send_data();
}
void MotorControl::write_current_pid(const float& KP, const float& KI)
{
ID.back() = static_cast<int>(Mode::CURRENT_PID_SET);
ID[2] = motor_id;
union buf_kp_ki
{
float f[2];
uint8_t u8[8];
};
buf_kp_ki kp_ki;
kp_ki.f[0] = KP;
kp_ki.f[1] = KI;
Data[0] = kp_ki.u8[3];
Data[1] = kp_ki.u8[2];
Data[2] = kp_ki.u8[1];
Data[3] = kp_ki.u8[0];
Data[4] = kp_ki.u8[7];
Data[5] = kp_ki.u8[6];
Data[6] = kp_ki.u8[5];
Data[7] = kp_ki.u8[4];
insert_and_send_data();
}
void MotorControl::write_velocity_pid(const float& KP, const float& KI)
{
ID.back() = static_cast<int>(Mode::VELOCITY_PID_SET);
ID[2] = motor_id;
union buf_kp_ki
{
float f[2];
uint8_t u8[8];
};
buf_kp_ki kp_ki;
kp_ki.f[0] = KP;
kp_ki.f[1] = KI;
Data[0] = kp_ki.u8[3];
Data[1] = kp_ki.u8[2];
Data[2] = kp_ki.u8[1];
Data[3] = kp_ki.u8[0];
Data[4] = kp_ki.u8[7];
Data[5] = kp_ki.u8[6];
Data[6] = kp_ki.u8[5];
Data[7] = kp_ki.u8[4];
insert_and_send_data();
}
void MotorControl::write_position_pid(const float& KP, const float& KD)
{
ID.back() = static_cast<int>(Mode::POSITION_PID_SET);
ID[2] = motor_id;
union buf_kp_kd
{
float f[2];
uint8_t u8[8];
};
buf_kp_kd kp_kd;
kp_kd.f[0] = KP;
kp_kd.f[1] = KD;
Data[0] = kp_kd.u8[3];
Data[1] = kp_kd.u8[2];
Data[2] = kp_kd.u8[1];
Data[3] = kp_kd.u8[0];
Data[4] = kp_kd.u8[7];
Data[5] = kp_kd.u8[6];
Data[6] = kp_kd.u8[5];
Data[7] = kp_kd.u8[4];
insert_and_send_data();
}
void MotorControl::brake_control(const int16_t& PWM_Value, const uint16_t& Time_Value)
{
ID.back() = static_cast<int>(Mode::BRAKE_CONTROL);
ID[2] = motor_id;
Data[0] = PWM_Value >> 8 & 0xFF;
Data[1] = PWM_Value & 0xFF;
Data[2] = Time_Value >> 8 & 0xFF;
Data[3] = Time_Value & 0xFF;
insert_and_send_data();
}
void MotorControl::insert_and_send_data()
{
vector<uint8_t> data;
data.insert(data.end(), Header.begin(), Header.end());
data.insert(data.end(), ID.begin(), ID.end());
data.insert(data.end(), Data.begin(), Data.end());
send_data(data.data(), data.size());
// for (size_t i = 0; i < data.size(); i++)
// {
// cout << "0X" << hex << setw(2) << setfill('0') << static_cast<int>(data[i]) << " ";
// if (i % 16 == 15) cout << endl;
// }
// cout << endl;
}
void MotorControl::send_data(const uint8_t* data, size_t size)
{
size_t num_send = ser.write(data, size);
if (num_send != size)
{
cout<< "Data send failed. bytes be send: " << num_send << endl;
}
}
MotorStatus MotorControl::get_motor_status(vector<uint8_t> header_types)
{
auto NBytes = 39;
switch (header_types.size())
{
case 1:
case 2:
NBytes = 117;
break;
case 3:
NBytes = 156;
break;
default:
break;
}
clearBuffer();
MotorStatus status;
auto start_time = chrono::steady_clock::now();
vector<uint8_t> feedback;
while (ser.available() < NBytes && chrono::duration_cast<chrono::seconds>(chrono::steady_clock::now() - start_time).count() <= 3)
{
this_thread::sleep_for(chrono::milliseconds(10));
}
if (ser.available() >= NBytes)
{
feedback.resize(ser.available());
ser.read(feedback.data(), feedback.size());
}
for (const auto& header_type : header_types)
{
if (header_type == 0xB1 || header_type == 0xB2 || header_type == 0xB3)
{
if (header_type == 0xB1)
{
vector<uint8_t> header1 = { 0x88, 0x05, 0x06, motor_id, 0xB1 };
size_t start_feedback1 = find_start_of_feedback(feedback, header1);
if (start_feedback1 != string::npos && feedback.size() >= start_feedback1 + 13)
{
vector<uint8_t> packet(feedback.begin() + start_feedback1, feedback.begin() + start_feedback1 + 13);
status.voltage_value = static_cast<int16_t>((packet[5] << 8) | packet[6]);
status.pwm_value = static_cast<int16_t>((packet[7] << 8) | packet[8]);
status.current_value = static_cast<int16_t>((packet[9] << 8) | packet[10]);
status.velocity_value = static_cast<int16_t>((packet[11] << 8) | packet[12]);
}
}
if (header_type == 0xB2)
{
vector<uint8_t> header2 = { 0x88, 0x05, 0x06, motor_id, 0xB2 };
size_t start_feedback2 = find_start_of_feedback(feedback, header2);
if (start_feedback2 != string::npos && feedback.size() >= start_feedback2 + 13)
{
vector<uint8_t> packet(feedback.begin() + start_feedback2, feedback.begin() + start_feedback2 + 13);
status.motor_position_value = static_cast<int32_t>((packet[5] << 24) | (packet[6] << 16) | (packet[7] << 8) | packet[8]);
status.actuator_position_value = static_cast<int32_t>((packet[9] << 24) | (packet[10] << 16) | (packet[11] << 8) | packet[12]);
}
}
if (header_type == 0xB3)
{
vector<uint8_t> header3 = { 0x88, 0x05, 0x06, motor_id, 0xB3 };
size_t start_feedback3 = find_start_of_feedback(feedback, header3);
if (start_feedback3 != string::npos && feedback.size() >= start_feedback3 + 13)
{
vector<uint8_t> packet(feedback.begin() + start_feedback3, feedback.begin() + start_feedback3 + 13);
status.mos_temp = packet[5];
status.mt_temp = packet[6];
status.g431_temp = packet[7];
status.warning_type = packet[8];
status.error_type = packet[9];
}
}
}
else
{
cout << "Only 0xB1, 0xB2, 0xB3 can be entered" << endl;
return {};
}
}
return status;
}
size_t MotorControl::find_start_of_feedback(const vector<uint8_t>& feedback, const vector<uint8_t>& header)
{
for (size_t i = 0; i <= feedback.size() - header.size(); ++i)
{
if (equal(header.begin(), header.end(), feedback.begin() + i))
{
return i;
}
}
return string::npos;
}
控制多执行器：
multi_motor_control.h：
#ifndef MULTI_MOTOR_CONTROL_H
#define MULTI_MOTOR_CONTROL_H
#include <memory>
#include <unordered_map>
#include <stdexcept>
#include <motor_control.h>
class MultiMotorControl:public MotorControl
{
public:
MultiMotorControl(const string& port, const uint32_t& baudrate, const vector<uint8_t>& motor_ids);
//~MultiMotorControl();
void closeport();
void set_object_id(const vector<uint8_t>& new_id);
vector<vector<int>> reset();
void mode_selection(const Mode& mode, const uint8_t& Feedback_cycle1, const uint8_t& Feedback_cycle2, const uint8_t& Feedback_cycle3);
void write_async(const vector<int16_t>& PWM_Values);
void write_open_loop(const vector<int16_t>& PWM_Values);
void write_current(const vector<int16_t>& Current_Values);
void write_profile_velocity(const vector<int16_t>& Current_Values, const vector<int16_t>& Velocity_Values);
void write_profile_position(const vector<int16_t>& Current_Values, const vector<int16_t>& Velocity_Values,
const vector<int32_t>& Position_Values);
void write_position(const vector<int16_t>& Current_Values, const vector<int16_t>& Velocity_Values,
const vector<int32_t>& Position_Values);
void write_mit(const vector<int16_t>& Current_Values, const vector<int16_t>& Velocity_Values, const vector<int32_t>& Position_Values,
const vector<float>& KPs, const vector<float>& KIs);
void write_openloop_acc_dec(const vector<int16_t>& Acc_Value, const vector<int16_t>& Dec_Value);
void write_velocity_acc_dec(const vector<int16_t>& Acc_Values, const vector<int16_t>& Dec_Values);
void write_current_pid(const vector<float>& KPs, const vector<float>& KIs);
void write_velocity_pid(const vector<float>& KPs, const vector<float>& KIs);
void write_position_pid(const vector<float>& KPs, const vector<float>& KDs);
void brake_control(const vector<int16_t>& PWM_Values, const vector<uint16_t>& Time_Values);
vector<MotorStatus> get_motor_status(vector<uint8_t> header_types);
private:
serial::Serial ser;
vector<uint8_t> motor_ids_;
template <typename T>
void validate_param_size(const vector<T>& values, const string& param_name)
{
if (values.size() != motor_ids_.size() && values.size() != 1)
{
throw invalid_argument(param_name + " size must match the number of motors or be 1");
}
}
};
#endif
multi_motor_control.cpp：
#include <multi_motor_control.h>
MultiMotorControl::MultiMotorControl(const string& port, const uint32_t& baudrate, const vector<uint8_t>& motor_ids)
:MotorControl(port, baudrate, motor_ids[0]), motor_ids_(motor_ids)
{}
void MultiMotorControl::closeport()
{
MotorControl::closeport();
}
void MultiMotorControl::set_object_id(const vector<uint8_t>& new_id)
{
motor_ids_ = new_id;
}
vector<vector<int>> MultiMotorControl::reset()
{
vector<vector<int>> results;
for (uint8_t id : motor_ids_)
{
MotorControl::set_object_id(id);
vector<int> result = MotorControl::reset();
results.push_back(result);
}
return results;
}
void MultiMotorControl::mode_selection(const Mode& mode, const uint8_t& Feedback_cycle1, const uint8_t& Feedback_cycle2, const uint8_t& Feedback_cycle3)
{
for (uint8_t id : motor_ids_)
{
MotorControl::set_object_id(id);
MotorControl::mode_selection(mode, Feedback_cycle1, Feedback_cycle2, Feedback_cycle3);
}
}
void MultiMotorControl::write_async(const vector<int16_t>& PWM_Values)
{
validate_param_size(PWM_Values, "PWM_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t PWM_Value = PWM_Values.size() == 1 ? PWM_Values[0] : PWM_Values[i];
MotorControl::set_object_id(id);
MotorControl::write_async(PWM_Value);
}
}
void MultiMotorControl::write_open_loop(const vector<int16_t>& PWM_Values)
{
validate_param_size(PWM_Values, "PWM_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t PWM_Value = PWM_Values.size() == 1 ? PWM_Values[0] : PWM_Values[i];
MotorControl::set_object_id(id);
MotorControl::write_open_loop(PWM_Value);
}
}
void MultiMotorControl::write_current(const vector<int16_t>& Current_Values)
{
validate_param_size(Current_Values, "Current_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t Current_Value = Current_Values.size() == 1 ? Current_Values[0] : Current_Values[i];
MotorControl::set_object_id(id);
MotorControl::write_open_loop(Current_Value);
}
}
void MultiMotorControl::write_profile_velocity(const vector<int16_t>& Current_Values, const vector<int16_t>& Velocity_Values)
{
validate_param_size(Current_Values, "Current_Values");
validate_param_size(Velocity_Values, "Velocity_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t Current_Value = Current_Values.size() == 1 ? Current_Values[0] : Current_Values[i];
int16_t Velocity_Value = Velocity_Values.size() == 1 ? Velocity_Values[0] : Velocity_Values[i];
MotorControl::set_object_id(id);
MotorControl::write_profile_velocity(Current_Value, Velocity_Value);
}
}
void MultiMotorControl::write_profile_position(const vector<int16_t>& Current_Values, const vector<int16_t>& Velocity_Values,
const vector<int32_t>& Position_Values)
{
validate_param_size(Current_Values, "Current_Values");
validate_param_size(Velocity_Values, "Velocity_Values");
validate_param_size(Position_Values, "Position_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t Current_Value = Current_Values.size() == 1 ? Current_Values[0] : Current_Values[i];
int16_t Velocity_Value = Velocity_Values.size() == 1 ? Velocity_Values[0] : Velocity_Values[i];
int32_t Positin_Value = Position_Values.size() == 1 ? Position_Values[0] : Position_Values[i];
MotorControl::set_object_id(id);
MotorControl::write_profile_position(Current_Value, Velocity_Value, Positin_Value);
}
}
void MultiMotorControl::write_position(const vector<int16_t>& Current_Values, const vector<int16_t>& Velocity_Values,
const vector<int32_t>& Position_Values)
{
validate_param_size(Current_Values, "Current_Values");
validate_param_size(Velocity_Values, "Velocity_Values");
validate_param_size(Position_Values, "Position_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t Current_Value = Current_Values.size() == 1 ? Current_Values[0] : Current_Values[i];
int16_t Velocity_Value = Velocity_Values.size() == 1 ? Velocity_Values[0] : Velocity_Values[i];
int32_t Positin_Value = Position_Values.size() == 1 ? Position_Values[0] : Position_Values[i];
MotorControl::set_object_id(id);
MotorControl::write_position(Current_Value, Velocity_Value, Positin_Value);
}
}
void MultiMotorControl::write_mit(const vector<int16_t>& Current_Values, const vector<int16_t>& Velocity_Values, const vector<int32_t>& Position_Values,
const vector<float>& KPs, const vector<float>& KIs)
{
validate_param_size(Current_Values, "Current_Values");
validate_param_size(Velocity_Values, "Velocity_Values");
validate_param_size(Position_Values, "Positin_Values");
validate_param_size(KPs, "KPs");
validate_param_size(KIs, "KIs");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t Current_Value = Current_Values.size() == 1 ? Current_Values[0] : Current_Values[i];
int16_t Velocity_Value = Velocity_Values.size() == 1 ? Velocity_Values[0] : Velocity_Values[i];
int32_t Positin_Value = Position_Values.size() == 1 ? Position_Values[0] : Position_Values[i];
float KP = KPs.size() == 1 ? KPs[0] : KPs[i];
float KI = KIs.size() == 1 ? KIs[0] : KIs[i];
MotorControl::set_object_id(id);
MotorControl::write_mit(Current_Value, Velocity_Value, Positin_Value, KP, KI);
}
}
void MultiMotorControl::write_openloop_acc_dec(const vector<int16_t>& Acc_Values, const vector<int16_t>& Dec_Values)
{
validate_param_size(Acc_Values, "Acceleration_Values");
validate_param_size(Dec_Values, "Deceleration_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t Acc_Value = Acc_Values.size() == 1 ? Acc_Values[0] : Acc_Values[i];
int16_t Dec_Value = Dec_Values.size() == 1 ? Dec_Values[0] : Dec_Values[i];
MotorControl::set_object_id(id);
MotorControl::write_openloop_acc_dec(Acc_Value, Dec_Value);
}
}
void MultiMotorControl::write_velocity_acc_dec(const vector<int16_t>& Acc_Values, const vector<int16_t>& Dec_Values)
{
validate_param_size(Acc_Values, "Acceleration_Values");
validate_param_size(Dec_Values, "Deceleration_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t Acc_Value = Acc_Values.size() == 1 ? Acc_Values[0] : Acc_Values[i];
int16_t Dec_Value = Dec_Values.size() == 1 ? Dec_Values[0] : Dec_Values[i];
MotorControl::set_object_id(id);
MotorControl::write_velocity_acc_dec(Acc_Value, Dec_Value);
}
}
void MultiMotorControl::write_current_pid(const vector<float>& KPs, const vector<float>& KIs)
{
validate_param_size(KPs, "KPs");
validate_param_size(KIs, "KIs");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
float KP = KPs.size() == 1 ? KPs[0] : KPs[i];
float KI = KIs.size() == 1 ? KIs[0] : KIs[i];
MotorControl::set_object_id(id);
MotorControl::write_current_pid(KP, KI);
}
}
void MultiMotorControl::write_velocity_pid(const vector<float>& KPs, const vector<float>& KIs)
{
validate_param_size(KPs, "KPs");
validate_param_size(KIs, "KIs");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
float KP = KPs.size() == 1 ? KPs[0] : KPs[i];
float KI = KIs.size() == 1 ? KIs[0] : KIs[i];
MotorControl::set_object_id(id);
MotorControl::write_velocity_pid(KP, KI);
}
}
void MultiMotorControl::write_position_pid(const vector<float>& KPs, const vector<float>& KDs)
{
validate_param_size(KPs, "KPs");
validate_param_size(KDs, "KIs");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
float KP = KPs.size() == 1 ? KPs[0] : KPs[i];
float KD = KDs.size() == 1 ? KDs[0] : KDs[i];
MotorControl::set_object_id(id);
MotorControl::write_position_pid(KP, KD);
}
}
void MultiMotorControl::brake_control(const vector<int16_t>& PWM_Values, const vector<uint16_t>& Time_Values)
{
validate_param_size(PWM_Values, "PWM_Values");
validate_param_size(Time_Values, "Time_Values");
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
uint8_t id = motor_ids_[i];
int16_t PWM_Value = PWM_Values.size() == 1 ? PWM_Values[0] : PWM_Values[i];
int16_t Time_Value = Time_Values.size() == 1 ? Time_Values[0] : Time_Values[i];
MotorControl::set_object_id(id);
MotorControl::brake_control(PWM_Value, Time_Value);
}
}
vector<MotorStatus> MultiMotorControl::get_motor_status(vector<uint8_t> header_types)
{
vector<MotorStatus> all_motor_status;
for (size_t i = 0; i < motor_ids_.size(); ++i)
{
MotorStatus status;
uint8_t id = motor_ids_[i];
MotorControl::set_object_id(id);
status = MotorControl::get_motor_status({ header_types });
all_motor_status.push_back(status);
}
return all_motor_status;
}