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
application/cmd/robot_cmd.c
+77 -5
View File
@@ -13,7 +13,7 @@
#include "bsp_log.h"
/* ---- 模式 ---- */
static Vision_Mode_e current_mode = VISION_MODE_MANUAL;
static Vision_Mode_e current_mode = VISION_MODE_DEBUG;
/* ---- 视觉追踪 PID (外环) ---- */
static PIDInstance vision_yaw_pid;
@@ -32,6 +32,8 @@ static float gimbal_pitch_ref;
/* ---- pub-sub ---- */
static Publisher_t *gimbal_cmd_pub;
static Subscriber_t *gimbal_feed_sub;
static Publisher_t *motor_enable_pub;
static Publisher_t *motor_pid_pub;
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;
static Gimbal_Upload_Data_s gimbal_fetch_data;
@@ -91,6 +93,14 @@ static void SwitchToAutoMode(void)
vision_data.manual_updated = 0;
VisionPIDReset();
/* 从调试模式切回时重新使能电机 */
if (current_mode == VISION_MODE_DEBUG)
{
Motor_Enable_s msg = { .enable = 1 };
PubPushMessage(motor_enable_pub, &msg);
}
current_mode = VISION_MODE_AUTO;
LOGINFO("[CMD] switched to AUTO mode");
}
@@ -104,10 +114,32 @@ static void SwitchToManualMode(void)
gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch;
vision_data.auto_updated = 0;
/* 从调试模式切回时重新使能电机 */
if (current_mode == VISION_MODE_DEBUG)
{
Motor_Enable_s msg = { .enable = 1 };
PubPushMessage(motor_enable_pub, &msg);
}
current_mode = VISION_MODE_MANUAL;
LOGINFO("[CMD] switched to MANUAL mode");
}
static void SwitchToDebugMode(void)
{
if (current_mode == VISION_MODE_DEBUG)
return;
current_mode = VISION_MODE_DEBUG;
/* 失能 yaw 和 pitch 电机 */
Motor_Enable_s msg = { .enable = 0 };
PubPushMessage(motor_enable_pub, &msg);
LOGINFO("[CMD] switched to DEBUG mode, motors disabled");
}
/* ---- 命令处理 ---- */
static void ProcessCommand(void)
{
@@ -124,6 +156,9 @@ static void ProcessCommand(void)
case VISION_CMD_SWITCH_MANUAL:
SwitchToManualMode();
break;
case VISION_CMD_SWITCH_DEBUG:
SwitchToDebugMode();
break;
case VISION_CMD_LASER_ON:
laser_on();
break;
@@ -170,6 +205,8 @@ void RobotCMDInit()
{
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
motor_enable_pub = PubRegister("motor_enable", sizeof(Motor_Enable_s));
motor_pid_pub = PubRegister("motor_pid", sizeof(Motor_PID_Tune_s));
static PID_Init_Config_s yaw_vpid_config = {
.Kp = 1.5f, .Ki = 3.0f, .Kd = 0.07f,
@@ -196,7 +233,7 @@ void RobotCMDInit()
}
/* ---- 自动 / 视觉追踪模式 ---- */
static void VisionTrackingSet()
static void VisionTracking()
{
if (vision_data.auto_updated)
{
@@ -227,7 +264,7 @@ static void VisionTrackingSet()
}
/* ---- 手动模式: 上位机角度参考 ---- */
static void ManualModeSet()
static void ManualMode()
{
if (vision_data.manual_updated)
{
@@ -245,6 +282,39 @@ static void ManualModeSet()
gimbal_cmd_send.pitch = gimbal_pitch_ref;
}
/* ---- 调试模式: 上位机下发 PID 参数调参 ---- */
static void DebugMode()
{
if (vision_data.debug_updated)
{
vision_data.debug_updated = 0;
/* 直接设置视觉环 PID 参数 */
vision_yaw_pid.Kp = vision_data.vision_yaw_Kp;
vision_yaw_pid.Ki = vision_data.vision_yaw_Ki;
vision_yaw_pid.Kd = vision_data.vision_yaw_Kd;
vision_pitch_pid.Kp = vision_data.vision_pitch_Kp;
vision_pitch_pid.Ki = vision_data.vision_pitch_Ki;
vision_pitch_pid.Kd = vision_data.vision_pitch_Kd;
/* 发布电机角度环 PID 参数到 gimbal */
Motor_PID_Tune_s tune = {
.yaw_Kp = vision_data.motor_yaw_Kp,
.yaw_Ki = vision_data.motor_yaw_Ki,
.yaw_Kd = vision_data.motor_yaw_Kd,
.pitch_Kp = vision_data.motor_pitch_Kp,
.pitch_Ki = vision_data.motor_pitch_Ki,
.pitch_Kd = vision_data.motor_pitch_Kd,
};
PubPushMessage(motor_pid_pub, &tune);
LOGINFO("[CMD] PID tuned: v_yaw(%.2f,%.2f,%.2f) v_pitch(%.2f,%.2f,%.2f)",
vision_data.vision_yaw_Kp, vision_data.vision_yaw_Ki, vision_data.vision_yaw_Kd,
vision_data.vision_pitch_Kp, vision_data.vision_pitch_Ki, vision_data.vision_pitch_Kd);
}
}
void RobotCMDTask()
{
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
@@ -252,9 +322,11 @@ void RobotCMDTask()
ProcessCommand();
if (current_mode == VISION_MODE_AUTO)
VisionTrackingSet();
VisionTracking();
else if (current_mode == VISION_MODE_DEBUG)
DebugMode();
else
ManualModeSet();
ManualMode();
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
}
application/gimbal/gimbal.c
+35 -1
View File
@@ -9,6 +9,8 @@ static INS_t *gimbal_IMU_data;
static Publisher_t *gimbal_pub;
static Subscriber_t *gimbal_sub;
static Subscriber_t *motor_enable_sub;
static Subscriber_t *motor_pid_sub;
static Gimbal_Upload_Data_s gimbal_feedback_data;
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;
@@ -21,6 +23,8 @@ void GimbalInit()
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
motor_enable_sub = SubRegister("motor_enable", sizeof(Motor_Enable_s));
motor_pid_sub = SubRegister("motor_pid", sizeof(Motor_PID_Tune_s));
/* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */
DM_Motor_Init_Config_s yaw_config = {
@@ -78,6 +82,8 @@ void GimbalInit()
.mit_kp = 0, .mit_kd = 0,
};
pitch_motor = DMMotorInit(&pitch_config);
gimbal_feedback_data.gimbal_imu_data = gimbal_IMU_data;
}
void GimbalTask()
@@ -87,6 +93,34 @@ void GimbalTask()
DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
gimbal_feedback_data.gimbal_imu_data = gimbal_IMU_data;
/* 电机使能/失能控制 */
static Motor_Enable_s enable_msg;
if (SubGetMessage(motor_enable_sub, &enable_msg))
{
if (enable_msg.enable)
{
DMMotorSetMode(DM_CMD_ENABLE_MODE, yaw_motor);
DMMotorSetMode(DM_CMD_ENABLE_MODE, pitch_motor);
}
else
{
DMMotorSetMode(DM_CMD_DISABLE_MODE, yaw_motor);
DMMotorSetMode(DM_CMD_DISABLE_MODE, pitch_motor);
}
}
/* 电机角度环 PID 调参 */
static Motor_PID_Tune_s pid_tune;
if (SubGetMessage(motor_pid_sub, &pid_tune))
{
yaw_motor->motor_controller.angle_PID.Kp = pid_tune.yaw_Kp;
yaw_motor->motor_controller.angle_PID.Ki = pid_tune.yaw_Ki;
yaw_motor->motor_controller.angle_PID.Kd = pid_tune.yaw_Kd;
pitch_motor->motor_controller.angle_PID.Kp = pid_tune.pitch_Kp;
pitch_motor->motor_controller.angle_PID.Ki = pid_tune.pitch_Ki;
pitch_motor->motor_controller.angle_PID.Kd = pid_tune.pitch_Kd;
}
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}
application/robot_def.h
+13
View File
@@ -47,6 +47,19 @@ typedef struct
uint8_t gimbal_power;
} Gimbal_Upload_Data_s;
/* ---- CMD 发布给 gimbal 的电机使能指令 ---- */
typedef struct
{
uint8_t enable; // 0=失能, 1=使能
} Motor_Enable_s;
/* ---- CMD 发布给 gimbal 的电机角度环 PID 参数 ---- */
typedef struct
{
float yaw_Kp, yaw_Ki, yaw_Kd;
float pitch_Kp, pitch_Ki, pitch_Kd;
} Motor_PID_Tune_s;
#pragma pack()
#endif // !ROBOT_DEF_H