AimBot/Gimbal
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add DEBUG mode with host-side PID tuning, motor enable/disable protection

Browse Source 
- Add DEBUG mode (parallel to AUTO/MANUAL) for real-time PID parameter tuning
- New 0xA8 debug frame: 12 floats for vision/motor angle PID tuning params
- DMMotorSetMode: send zero-velocity before disable, clear PID to prevent jumps
- motor_enable topic: pub from robot_cmd, sub in gimbal, controls motor on/off
- motor_pid topic: pub from robot_cmd, sub in gimbal, updates motor angle PID
- Rename DM motor enums (DM_CMD_ENABLE_MODE/DISABLE_MODE, DM_Motor_Ctrl_Mode_e)
- SwitchToAuto/Manual: re-enable motors when switching back from DEBUG mode
- Static-allocate enable_msg/pid_tune in GimbalTask to avoid stack churn
- Add docs/host_comm_guide.md with Python Tkinter tuning GUI example
- Remove ARCHITECTURE.md
This commit is contained in:
李傲琦
2026-05-31 03:49:41 +08:00
parent 77a3aeb25c
commit 9b8e204ff7
10 changed files with 747 additions and 643 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/motor/DMmotor/dmmotor.c
+22 -8
View File
@@ -91,12 +91,29 @@ void DMMotorSendVelocity(CANInstance *can, float vel)
/* ---- Motor commands (enable/disable/zero/clear error) ---- */
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
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;
}
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 ---- */
@@ -175,7 +192,7 @@ DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
//motor->motor_daemon = DaemonRegister(&conf);
/* enable motor */
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
DMMotorSetMode(DM_CMD_ENABLE_MODE, motor);
dm_motor_instance[idx++] = motor;
return motor;
@@ -188,11 +205,6 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref)
motor->motor_controller.pid_ref = ref;
}
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode)
{
motor->ctrl_mode = mode;
}
/* ---- Main control: called from Motor Task at 1kHz ---- */
/* 硬编码: IMU 角度反馈 → 角度 PID → 速度指令 → 电机速度模式 */
@@ -201,6 +213,8 @@ 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;
/* handle motor error states */
@@ -208,7 +222,7 @@ void DMMotorControl(void)
{
DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
DWT_Delay(0.001);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
DMMotorSetMode(DM_CMD_ENABLE_MODE, motor);
continue;
}
modules/motor/DMmotor/dmmotor.h
+15 -14
View File
@@ -21,13 +21,23 @@
#define DM_KD_MIN 0.0f
#define DM_KD_MAX 5.0f
/* DM motor control modes */
/* DM motor command codes */
typedef enum
{
DM_CMD_ENABLE_MODE = 0xFC, // enable motor
DM_CMD_DISABLE_MODE = 0xFD, // disable motor
DM_CMD_ZERO_POSITION = 0xFE, // set current position as encoder zero
DM_CMD_CLEAR_ERROR = 0xFB, // clear motor error
} DM_Motor_Mode_e;
/* DM motor control modes */
typedef enum
{
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_Ctrl_Mode_e;
/* DM motor measurement data (parsed from feedback CAN frame) */
typedef struct
@@ -53,7 +63,8 @@ typedef struct
CANInstance *motor_can_instance; // CAN communication instance
DaemonInstance *motor_daemon; // daemon watchdog
Output_Reverse_Flag_e output_reverse; // output reverse flag
DM_Motor_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
DM_Motor_Ctrl_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
DM_Motor_Mode_e motor_mode; // last command mode (enable/disable/zero/clear error)
float mit_kp; // MIT mode KP (motor internal position gain, 0~500)
float mit_kd; // MIT mode KD (motor internal velocity gain, 0~5)
@@ -63,27 +74,17 @@ typedef struct
typedef struct
{
Motor_Init_Config_s motor_config; // generic motor PID and setting config
DM_Motor_Mode_e ctrl_mode; // default control mode
DM_Motor_Ctrl_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, // 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;
/* ---- API ---- */
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config);
void DMMotorSetRef(DMMotorInstance *motor, float ref);
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode);
void DMMotorSetMode(DMMotor_Mode_e mode, DMMotorInstance *motor);
void DMMotorSetMode(DM_Motor_Mode_e mode, DMMotorInstance *motor);
/**
* @brief Main control function called from Motor Task at 1kHz.