137 lines
4.6 KiB
C++
137 lines
4.6 KiB
C++
#include "../2025_include_user/Stepmotor.h"
|
||
|
||
uint8_t StepMotor::_buffer = 0;
|
||
StepMotor::Data StepMotor::_data = {};
|
||
static uint8_t frame_index = 0;
|
||
static uint8_t frame_buffer[9];
|
||
|
||
extern "C" void Stepmotor_INST_IRQHandler(void) {
|
||
switch (DL_UART_getPendingInterrupt(Stepmotor_INST)) {
|
||
case DL_UART_IIDX_RX:
|
||
StepMotor::_buffer = DL_UART_Main_receiveData(Stepmotor_INST);
|
||
StepMotor::inject();
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
NVIC_ClearPendingIRQ(Stepmotor_INST_INT_IRQN);
|
||
DL_UART_enableInterrupt(Stepmotor_INST, DL_UART_INTERRUPT_RX); // 启用接收中断
|
||
}
|
||
|
||
void StepMotor::init() {
|
||
NVIC_ClearPendingIRQ(Stepmotor_INST_INT_IRQN);
|
||
NVIC_EnableIRQ(Stepmotor_INST_INT_IRQN);
|
||
DL_UART_enable(Stepmotor_INST); // 启用 UART 模块
|
||
DL_UART_enableInterrupt(Stepmotor_INST, DL_UART_INTERRUPT_RX); // 启用接收中断
|
||
}
|
||
|
||
void StepMotor::inject() {
|
||
frame_buffer[frame_index++] = _buffer;
|
||
|
||
if (frame_index >= 9) {
|
||
// 校验BCC
|
||
uint8_t bcc = 0;
|
||
for (int i = 0; i < 8; i++) {
|
||
bcc ^= frame_buffer[i];
|
||
}
|
||
|
||
if (bcc == frame_buffer[8]) {
|
||
// BCC校验通过，解析数据存储到ttl_data
|
||
_data.address = frame_buffer[0];
|
||
_data.flag = frame_buffer[1];
|
||
_data.speed = (frame_buffer[2] << 8) | frame_buffer[3];
|
||
uint32_t position_raw = (frame_buffer[4] << 24) | (frame_buffer[5] << 16) | (frame_buffer[6] << 8) | frame_buffer[7];
|
||
|
||
// 位置数据处理为有符号值（高位为符号位）
|
||
if (position_raw & 0x80000000) {
|
||
_data.position = (int32_t)(position_raw | 0xFFFFFFFF00000000);
|
||
} else {
|
||
_data.position = (int32_t)position_raw;
|
||
}
|
||
|
||
_data.bcc = frame_buffer[8];
|
||
}
|
||
|
||
// 无论校验是否通过，都重置frame_index，准备接收下一帧
|
||
frame_index = 0;
|
||
}
|
||
}
|
||
|
||
void StepMotor::transmit_command(const uint8_t *cmd) {
|
||
for (uint8_t i = 0; i < FRAME_LENGTH; i++) {
|
||
DL_UART_Main_transmitDataBlocking(Stepmotor_INST, cmd[i]);
|
||
}
|
||
}
|
||
|
||
|
||
// 通用编码函数
|
||
void StepMotor::encode_frame(const StepMotor::ControlMode mode, const uint8_t direction, const uint8_t subdivide,
|
||
const float angle_deg, const uint16_t torque_mA, const float absolute_angle_deg,
|
||
const float speed_rad_s, uint8_t *frame_buffer) {
|
||
uint16_t angle_or_current = 0;
|
||
uint16_t speed = (uint16_t) (speed_rad_s * 10); // 转速 *10 放大
|
||
|
||
frame_buffer[0] = 0x7B; // 帧头
|
||
frame_buffer[1] = 0x01; // 控制ID
|
||
frame_buffer[2] = mode; // 控制模式
|
||
frame_buffer[3] = direction; // 方向
|
||
frame_buffer[4] = subdivide; // 细分
|
||
|
||
if (mode == StepMotor::POSITION_MODE) {
|
||
angle_or_current = (uint16_t) (angle_deg * 10); // 角度 *10 放大
|
||
} else if (mode != StepMotor::TORQUE_MODE) {
|
||
angle_or_current = torque_mA; // 电流mA
|
||
} else if (mode == StepMotor::ABSOLUTE_ANGLE_MODE) {
|
||
angle_or_current = (uint16_t) (absolute_angle_deg * 10); // 角度 *10 放大
|
||
} else {
|
||
angle_or_current = 0; // 速度模式时为0
|
||
}
|
||
|
||
frame_buffer[5] = (angle_or_current >> 8) & 0xFF;
|
||
frame_buffer[6] = angle_or_current & 0xFF;
|
||
|
||
frame_buffer[7] = (speed >> 8) & 0xFF;
|
||
frame_buffer[8] = speed & 0xFF;
|
||
|
||
uint8_t bcc = 0;
|
||
for (int i = 0; i < 9; i++) {
|
||
bcc ^= frame_buffer[i];
|
||
}
|
||
frame_buffer[9] = bcc;
|
||
|
||
frame_buffer[10] = 0x7D; // 帧尾
|
||
}
|
||
|
||
void StepMotor::setSpeed(uint8_t direction, uint8_t subdivide, float speed_rad_s) {
|
||
uint8_t frame[FRAME_LENGTH];
|
||
encode_frame(SPEED_MODE, direction, subdivide, 0, 0, 0, speed_rad_s, frame);
|
||
transmit_command(frame);
|
||
}
|
||
|
||
void StepMotor::setPosition(uint8_t direction, uint8_t subdivide, float angle_deg, float speed_rad_s) {
|
||
uint8_t frame[FRAME_LENGTH];
|
||
encode_frame(POSITION_MODE, direction, subdivide, angle_deg, 0, 0, speed_rad_s, frame);
|
||
transmit_command(frame);
|
||
}
|
||
|
||
void StepMotor::setTorque(uint8_t direction, uint8_t subdivide, uint16_t torque_mA, float speed_rad_s) {
|
||
uint8_t frame[FRAME_LENGTH];
|
||
encode_frame(TORQUE_MODE, direction, subdivide, 0, torque_mA, 0, speed_rad_s, frame);
|
||
transmit_command(frame);
|
||
}
|
||
|
||
void StepMotor::setAbsoluteAngle(uint8_t direction, uint8_t subdivide, float absolute_angle_deg, float speed_rad_s) {
|
||
uint8_t frame[FRAME_LENGTH];
|
||
encode_frame(ABSOLUTE_ANGLE_MODE, direction, subdivide, 0, 0, absolute_angle_deg, speed_rad_s, frame);
|
||
transmit_command(frame);
|
||
}
|
||
|
||
uint16_t StepMotor::getSpeed() {
|
||
return _data.speed;
|
||
}
|
||
|
||
uint32_t StepMotor::getPosition() {
|
||
return _data.position;
|
||
}
|