SkyLiCoder
e1a01e6b53
- host 0xA9 frame → master_machine decode → robot_cmd publish via gimbal_feedforward topic → gimbal subscribe → yaw_motor.feed_input - add DM_Feedforward_Controller_s with Kp and feed_input fields - add feedforward term (Kp * feed_input) to motor control output - add DM_Feedforward_Kp to motor init config
266 lines
8.7 KiB
C
266 lines
8.7 KiB
C
#include "dmmotor.h"
|
|
#include "general_def.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];
|
|
|
|
/* ---- 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;
|
|
float offset = x_min;
|
|
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);
|
|
|
|
can->txconf.Identifier = can->tx_id;
|
|
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];
|
|
|
|
can->txconf.Identifier = can->tx_id + 0x100;
|
|
CANTransmit(can, 1);
|
|
}
|
|
|
|
/* 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;
|
|
|
|
can->txconf.Identifier = can->tx_id + 0x200;
|
|
CANTransmit(can, 1);
|
|
}
|
|
|
|
/* ---- Motor commands (enable/disable/zero/clear error) ---- */
|
|
|
|
void DMMotorSetMode(DM_Motor_Mode_e cmd, DMMotorInstance *motor)
|
|
{
|
|
if (cmd == DM_CMD_DISABLE_MODE)
|
|
{
|
|
/* 先发送零速度指令让电机停止,防止切回 ENABLE 时突变 */
|
|
DMMotorSendVelocity(motor->motor_can_instance, 0.0f);
|
|
|
|
/* 清理 PID 状态 */
|
|
motor->motor_controller.angle_PID.Iout = 0;
|
|
motor->motor_controller.angle_PID.ITerm = 0;
|
|
motor->motor_controller.angle_PID.Output = 0;
|
|
motor->motor_controller.angle_PID.Err = 0;
|
|
motor->motor_controller.angle_PID.Last_Err = 0;
|
|
motor->motor_controller.angle_PID.Dout = 0;
|
|
motor->motor_controller.angle_PID.Last_Dout = 0;
|
|
motor->motor_controller.pid_ref = 0;
|
|
|
|
/* 清理前馈控制器 */
|
|
motor->feedforward_controller.feed_input = 0;
|
|
}
|
|
|
|
memset(motor->motor_can_instance->tx_buff, 0xFF, 7);
|
|
motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd;
|
|
motor->motor_can_instance->txconf.Identifier = motor->motor_can_instance->tx_id;
|
|
CANTransmit(motor->motor_can_instance, 1);
|
|
motor->motor_mode = cmd;
|
|
}
|
|
|
|
/* ---- Feedback decode: CAN RX interrupt callback ---- */
|
|
|
|
static void DMMotorDecode(CANInstance *motor_can)
|
|
{
|
|
uint8_t *rxbuff = motor_can->rx_buff;
|
|
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
|
|
DM_Motor_Measure_s *measure = &motor->measure;
|
|
uint16_t tmp;
|
|
|
|
//DaemonReload(motor->motor_daemon);
|
|
|
|
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));
|
|
measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
|
|
|
|
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_Rotor = (float)rxbuff[7];
|
|
|
|
/* 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;
|
|
LOGWARNING("[DMmotor] Motor lost, id [%d]", motor->motor_can_instance->tx_id);
|
|
}
|
|
|
|
/* ---- Init ---- */
|
|
|
|
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
|
|
{
|
|
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
|
|
memset(motor, 0, sizeof(DMMotorInstance));
|
|
|
|
motor->output_reverse = config->motor_config.output_reverse;
|
|
motor->ctrl_mode = config->ctrl_mode;
|
|
motor->mit_kp = config->mit_kp;
|
|
motor->mit_kd = config->mit_kd;
|
|
|
|
/* init angle PID controller */
|
|
PIDInit(&motor->motor_controller.angle_PID, &config->motor_config.controller_param_init_config.angle_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;
|
|
|
|
/* 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);
|
|
|
|
/* 设置前馈控制器增益 */
|
|
motor->feedforward_controller.Kp = config->DM_Feedforward_Kp;
|
|
|
|
/* register daemon */
|
|
Daemon_Init_Config_s conf = {
|
|
.callback = DMMotorLostCallback,
|
|
.owner_id = motor,
|
|
.reload_count = 10,
|
|
};
|
|
//motor->motor_daemon = DaemonRegister(&conf);
|
|
|
|
dm_motor_instance[idx++] = motor;
|
|
return motor;
|
|
}
|
|
|
|
/* ---- Runtime control ---- */
|
|
|
|
void DMMotorSetRef(DMMotorInstance *motor, float ref)
|
|
{
|
|
motor->motor_controller.pid_ref = ref;
|
|
}
|
|
|
|
/* ---- Main control: called from Motor Task at 1kHz ---- */
|
|
/* 硬编码: IMU 角度反馈 → 角度 PID → 速度指令 → 电机速度模式 */
|
|
|
|
void DMMotorControl(void)
|
|
{
|
|
for (uint8_t i = 0; i < idx; i++)
|
|
{
|
|
DMMotorInstance *motor = dm_motor_instance[i];
|
|
if(motor->motor_mode == DM_CMD_DISABLE_MODE)
|
|
continue;
|
|
CANInstance *can = motor->motor_can_instance;
|
|
DM_Feedforward_Controller_s feedforward_controller = motor->feedforward_controller;
|
|
/* handle motor error states */
|
|
if (motor->measure.state != 0x00 && motor->measure.state != 0x01)
|
|
{
|
|
DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
|
|
DWT_Delay(0.001);
|
|
DMMotorSetMode(DM_CMD_ENABLE_MODE, motor);
|
|
continue;
|
|
}
|
|
|
|
/* angle PID: IMU feedback → velocity output */
|
|
float pid_out = PIDCalculate(&motor->motor_controller.angle_PID,
|
|
*motor->motor_controller.other_angle_feedback_ptr,
|
|
motor->motor_controller.pid_ref);
|
|
|
|
if (motor->output_reverse == OUTPUT_DIRECTION_REVERSE)
|
|
pid_out *= -1;
|
|
|
|
pid_out += feedforward_controller.Kp * feedforward_controller.feed_input;
|
|
|
|
float vel_cmd = pid_out * DEGREE_2_RAD;
|
|
LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);
|
|
|
|
switch (motor->ctrl_mode)
|
|
{
|
|
case DM_MODE_VELOCITY:{
|
|
DMMotorSendVelocity(can, vel_cmd);
|
|
break;
|
|
}
|
|
case DM_MODE_MIT: {
|
|
float pos_cmd = pid_out * 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;
|
|
}
|
|
case DM_MODE_POS_VEL: {
|
|
float pos_cmd = pid_out * DEGREE_2_RAD;
|
|
LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
|
|
DMMotorSendPosVel(can, pos_cmd, vel_cmd);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|