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/master_machine/master_machine.c
+19
View File
@@ -42,6 +42,25 @@ static void DecodeVision()
memcpy(&vision_data.pitch_ref, buf + 6, 4);
vision_data.manual_updated = 1;
}
else if (sof == VISION_SOF_DEBUG && len == VISION_DEBUG_LEN)
{
if (calc_checksum(buf, VISION_DEBUG_LEN) != buf[VISION_DEBUG_LEN - 1])
return;
uint8_t *p = buf + 1;
memcpy(&vision_data.vision_yaw_Kp, p, 4); p += 4;
memcpy(&vision_data.vision_yaw_Ki, p, 4); p += 4;
memcpy(&vision_data.vision_yaw_Kd, p, 4); p += 4;
memcpy(&vision_data.vision_pitch_Kp, p, 4); p += 4;
memcpy(&vision_data.vision_pitch_Ki, p, 4); p += 4;
memcpy(&vision_data.vision_pitch_Kd, p, 4); p += 4;
memcpy(&vision_data.motor_yaw_Kp, p, 4); p += 4;
memcpy(&vision_data.motor_yaw_Ki, p, 4); p += 4;
memcpy(&vision_data.motor_yaw_Kd, p, 4); p += 4;
memcpy(&vision_data.motor_pitch_Kp, p, 4); p += 4;
memcpy(&vision_data.motor_pitch_Ki, p, 4); p += 4;
memcpy(&vision_data.motor_pitch_Kd, p, 4);
vision_data.debug_updated = 1;
}
else if (sof == VISION_SOF_CMD && len == VISION_CMD_LEN)
{
if (calc_checksum(buf, VISION_CMD_LEN) != buf[VISION_CMD_LEN - 1])
modules/master_machine/master_machine.h
+20 -2
View File
@@ -9,7 +9,7 @@
#include <string.h>
/*
* 上位机串口协议 — 三种帧类型, 通过帧头区分
* 上位机串口协议 — 四种帧类型, 通过帧头区分
*
* ---- 自动/视觉追踪数据帧 0xA5 (10 字节) ----
* [SOF:0xA5][error_x:4][error_y:4][checksum:1]
@@ -22,7 +22,14 @@
*
* ---- 命令帧 0xA7 (3 字节) ----
* [SOF:0xA7][cmd:1][checksum:1]
* cmd: 0x01=切换到自动模式, 0x02=切换到手动模式, 0x03=激光开, 0x04=激光关
* cmd: 0x01=自动模式, 0x02=手动模式, 0x03=激光开, 0x04=激光关, 0x05=调试模式
*
* ---- 调试调参帧 0xA8 (50 字节) ----
* [SOF:0xA8][vision_yaw_Kp:4][vision_yaw_Ki:4][vision_yaw_Kd:4]
* [vision_pitch_Kp:4][vision_pitch_Ki:4][vision_pitch_Kd:4]
* [motor_yaw_Kp:4][motor_yaw_Ki:4][motor_yaw_Kd:4]
* [motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4][checksum:1]
* 全部 float, 依次为视觉环 yaw/pitch 和电机角度环 yaw/pitch 的 PID 参数
*
* checksum: 帧头到数据末尾前一字节累加和取低8位
*/
@@ -30,22 +37,26 @@
#define VISION_SOF_AUTO 0xA5
#define VISION_SOF_MANUAL 0xA6
#define VISION_SOF_CMD 0xA7
#define VISION_SOF_DEBUG 0xA8
#define VISION_AUTO_LEN 10
#define VISION_MANUAL_LEN 11
#define VISION_CMD_LEN 3
#define VISION_DEBUG_LEN 50
/* 命令码 */
#define VISION_CMD_SWITCH_AUTO 0x01
#define VISION_CMD_SWITCH_MANUAL 0x02
#define VISION_CMD_LASER_ON 0x03
#define VISION_CMD_LASER_OFF 0x04
#define VISION_CMD_SWITCH_DEBUG 0x05
/* 工作模式 */
typedef enum
{
VISION_MODE_AUTO = 0, // 自动/视觉追踪
VISION_MODE_MANUAL = 1, // 手动
VISION_MODE_DEBUG = 2, // 调试
} Vision_Mode_e;
typedef struct
@@ -61,6 +72,13 @@ typedef struct
float pitch_ref;
uint8_t manual_updated; // 收到新的手动数据帧标志
/* 调试模式数据 */
float vision_yaw_Kp, vision_yaw_Ki, vision_yaw_Kd;
float vision_pitch_Kp, vision_pitch_Ki, vision_pitch_Kd;
float motor_yaw_Kp, motor_yaw_Ki, motor_yaw_Kd;
float motor_pitch_Kp, motor_pitch_Ki, motor_pitch_Kd;
uint8_t debug_updated;
/* 命令 */
uint8_t cmd; // 最新命令码
uint8_t cmd_pending; // 有待处理命令标志
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.