From fd2386d33aed36208f0685490037275b92f79570 Mon Sep 17 00:00:00 2001 From: SkyLi Date: Tue, 2 Jun 2026 23:44:08 +0800 Subject: [PATCH] tune: retune vision and gimbal angle PID, fix manual mode velocity hold - Vision PID: increase Kp to 100, zero Ki/Kd, tighten CoefA/B - Gimbal angle PID: reduce to P-only (Kp=1), lower IntegralLimit, remove derivative/output filters, unify MaxOut to 2000 - Manual mode: hold last velocity command on lost frames instead of stopping - Debug mode: reset debug_updated flag on mode switch - Remove stale .vscode settings copy --- .vscode/settings copy.json | 11 ----------- application/cmd/robot_cmd.c | 26 ++++++++++++++++---------- application/gimbal/gimbal.c | 15 +++++++-------- 3 files changed, 23 insertions(+), 29 deletions(-) delete mode 100644 .vscode/settings copy.json diff --git a/.vscode/settings copy.json b/.vscode/settings copy.json deleted file mode 100644 index fae2d40..0000000 --- a/.vscode/settings copy.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "files.associations": { - "stm32f4xx_hal_def.h": "c", - "bsp_usb.h": "c", - "usb_device.h": "c", - "usbd_cdc.h": "c", - "usbd_conf.h": "c", - "usbd_desc.h": "c", - "usbd_cdc_if.h": "c" - } -} \ No newline at end of file diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index b88607e..fe41f27 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -91,7 +91,7 @@ static void SwitchToAutoMode(void) gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; - vision_data.manual_updated = 0; + vision_data.auto_updated = 0; VisionPIDReset(); /* 从调试模式切回时重新使能电机 */ @@ -113,7 +113,7 @@ static void SwitchToManualMode(void) gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; - vision_data.auto_updated = 0; + vision_data.manual_updated = 0; /* 从调试模式切回时重新使能电机 */ if (current_mode == VISION_MODE_DEBUG) @@ -131,6 +131,8 @@ static void SwitchToDebugMode(void) if (current_mode == VISION_MODE_DEBUG) return; + vision_data.debug_updated = 0; + current_mode = VISION_MODE_DEBUG; /* 失能 yaw 和 pitch 电机 */ @@ -209,17 +211,17 @@ void RobotCMDInit() 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, + .Kp = 100.0f, .Ki = 0.0f, .Kd = 0.0f, .IntegralLimit = 100.0f, - .CoefA = 20.0f, .CoefB = 20.0f, + .CoefA = 0.5f, .CoefB = 0.1f, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, .MaxOut = 200.0f, }; static PID_Init_Config_s pitch_vpid_config = { - .Kp = 1.0f, .Ki = 2.0f, .Kd = 0.05f, + .Kp = 100.0f, .Ki = 0.0f, .Kd = 0.0f, .IntegralLimit = 100.0f, - .CoefA = 10.0f, .CoefB = 30.0f, + .CoefA = 0.5f, .CoefB = 0.1f, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, .MaxOut = 200.0f, @@ -250,8 +252,8 @@ static void VisionTracking() float yaw_degree_velocity = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0); float pitch_degree_velocity = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0); - gimbal_yaw_ref += 0.001f *ROBOT_CMD_DELAY * yaw_degree_velocity; - gimbal_pitch_ref += 0.001f *ROBOT_CMD_DELAY * pitch_degree_velocity; + gimbal_yaw_ref += 0.001f * ROBOT_CMD_DELAY * yaw_degree_velocity; + gimbal_pitch_ref += 0.001f * ROBOT_CMD_DELAY * pitch_degree_velocity; } if (gimbal_pitch_ref > 30.0f) @@ -264,14 +266,18 @@ static void VisionTracking() } /* ---- 手动模式: 上位机角度参考 ---- */ +static float yaw_degree_velocity = 0; +static float pitch_degree_velocity = 0; static void ManualMode() { if (vision_data.manual_updated) { vision_data.manual_updated = 0; - gimbal_yaw_ref += 0.001f *ROBOT_CMD_DELAY * vision_data.yaw_degree_velocity; - gimbal_pitch_ref += 0.001f *ROBOT_CMD_DELAY * vision_data.pitch_degree_velocity; + yaw_degree_velocity = vision_data.yaw_degree_velocity; + pitch_degree_velocity = vision_data.pitch_degree_velocity; } + gimbal_yaw_ref += 0.001f * ROBOT_CMD_DELAY * yaw_degree_velocity; + gimbal_pitch_ref += 0.001f * ROBOT_CMD_DELAY * pitch_degree_velocity; if (gimbal_pitch_ref > 30.0f) gimbal_pitch_ref = 30.0f; diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index cc99bb1..296119b 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -36,12 +36,11 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 30, .Ki = 80, .Kd = 0.2f, - .IntegralLimit = 1000, - .CoefA = 20, .CoefB = 10, + .Kp = 1, .Ki = 0, .Kd = 0, + .IntegralLimit = 45, + .CoefA = 15, .CoefB = 5, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | - PID_Derivative_On_Measurement | PID_ChangingIntegrationRate | - PID_DerivativeFilter | PID_OutputFilter, + PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, .MaxOut = 2000, }, .other_angle_feedback_ptr = &gimbal_IMU_data->YawTotalAngle, @@ -65,12 +64,12 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 20, .Ki = 50, .Kd = 0.1f, - .IntegralLimit = 500, + .Kp = 1, .Ki = 0, .Kd = 0, + .IntegralLimit = 45, .CoefA = 15, .CoefB = 5, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate, - .MaxOut = 1000, + .MaxOut = 2000, }, .other_angle_feedback_ptr = &gimbal_IMU_data->Pitch, .other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[0],