From a99307cd0492cd98c71da874721bf1f788cf5105 Mon Sep 17 00:00:00 2001 From: fcl <2136892861@qq.com> Date: Sat, 7 Mar 2026 19:52:36 +0800 Subject: [PATCH] reset again pro --- application/chassis/chassis.c | 2 +- application/cmd/robot_cmd.c | 34 ++++++++++++++++++++++++++++++++-- application/gimbal/gimbal.c | 11 +++++++---- application/robot_def.h | 4 ++-- application/shoot/shoot.c | 2 +- 5 files changed, 43 insertions(+), 10 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index dba99c1..a2597c8 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -154,7 +154,7 @@ void ChassisInit() .Kp = 6, // 6 .Ki = 0, .Kd = 0.495, // 0.5 - // .DeadBand = 0.5f, + .DeadBand = 0.5f,// 启用死区，值为0.5度 //.CoefA = 0.2, //.CoefB = 0.3, //.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement, diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 3e8d313..17ee02d 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -80,11 +80,24 @@ void RobotCMDInit() * @brief 根据gimbal app传回的当前电机角度计算和零位的误差 * 单圈绝对角度的范围是0~360,说明文档中有图示 * - */ + */static float base_total_angle = 0.0f; // 用于计算偏移的基准角度 static void CalcOffsetAngle() { static float last_offset_angle = 0; const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显 + + //const float deadband = 0.5f; // 死区值 + // 新增：跟踪模式变化，用于重置基准角度 + static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;// + + + // 当从旋转模式切换到跟随模式时，重置基准角度 + if (last_chassis_mode == CHASSIS_ROTATE && chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) + { + base_total_angle = gimbal_fetch_data.yaw_motor_total_angle; + last_offset_angle = 0.0f; // 同时重置历史偏移值 + } + float new_offset = gimbal_fetch_data.yaw_motor_total_angle - YAW_ALIGN_ANGLE; #if YAW_ECD_GREATER_THAN_4096 new_offset = -new_offset; @@ -94,8 +107,25 @@ static void CalcOffsetAngle() new_offset -= 360.0f; else if (angle_diff < -180.0f) new_offset += 360.0f; - chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor; + ///// + // 应用死区 + //if (fabs(new_offset) < deadband) { + // new_offset = 0.0f; + // } else if (new_offset > 0) { + // new_offset -= deadband; + //} else { + // new_offset += deadband; + //}////// + chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor; last_offset_angle = chassis_cmd_send.offset_angle; + // 更新模式状态 + last_chassis_mode = chassis_cmd_send.chassis_mode; + // 当处于旋转模式时，更新基准角度以防止累积过大误差 + if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE) + { + base_total_angle = gimbal_fetch_data.yaw_motor_total_angle; + } + // static float angle; // static float last_diff = 0.0f; // 用于在±180°附近保持上次符号，避免来回跳变 // /* 使用 yaw 电机的单圈角度与 `YAW_ALIGN_ANGLE` 对齐（保持原实现的参考系） diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index d4ecc3c..6b3798b 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -26,8 +26,10 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 0.685, // 8 - .Ki = 0.315, + //.Kp = 0.685, // 8 + .Kp = 0.6, // 8 + //.Ki = 0.315, + .Ki = 0.25, .Kd = 0.022, //.DeadBand = 0.1, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate, @@ -38,7 +40,8 @@ void GimbalInit() .Derivative_LPF_RC = 0.001 }, .speed_PID = { - .Kp = 8000, // 50 + .Kp = 8000, // 50 + //.Kp = 5000, // 50 .Ki = 5000, // 200 .Kd = 0, .CoefB = 1, @@ -93,7 +96,7 @@ void GimbalInit() .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, - .motor_type = GM6020, + .motor_type = M3508, }; // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 yaw_motor = DJIMotorInit(&yaw_config); diff --git a/application/robot_def.h b/application/robot_def.h index b3e4ee6..ab6b79f 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -35,8 +35,8 @@ // #define CHASSIS_OMNI_WHEEL // 是否为全向轮底盘 // #define CHASSIS_MCNAMEE_WHEEL // 是否为麦克纳姆轮底盘 #define CHASSIS_STEERING_WHEEL // 是否为舵轮 -#define STEERING_CHASSIS_ALIGN_ECD_RF 7294 // 舵电机 A 编码器值，若有机械改动需要修改 -#define STEERING_CHASSIS_ALIGN_ECD_LB 1221 // 舵电机 D 编码器值，若有机械改动需要修改 +#define STEERING_CHASSIS_ALIGN_ECD_RF 7367 // 舵电机 A 编码器值，若有机械改动需要修改 +#define STEERING_CHASSIS_ALIGN_ECD_LB 1195 // 舵电机 D 编码器值，若有机械改动需要修改 #define STEERING_CHASSIS_ALIGN_ANGLE_RF STEERING_CHASSIS_ALIGN_ECD_RF / 8192.f * 360.f // 舵轮 A 对齐角度 #define STEERING_CHASSIS_ALIGN_ANGLE_LB STEERING_CHASSIS_ALIGN_ECD_LB / 8192.f * 360.f // 舵轮 D 对齐角度 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 514ea36..921402c 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -96,7 +96,7 @@ void ShootInit() .close_loop_type = CURRENT_LOOP | SPEED_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 }, - .motor_type = M2006 // 英雄使用m3508 + .motor_type = M3508 // 英雄使用m3508 }; loader = DJIMotorInit(&loader_config);