refactor: migrate from DJI motors to DM motors, simplify to gimbal-only control
- Rewrite DM motor driver based on DM_6220 reference protocol (MIT/PosVel/Velocity modes) - Use velocity control mode: MCU runs angle PID with IMU feedback, motor handles speed loop - Rewrite vision protocol with 3 frame types (0xA5 auto data, 0xA6 manual data, 0xA7 command) - Decouple serial parsing (opencv module) from mode switching (robot_cmd module) - Implement dual-loop architecture: vision PID outer loop → angle ref → motor PID inner loop - Add KF/PID reset on mode switch transitions - Merge motor_reverse_flag and feedback_reverse_flag into single output_reverse flag - Remove DMMotorEnable/DMMotorStop (motor always runs, controlled by angle ref) - Remove unused modules: DJI/LK/HT/step/servo motors, chassis, shoot, referee - Add ExitRun0Mode() for STM32H723 power supply configuration - Clean up CMakeLists: 115 → 76 C source files
This commit is contained in:
+187
-129
@@ -1,23 +1,23 @@
|
||||
#include "dmmotor.h"
|
||||
#include "memory.h"
|
||||
#include "general_def.h"
|
||||
#include "user_lib.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "string.h"
|
||||
#include "daemon.h"
|
||||
#include "stdlib.h"
|
||||
#include "bsp_log.h"
|
||||
#include "bsp_dwt.h"
|
||||
|
||||
static uint8_t idx;
|
||||
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
|
||||
static osThreadId dm_task_handle[DM_MOTOR_CNT];
|
||||
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
|
||||
|
||||
/* ---- float <-> uint conversion for MIT mode ---- */
|
||||
|
||||
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
|
||||
{
|
||||
float span = x_max - x_min;
|
||||
float offset = x_min;
|
||||
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
|
||||
}
|
||||
|
||||
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
||||
{
|
||||
float span = x_max - x_min;
|
||||
@@ -25,74 +25,155 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
||||
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
|
||||
}
|
||||
|
||||
/* ---- Low-level CAN send functions (all three modes) ---- */
|
||||
|
||||
/* MIT mode: CAN ID = motor_id */
|
||||
void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq)
|
||||
{
|
||||
uint16_t pos_uint = float_to_uint(pos, DM_P_MIN, DM_P_MAX, 16);
|
||||
uint16_t vel_uint = float_to_uint(vel, DM_V_MIN, DM_V_MAX, 12);
|
||||
uint16_t kp_uint = float_to_uint(kp, DM_KP_MIN, DM_KP_MAX, 12);
|
||||
uint16_t kd_uint = float_to_uint(kd, DM_KD_MIN, DM_KD_MAX, 12);
|
||||
uint16_t torq_uint = float_to_uint(torq, DM_T_MIN, DM_T_MAX, 12);
|
||||
|
||||
uint8_t *buf = can->tx_buff;
|
||||
buf[0] = (uint8_t)(pos_uint >> 8);
|
||||
buf[1] = (uint8_t)(pos_uint);
|
||||
buf[2] = (uint8_t)(vel_uint >> 4);
|
||||
buf[3] = (uint8_t)(((vel_uint & 0xF) << 4) | (kp_uint >> 8));
|
||||
buf[4] = (uint8_t)(kp_uint);
|
||||
buf[5] = (uint8_t)(kd_uint >> 4);
|
||||
buf[6] = (uint8_t)(((kd_uint & 0xF) << 4) | (torq_uint >> 8));
|
||||
buf[7] = (uint8_t)(torq_uint);
|
||||
|
||||
CANTransmit(can, 1);
|
||||
}
|
||||
|
||||
/* Position-Velocity mode: CAN ID = motor_id + 0x100 */
|
||||
void DMMotorSendPosVel(CANInstance *can, float pos, float vel)
|
||||
{
|
||||
uint8_t *buf = can->tx_buff;
|
||||
uint8_t *pos_bytes = (uint8_t *)&pos;
|
||||
uint8_t *vel_bytes = (uint8_t *)&vel;
|
||||
|
||||
buf[0] = pos_bytes[0];
|
||||
buf[1] = pos_bytes[1];
|
||||
buf[2] = pos_bytes[2];
|
||||
buf[3] = pos_bytes[3];
|
||||
buf[4] = vel_bytes[0];
|
||||
buf[5] = vel_bytes[1];
|
||||
buf[6] = vel_bytes[2];
|
||||
buf[7] = vel_bytes[3];
|
||||
|
||||
/* Temporarily change tx_id to motor_id + 0x100 for this transmission */
|
||||
uint32_t saved_id = can->tx_id;
|
||||
can->tx_id = saved_id + 0x100;
|
||||
CANTransmit(can, 1);
|
||||
can->tx_id = saved_id;
|
||||
}
|
||||
|
||||
/* Velocity mode: CAN ID = motor_id + 0x200 */
|
||||
void DMMotorSendVelocity(CANInstance *can, float vel)
|
||||
{
|
||||
uint8_t *buf = can->tx_buff;
|
||||
uint8_t *vel_bytes = (uint8_t *)&vel;
|
||||
|
||||
buf[0] = vel_bytes[0];
|
||||
buf[1] = vel_bytes[1];
|
||||
buf[2] = vel_bytes[2];
|
||||
buf[3] = vel_bytes[3];
|
||||
buf[4] = 0;
|
||||
buf[5] = 0;
|
||||
buf[6] = 0;
|
||||
buf[7] = 0;
|
||||
|
||||
uint32_t saved_id = can->tx_id;
|
||||
can->tx_id = saved_id + 0x200;
|
||||
CANTransmit(can, 1);
|
||||
can->tx_id = saved_id;
|
||||
}
|
||||
|
||||
/* ---- Motor commands (enable/disable/zero/clear error) ---- */
|
||||
|
||||
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
|
||||
{
|
||||
memset(motor->motor_can_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
|
||||
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
|
||||
CANTransmit(motor->motor_can_instace, 1);
|
||||
memset(motor->motor_can_instance->tx_buff, 0xFF, 7);
|
||||
motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd;
|
||||
CANTransmit(motor->motor_can_instance, 1);
|
||||
}
|
||||
|
||||
/* ---- Feedback decode: CAN RX interrupt callback ---- */
|
||||
|
||||
static void DMMotorDecode(CANInstance *motor_can)
|
||||
{
|
||||
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
|
||||
uint8_t *rxbuff = motor_can->rx_buff;
|
||||
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
|
||||
DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
|
||||
DM_Motor_Measure_s *measure = &motor->measure;
|
||||
uint16_t tmp;
|
||||
|
||||
DaemonReload(motor->motor_daemon);
|
||||
measure->id = rxbuff[0] & 0x0F; // 低 4 位是控制器 ID
|
||||
measure->state = (rxbuff[0] >> 4) & 0x0F; // ERR 存储在 state 字段
|
||||
|
||||
measure->id = rxbuff[0] & 0x0F;
|
||||
measure->state = (rxbuff[0] >> 4) & 0x0F;
|
||||
|
||||
measure->last_position = measure->position;
|
||||
|
||||
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
|
||||
measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
|
||||
|
||||
tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
|
||||
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
|
||||
measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
|
||||
|
||||
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
|
||||
tmp = (uint16_t)(((rxbuff[4] & 0x0F) << 8) | rxbuff[5]);
|
||||
measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
|
||||
|
||||
measure->T_Mos = (float)rxbuff[6];
|
||||
measure->T_Mos = (float)rxbuff[6];
|
||||
measure->T_Rotor = (float)rxbuff[7];
|
||||
// 多圈角度计算
|
||||
if (measure->position - measure->last_position > 12.5)
|
||||
measure->total_round--;
|
||||
else if (measure->position - measure->last_position < -12.5)
|
||||
measure->total_round++;
|
||||
measure->total_angle = measure->position + measure->total_round * 25;
|
||||
|
||||
measure->total_angle = measure->total_angle * RAD_2_DEGREE;
|
||||
measure->velocity = measure->velocity * RAD_2_DEGREE;
|
||||
/* multi-turn tracking */
|
||||
if (measure->position - measure->last_position > 10.0f)
|
||||
measure->total_round--;
|
||||
else if (measure->position - measure->last_position < -10.0f)
|
||||
measure->total_round++;
|
||||
|
||||
measure->total_angle = (measure->position + measure->total_round * 25.1327f) * RAD_2_DEGREE;
|
||||
measure->velocity = measure->velocity * RAD_2_DEGREE;
|
||||
}
|
||||
|
||||
static void DMMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
|
||||
uint16_t can_bus = motor->motor_can_instace->can_handle == &hcan1 ? 1 : 2;
|
||||
LOGWARNING("[DMmotor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instace->tx_id);
|
||||
LOGWARNING("[DMmotor] Motor lost, id [%d]", motor->motor_can_instance->tx_id);
|
||||
}
|
||||
void DMMotorCaliEncoder(DMMotorInstance *motor)
|
||||
{
|
||||
DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
|
||||
DWT_Delay(0.1);
|
||||
}
|
||||
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
|
||||
|
||||
/* ---- Init ---- */
|
||||
|
||||
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
|
||||
{
|
||||
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
|
||||
memset(motor, 0, sizeof(DMMotorInstance));
|
||||
|
||||
motor->motor_settings = config->controller_setting_init_config;
|
||||
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
|
||||
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
motor->motor_settings = config->motor_config.controller_setting_init_config;
|
||||
motor->ctrl_mode = config->ctrl_mode;
|
||||
motor->mit_kp = config->mit_kp;
|
||||
motor->mit_kd = config->mit_kd;
|
||||
|
||||
config->can_init_config.can_module_callback = DMMotorDecode;
|
||||
config->can_init_config.id = motor;
|
||||
motor->motor_can_instace = CANRegister(&config->can_init_config);
|
||||
/* init PID controllers */
|
||||
PIDInit(&motor->motor_controller.angle_PID, &config->motor_config.controller_param_init_config.angle_PID);
|
||||
PIDInit(&motor->motor_controller.speed_PID, &config->motor_config.controller_param_init_config.speed_PID);
|
||||
PIDInit(&motor->motor_controller.current_PID, &config->motor_config.controller_param_init_config.current_PID);
|
||||
|
||||
motor->motor_controller.other_angle_feedback_ptr = config->motor_config.controller_param_init_config.other_angle_feedback_ptr;
|
||||
motor->motor_controller.other_speed_feedback_ptr = config->motor_config.controller_param_init_config.other_speed_feedback_ptr;
|
||||
motor->motor_controller.speed_feedforward_ptr = config->motor_config.controller_param_init_config.speed_feedforward_ptr;
|
||||
motor->motor_controller.current_feedforward_ptr = config->motor_config.controller_param_init_config.current_feedforward_ptr;
|
||||
|
||||
/* register CAN instance */
|
||||
config->motor_config.can_init_config.can_module_callback = DMMotorDecode;
|
||||
config->motor_config.can_init_config.id = motor;
|
||||
motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config);
|
||||
|
||||
/* register daemon */
|
||||
Daemon_Init_Config_s conf = {
|
||||
.callback = DMMotorLostCallback,
|
||||
.owner_id = motor,
|
||||
@@ -100,133 +181,110 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
|
||||
};
|
||||
motor->motor_daemon = DaemonRegister(&conf);
|
||||
|
||||
DMMotorEnable(motor);
|
||||
/* enable motor */
|
||||
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
|
||||
// DWT_Delay(0.1);
|
||||
// DMMotorCaliEncoder(motor);
|
||||
// DWT_Delay(0.1);
|
||||
|
||||
dm_motor_instance[idx++] = motor;
|
||||
return motor;
|
||||
}
|
||||
|
||||
/* ---- Runtime control ---- */
|
||||
|
||||
void DMMotorSetRef(DMMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->pid_ref = ref;
|
||||
}
|
||||
|
||||
void DMMotorEnable(DMMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
void DMMotorStop(DMMotorInstance *motor) // 不使用使能模式是因为需要收到反馈
|
||||
{
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
|
||||
{
|
||||
motor->motor_settings.outer_loop_type = type;
|
||||
}
|
||||
|
||||
//@Todo: 目前只实现了力控，更多位控PID等请自行添加
|
||||
void DMMotorTask(void const *argument)
|
||||
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode)
|
||||
{
|
||||
float pid_ref, set;
|
||||
DMMotorInstance *motor = (DMMotorInstance *)argument;
|
||||
DM_Motor_Measure_s *measure = &motor->measure;
|
||||
Motor_Control_Setting_s *setting = &motor->motor_settings;
|
||||
// CANInstance *motor_can = motor->motor_can_instace;
|
||||
// uint16_t tmp;
|
||||
DMMotor_Send_s motor_send_mailbox;
|
||||
float pid_measure; // 电机PID测量值和设定值
|
||||
while (1)
|
||||
motor->ctrl_mode = mode;
|
||||
}
|
||||
|
||||
/* ---- Main control: called from Motor Task at 1kHz ---- */
|
||||
|
||||
void DMMotorControl(void)
|
||||
{
|
||||
for (uint8_t i = 0; i < idx; i++)
|
||||
{
|
||||
if(motor->measure.state == 0x00)
|
||||
{
|
||||
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
|
||||
osDelay(20);
|
||||
}
|
||||
if(motor->measure.state != 0x00&&motor->measure.state != 0x01)
|
||||
DMMotorInstance *motor = dm_motor_instance[i];
|
||||
DM_Motor_Measure_s *measure = &motor->measure;
|
||||
Motor_Control_Setting_s *setting = &motor->motor_settings;
|
||||
Motor_Controller_s *ctrl = &motor->motor_controller;
|
||||
CANInstance *can = motor->motor_can_instance;
|
||||
|
||||
/* handle motor error states */
|
||||
if (measure->state != 0x00 && measure->state != 0x01)
|
||||
{
|
||||
DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
|
||||
osDelay(20);
|
||||
DWT_Delay(0.001);
|
||||
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
|
||||
osDelay(20);
|
||||
continue;
|
||||
}
|
||||
pid_ref = motor->pid_ref;
|
||||
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
pid_ref *= -1; // 设置反转
|
||||
|
||||
float pid_ref = motor->pid_ref;
|
||||
|
||||
/* ---- Angle loop (outermost) ---- */
|
||||
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
float pid_measure;
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
{
|
||||
pid_measure = *motor->other_angle_feedback_ptr;
|
||||
// char buff[20];
|
||||
// Float2Str(buff, pid_measure);
|
||||
// LOGINFO("pid_measure: %s", buff);
|
||||
}
|
||||
pid_measure = *ctrl->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->total_angle;
|
||||
// 更新pid_ref进入下一个环
|
||||
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
|
||||
pid_ref = PIDCalculate(&ctrl->angle_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
/* ---- Speed loop (inner) ---- */
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
|
||||
{
|
||||
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor->speed_feedforward_ptr;
|
||||
pid_ref += *ctrl->speed_feedforward_ptr;
|
||||
|
||||
float pid_measure;
|
||||
if (setting->speed_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_speed_feedback_ptr;
|
||||
else // MOTOR_FEED
|
||||
pid_measure = *ctrl->other_speed_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->velocity;
|
||||
// 更新pid_ref进入下一个环
|
||||
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
|
||||
|
||||
pid_ref = PIDCalculate(&ctrl->speed_PID, pid_measure, pid_ref);
|
||||
}
|
||||
if (setting->feedforward_flag == 1)
|
||||
pid_ref += 0.8;
|
||||
|
||||
if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
|
||||
|
||||
/* output reverse */
|
||||
if (setting->output_reverse == OUTPUT_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
set = pid_ref;
|
||||
/* ---- Send via selected control mode ---- */
|
||||
float vel_cmd = pid_ref * DEGREE_2_RAD; // deg/s → rad/s
|
||||
LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);
|
||||
|
||||
LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
|
||||
motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
|
||||
motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
|
||||
motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12);
|
||||
motor_send_mailbox.Kp = 0;
|
||||
motor_send_mailbox.Kd = 0;
|
||||
switch (motor->ctrl_mode)
|
||||
{
|
||||
case DM_MODE_VELOCITY:
|
||||
/* Velocity mode: MCU angle PID output → motor velocity ref */
|
||||
DMMotorSendVelocity(can, vel_cmd);
|
||||
break;
|
||||
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
|
||||
case DM_MODE_MIT: {
|
||||
/* MIT mode: MCU angle PID output → motor position ref, motor handles position-velocity internally */
|
||||
float pos_cmd = pid_ref * DEGREE_2_RAD;
|
||||
LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
|
||||
DMMotorSendMIT(can, pos_cmd, 0.0f, motor->mit_kp, motor->mit_kd, 0.0f);
|
||||
break;
|
||||
}
|
||||
|
||||
motor->motor_can_instace->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
|
||||
motor->motor_can_instace->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des);
|
||||
motor->motor_can_instace->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4);
|
||||
motor->motor_can_instace->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8));
|
||||
motor->motor_can_instace->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp);
|
||||
motor->motor_can_instace->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4);
|
||||
motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
|
||||
motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
|
||||
|
||||
CANTransmit(motor->motor_can_instace, 1);
|
||||
|
||||
osDelay(1);
|
||||
case DM_MODE_POS_VEL: {
|
||||
/* Position-Velocity mode: MCU angle PID output → motor position ref + velocity ff */
|
||||
float pos_cmd = pid_ref * DEGREE_2_RAD;
|
||||
LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
|
||||
DMMotorSendPosVel(can, pos_cmd, vel_cmd);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
void DMMotorControlInit()
|
||||
{
|
||||
char dm_task_name[5] = "dm";
|
||||
// 遍历所有电机实例,创建任务
|
||||
if (!idx)
|
||||
return;
|
||||
for (size_t i = 0; i < idx; i++)
|
||||
{
|
||||
char dm_id_buff[2] = {0};
|
||||
__itoa(i, dm_id_buff, 10);
|
||||
strcat(dm_task_name, dm_id_buff);
|
||||
osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);
|
||||
dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]);
|
||||
}
|
||||
}
|
||||
@@ -1,5 +1,6 @@
|
||||
#ifndef DMMOTOR_H
|
||||
#define DMMOTOR_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "bsp_can.h"
|
||||
#include "controller.h"
|
||||
@@ -8,73 +9,99 @@
|
||||
|
||||
#define DM_MOTOR_CNT 4
|
||||
|
||||
#define DM_P_MIN (-12.56637f)
|
||||
#define DM_P_MAX 12.56637f
|
||||
/* DM motor parameter ranges (from reference DM_6220 driver) */
|
||||
#define DM_P_MIN (-12.5f)
|
||||
#define DM_P_MAX 12.5f
|
||||
#define DM_V_MIN (-45.0f)
|
||||
#define DM_V_MAX 45.0f
|
||||
#define DM_T_MIN (-18.0f)
|
||||
#define DM_T_MAX 18.0f
|
||||
#define DM_T_MIN (-10.0f)
|
||||
#define DM_T_MAX 10.0f
|
||||
#define DM_KP_MIN 0.0f
|
||||
#define DM_KP_MAX 500.0f
|
||||
#define DM_KD_MIN 0.0f
|
||||
#define DM_KD_MAX 5.0f
|
||||
|
||||
typedef struct
|
||||
/* DM motor control modes */
|
||||
typedef enum
|
||||
{
|
||||
uint8_t id;
|
||||
uint8_t state;
|
||||
float velocity;
|
||||
float last_position;
|
||||
float position;
|
||||
float torque;
|
||||
float T_Mos;
|
||||
float T_Rotor;
|
||||
int32_t total_round;
|
||||
float total_angle;
|
||||
}DM_Motor_Measure_s;
|
||||
DM_MODE_MIT = 0, // MIT mode: CAN ID, packed pos/vel/KP/KD/torque
|
||||
DM_MODE_POS_VEL = 1, // Position-Velocity mode: CAN ID + 0x100, float pos + float vel
|
||||
DM_MODE_VELOCITY = 2, // Velocity mode: CAN ID + 0x200, float velocity only
|
||||
} DM_Motor_Mode_e;
|
||||
|
||||
/* DM motor measurement data (parsed from feedback CAN frame) */
|
||||
typedef struct
|
||||
{
|
||||
uint16_t position_des;
|
||||
uint16_t velocity_des;
|
||||
uint16_t torque_des;
|
||||
uint16_t Kp;
|
||||
uint16_t Kd;
|
||||
}DMMotor_Send_s;
|
||||
typedef struct
|
||||
uint8_t id; // motor ID from feedback byte[0:3]
|
||||
uint8_t state; // motor state from feedback byte[7:4]: 0=reset, 1=run
|
||||
float position; // current position (rad, range ±12.5)
|
||||
float velocity; // current velocity (rad/s, range ±45)
|
||||
float torque; // current torque (N·m, range ±10)
|
||||
float T_Mos; // MOS temperature (Celsius)
|
||||
float T_Rotor; // rotor temperature (Celsius)
|
||||
float last_position; // previous position for multi-turn calculation
|
||||
int32_t total_round; // accumulated full revolutions
|
||||
float total_angle; // multi-turn angle (degrees)
|
||||
} DM_Motor_Measure_s;
|
||||
|
||||
/* DM motor instance (runtime state) */
|
||||
typedef struct
|
||||
{
|
||||
DM_Motor_Measure_s measure;
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
PIDInstance current_PID;
|
||||
PIDInstance speed_PID;
|
||||
PIDInstance angle_PID;
|
||||
float *other_angle_feedback_ptr;
|
||||
float *other_speed_feedback_ptr;
|
||||
float *speed_feedforward_ptr;
|
||||
float *current_feedforward_ptr;
|
||||
float pid_ref;
|
||||
Motor_Working_Type_e stop_flag;
|
||||
CANInstance *motor_can_instace;
|
||||
DaemonInstance* motor_daemon;
|
||||
uint32_t lost_cnt;
|
||||
uint8_t lost_status;
|
||||
uint8_t last_lost_status;
|
||||
}DMMotorInstance;
|
||||
Motor_Controller_s motor_controller;
|
||||
|
||||
CANInstance *motor_can_instance; // CAN communication instance
|
||||
DaemonInstance *motor_daemon; // daemon watchdog
|
||||
DM_Motor_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
|
||||
float mit_kp; // MIT mode KP (motor internal position gain, 0~500)
|
||||
float mit_kd; // MIT mode KD (motor internal velocity gain, 0~5)
|
||||
|
||||
float pid_ref; // reference input from application
|
||||
} DMMotorInstance;
|
||||
|
||||
/* DM motor init config */
|
||||
typedef struct
|
||||
{
|
||||
Motor_Init_Config_s motor_config; // generic motor PID and setting config
|
||||
DM_Motor_Mode_e ctrl_mode; // default control mode
|
||||
float mit_kp; // MIT mode KP (only for MIT mode)
|
||||
float mit_kd; // MIT mode KD (only for MIT mode)
|
||||
} DM_Motor_Init_Config_s;
|
||||
|
||||
/* DM motor command codes */
|
||||
typedef enum
|
||||
{
|
||||
DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
|
||||
DM_CMD_RESET_MODE = 0xfd, // 停止
|
||||
DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
|
||||
DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
|
||||
}DMMotor_Mode_e;
|
||||
DM_CMD_MOTOR_MODE = 0xFC, // enable motor
|
||||
DM_CMD_RESET_MODE = 0xFD, // disable motor
|
||||
DM_CMD_ZERO_POSITION = 0xFE, // set current position as encoder zero
|
||||
DM_CMD_CLEAR_ERROR = 0xFB, // clear motor error
|
||||
} DMMotor_Mode_e;
|
||||
|
||||
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
|
||||
/* ---- API ---- */
|
||||
|
||||
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config);
|
||||
|
||||
void DMMotorSetRef(DMMotorInstance *motor, float ref);
|
||||
|
||||
void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
|
||||
|
||||
void DMMotorEnable(DMMotorInstance *motor);
|
||||
|
||||
void DMMotorStop(DMMotorInstance *motor);
|
||||
void DMMotorCaliEncoder(DMMotorInstance *motor);
|
||||
void DMMotorControlInit();
|
||||
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type);
|
||||
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode);
|
||||
void DMMotorSetMode(DMMotor_Mode_e mode, DMMotorInstance *motor);
|
||||
#endif // !DMMOTOR
|
||||
|
||||
/**
|
||||
* @brief Main control function called from Motor Task at 1kHz.
|
||||
* Iterates all registered motors, runs PID cascade, sends CAN frames.
|
||||
*/
|
||||
void DMMotorControl(void);
|
||||
|
||||
/* ---- Low-level CAN send functions (all three modes) ---- */
|
||||
|
||||
/* MIT mode: send position + velocity + KP + KD + torque in packed 8-byte frame */
|
||||
void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq);
|
||||
|
||||
/* Position-Velocity mode: send float position + float velocity */
|
||||
void DMMotorSendPosVel(CANInstance *can, float pos, float vel);
|
||||
|
||||
/* Velocity mode: send float velocity only */
|
||||
void DMMotorSendVelocity(CANInstance *can, float vel);
|
||||
|
||||
#endif // DMMOTOR_H
|
||||
|
||||
+12
-24
@@ -49,19 +49,13 @@ typedef enum
|
||||
OTHER_FEED,
|
||||
} Feedback_Source_e;
|
||||
|
||||
/* 电机正反转标志 */
|
||||
/* 输出反向标志: 取反最终PID输出 */
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_DIRECTION_NORMAL = 0,
|
||||
MOTOR_DIRECTION_REVERSE = 1
|
||||
} Motor_Reverse_Flag_e;
|
||||
OUTPUT_DIRECTION_NORMAL = 0,
|
||||
OUTPUT_DIRECTION_REVERSE = 1,
|
||||
} Output_Reverse_Flag_e;
|
||||
|
||||
/* 反馈量正反标志 */
|
||||
typedef enum
|
||||
{
|
||||
FEEDBACK_DIRECTION_NORMAL = 0,
|
||||
FEEDBACK_DIRECTION_REVERSE = 1
|
||||
} Feedback_Reverse_Flag_e;
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_STOP = 0,
|
||||
@@ -77,14 +71,13 @@ typedef enum
|
||||
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
|
||||
typedef struct
|
||||
{
|
||||
Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环
|
||||
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
|
||||
Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转
|
||||
Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向
|
||||
Feedback_Source_e angle_feedback_source; // 角度反馈类型
|
||||
Feedback_Source_e speed_feedback_source; // 速度反馈类型
|
||||
uint8_t feedforward_flag; // 前馈标志
|
||||
MOTOR_SEND_MODE_e send_mode; // 发送模式
|
||||
Closeloop_Type_e outer_loop_type; // 最外层的闭环
|
||||
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
|
||||
Output_Reverse_Flag_e output_reverse; // 输出反向
|
||||
Feedback_Source_e angle_feedback_source; // 角度反馈类型
|
||||
Feedback_Source_e speed_feedback_source; // 速度反馈类型
|
||||
uint8_t feedforward_flag; // 前馈标志
|
||||
MOTOR_SEND_MODE_e send_mode; // 发送模式
|
||||
|
||||
|
||||
} Motor_Control_Setting_s;
|
||||
@@ -111,12 +104,7 @@ typedef struct
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_TYPE_NONE = 0,
|
||||
GM6020,
|
||||
M3508,
|
||||
M2006,
|
||||
LK9025,
|
||||
HT04,
|
||||
GM6020_NEW //新版固件6020
|
||||
DM_MOTOR,
|
||||
} Motor_Type_e;
|
||||
|
||||
/**
|
||||
|
||||
@@ -1,25 +1,7 @@
|
||||
#include "motor_task.h"
|
||||
#include "LK9025.h"
|
||||
#include "HT04.h"
|
||||
#include "dji_motor.h"
|
||||
#include "step_motor.h"
|
||||
#include "servo_motor.h"
|
||||
#include "dmmotor.h"
|
||||
|
||||
void MotorControlTask()
|
||||
{
|
||||
// static uint8_t cnt = 0; 设定不同电机的任务频率
|
||||
// if(cnt%5==0) //200hz
|
||||
// if(cnt%10==0) //100hz
|
||||
DJIMotorControl();
|
||||
|
||||
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
|
||||
LKMotorControl();
|
||||
|
||||
// legacy support
|
||||
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
|
||||
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
|
||||
// HTMotorControl();
|
||||
// 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
|
||||
|
||||
// StepMotorControl();
|
||||
DMMotorControl();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user