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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user