CAN2.0B communication protocol
Serial Frame to CAN Frame Conversion
- During the process of converting serial frames to CAN messages, if any segment of the fixed-length serial data frame (13 bytes per frame) does not follow the standard format, that segment will not be converted, and the conversion will continue with the following data. If some CAN messages are missing after conversion, please check whether the corresponding serial data frame with fixed byte length matches the standard format.
- When converting frame data to CAN format, the length is fixed at 8 bytes. The effective length is determined by the values of DLC3–DLC0. If the effective data is shorter than the fixed length, zeros must be padded to reach the fixed length.
- In this mode, the serial data must strictly follow the fixed-byte format in order to be successfully converted. For CAN mode conversion, please refer to the example below. Ensure that the frame information and data length indication are correct before conversion; otherwise, the conversion will not proceed.
- 例子：
Serial Frame
88
17
65
43
21
99
88
77
66
55
44
33
22
↓
CAN Frame
Frame Info
Frame ID
Frame Data
88
17
65
43
21
99
88
77
66
55
44
33
22
Note: The header byte in the message here is required by our company’s USB-to-CAN module. If you are not using our USB-to-CAN module, this byte can be ignored, and only 12 bytes need to be sent.
Control Frame
- The control frame is sent from the master controller to the actuator for mode selection, parameter configuration, and motion control.
- Type of Frame：DATA
- Format of Frame：EXTENDED
- DLC: 8
Control Function | Default frame ID | DATA | |||||||
Data[0] | Data[1] | Data[2] | Data[3] | Data[4] | Data[5] | Data[6] | Data[7] | ||
Reset |
|
|
|
|
|
|
|
|
|
Restore Factory Settings |
|
|
|
|
|
|
|
|
|
Select Current Mode |
|
|
|
|
|
|
|
|
|
Set Phase Current (Current mode) |
| 16-bit phase current data content, using big-endian mode |
|
|
|
|
|
| |
Select Profile Velocity Mode |
|
|
|
|
|
|
|
|
|
Set Phase Current & Velocity (Profile Velocity Mode) |
| 16-bit phase current(limit current) data content, using big-endian mode | 16-bit velocity data content, using big-endian mode |
|
|
|
| ||
Select Profile Position Mode |
|
|
|
|
|
|
|
|
|
Set Phase Current & Velocity & Position (Profile Position Mode) |
| 16-bit phase current(limit current) data content, using big-endian mode | 16-bit velocity(limit velocity) data content, using big-endian mode | 32-bit position data content, using big-endian mode | |||||
Select Position Mode |
|
|
|
|
|
|
|
|
|
Set Phase Current & Velocity & Position (Position Mode) |
| 16-bit phase current(limit current) data content, using big-endian mode | 16-bit velocity(limit velocity) data content, using big-endian mode | 32-bit position data content, using big-endian mode | |||||
Select MIT Mode |
|
|
|
|
|
|
|
|
|
Set Phase Current & Velocity & Position (MIT mode) |
| 16-bit phase current data content, using big-endian mode | 16-bit velocity data content, using big-endian mode | 32-bit position data content, using big-endian mode | |||||
Select Openloop Mode |
|
|
|
|
|
|
|
|
|
Set Velocity (Openloop Mode) |
| 16-bit velocity data content, using big-endian mode，unit:Pul |
|
|
|
|
|
| |
Set Brake PWM and Duration |
| 16-bit PWM data content, using big-endian mode | 16-bit time data content, using big-endian mode |
|
|
|
| ||
Note: The following are real-time parameter modification instructions, which modify the RAM value. They are sent after entering the running mode. If they are not sent, the value in the Flash is called. | |||||||||
Real-time Setting of Current Loop PI（Current Mode & Profile Velocity Mode & Profile Position Mode&Position Mode） |
| 32-bit float type current loop coefficient Kp parameter | 32-bit float type current loop coefficient Ki parameter | ||||||
Real-time Setting of Velocity loop PI (Profile Velocity Mode & Profile Position Mode&Position Mode） |
| 32-bit float type velocity loop coefficient Kp parameter | 32-bit float type velocity loop coefficient Ki parameter | ||||||
Real-time Setting of Position Loop PD (Profile Position Mode&Position Mode） |
| 32-bit float type position loop coefficient Kp parameter | 32-bit float type position loop coefficient Kd parameter | ||||||
Real-time setting of Kp & Kd (MIT Mode) |
| 32-bit float type position coefficient (Kp) parameter | 32-bit float type velocity coefficient (Kd) parameter | ||||||
Real-time acceleration and deceleration setting (Open-loop mode) |
| 32-bit acceleration data in big-endian format, unit: Pul/s | 32-bit deceleration data in big-endian format, unit: Pul/s | ||||||
Real-time acceleration and deceleration setting (Profile Velocity Mode & Profile Position Mode & Position Mode） |
| 32-bit acceleration data in big-endian format, unit: rpm/s | 32-bit deceleration data in big-endian format, unit: rpm/s | ||||||
（1）Feedback Frame
- The feedback frame is sent from the actuator to the master controller to provide feedback on various statuses.
- Type of Frame：DATA
- Format of Frame：EXTENDED
- DLC:8
Type of Feedback | Default frame ID | DATA | |||||||
Data[0] | Data[1] | Data[2] | Data[3] | Data[4] | Data[5] | Data[6] | Data[7] | ||
Feedback 0 |
| 32-bit serial number | 32-bit firmware version date | ||||||
Feedback 1 |
| 16-bit Voltage | 16-bit PWM | 16-bit Phase Current | 16-bit Velocity | ||||
Feedback 2 |
| 32-bit Motor Position | 32-bit Reducer Position | ||||||
Feedback 3 |
| Mosfet temperature | Motor temperature | MCU temperature | Warning Type | Error Type | No Definition | No Definition | Operating Mode |
Error types include:
Error Type | |
|---|---|
| Power Supply Under Voltage |
| Power Supply Over Voltage |
| Motor Over Current |
| Mosfet Over Temperature |
| Motor Over Temperature |
| MCU Over Temperature |
Warning Type | |
| CAN Watch Dog Time Out |
Read and write register values
Control Function | Default frame ID | DATA | |||||||
Data[0] | Data[1] | Data[2] | Data[3] | Data[4] | Data[5] | Data[6] | Data[7] | ||
Select EEPROM Mode |
|
|
|
|
|
|
|
|
|
Read Register Value |
|
|
|
|
|
|
|
|
|
Write Register Value |
|
|
| 32-bit data content, using big-endian mode |
|
| |||
Motor Parament Calibration |
|
|
|
|
|
|
|
|
|
Definition | Register Adress | Default Value | Range | Unit |
|---|---|---|---|---|
Current Loop_Kp |
| 0.0006 | ||
Current Loop_Ki |
| 0.0001 | ||
Velocity Loop_Kp |
| 50 | ||
Velocity Loop_Ki |
| 1.2 | ||
Position Loop_Kp |
| 0.008 | ||
Position Loop_Kd |
| 0 | ||
MIT_Kp |
| 0.09 | ||
MIT_Kd |
| 13.1 | ||
CAN ID Start |
| 05060000 | 0-256 | |
Motor Rotation Direction |
| Auto-Set | +1/-1 | |
Encoder Selection for Each Positon Mode |
| 1 | 1:Reducer | |
reduction ratio Numerator |
| 31/51/101 | ||
reduction ratio Denominator |
| 1 | ||
Reducer Encoder Zero Bias |
| 0 | ||
Auto Return Zero Point |
| 1 | 0:No | |
Openloop Mode Acceleration |
| 2000 | Pul/s | |
Openloop Mode Deceleration |
| 2000 | Pul/s | |
Acceleration settings in profile velocity mode and profile position modes |
| 2000 | rpm/s | |
Deceleration settings in profile velocity mode and profile position modes |
| 2000 | rpm/s | |
Max Phase Current Limt（MIT Mode） |
| 10000 | ma | |
Max Velocity Limt |
| 4000 | rpm | |
Negative Position Limit |
| -2147480000 | ||
Positive Position Limit |
| 2147480000 | ||
Under Voltage Protection Value |
| 20 | V | |
Over Voltage Protection Value |
| 60 | V | |
Mosfet Shutdown Temperature |
| 90 | ℃ | |
Motor Shutdown Temperature |
| 90 | ℃ | |
MCU Shutdown Temperature |
| 90 | ℃ | |
CAN Watch Dog Period |
| 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;
}