From 2ad0d9718971c8a126dccf61c8fca864f54ecb4b Mon Sep 17 00:00:00 2001
From: Xiaocheng <2544262366@qq.com>
Date: Wed, 18 Mar 2026 14:36:40 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B2=A1=E6=8B=9B=E4=BA=86?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/module/chassis.c | 45 +++++++++++++++++++++++++++++++++++++++++--
 User/module/config.c  |  2 +-
 2 files changed, 44 insertions(+), 3 deletions(-)

diff --git a/User/module/chassis.c b/User/module/chassis.c
index 05af61b..618d71e 100644
--- a/User/module/chassis.c
+++ b/User/module/chassis.c
@@ -66,6 +66,12 @@ fp32 vofa_send[8];                //vofa输出数据
 #define CHASSIS_ROTOR_WZ_MAX 0.8f          /* 小陀螺旋转位移上界 */
 #define CHASSIS_FOLLOW_LOCK_DEADBAND_RAD 0.02f /* 跟随锁定死区，约1.1度 */
 #define CHASSIS_FOLLOW_VW_DEADBAND 0.03f       /* 跟随角速度输出死区 */
+#define CHASSIS_FOLLOW_TRANS_DEADBAND 0.02f    /* 跟随平移输入死区 */
+#define CHASSIS_FOLLOW_FREE_BAND_RAD 0.08f      /* 跟随自由区，约4.6度 */
+#define CHASSIS_FOLLOW_LOCK_DEADBAND_RAD 0.02f /* 跟随锁定死区，约1.1度 */
+#define CHASSIS_FOLLOW_VW_DEADBAND 0.03f       /* 跟随角速度输出死区 */
+#define CHASSIS_FOLLOW_DAMP_GAIN 0.08f          /* 跟随阻尼系数 */
+#define CHASSIS_FOLLOW_VW_MAX 1.0f              /* 跟随角速度上限 */
 
 static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode ,uint32_t now)
 {
@@ -104,6 +110,41 @@ static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
   return wz_vary > 0.8f ? max : wz_vary;
 }
 
+// static float Chassis_CalcFollowVw(Chassis_t *c)
+// {
+//   const float follow_target = c->4.517f;
+//   const float yaw_fb = c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle;
+//   const float yaw_speed_fb = c->motorfeedback.gimbal_yaw_encoder.rotor_speed;
+//   const float follow_err = CircleError(follow_target, yaw_fb, M_2PI);
+//   const float abs_follow_err = fabsf(follow_err);
+
+//   if (abs_follow_err < CHASSIS_FOLLOW_LOCK_DEADBAND_RAD) {
+//     PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid);
+//     return 0.0f;
+//   }
+
+//   /* 保留一段相对运动区，避免底盘与大YAW刚性同动 */
+//   if (abs_follow_err < CHASSIS_FOLLOW_FREE_BAND_RAD) {
+//     PID_ResetIntegral(&c->pid.chassis_follow_gimbal_pid);
+//     return 0.0f;
+//   }
+
+//   float follow_vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid,
+//                              follow_target,
+//                              yaw_fb,
+//                              0.0f,
+//                              c->dt);
+
+//   /* 对大YAW速度做阻尼，降低同向拖动感 */
+//   follow_vw -= CHASSIS_FOLLOW_DAMP_GAIN * yaw_speed_fb;
+//   follow_vw = AbsClip(follow_vw, CHASSIS_FOLLOW_VW_MAX);
+
+//   if (fabsf(follow_vw) < CHASSIS_FOLLOW_VW_DEADBAND) {
+//     follow_vw = 0.0f;
+//   }
+//   return follow_vw;
+// }
+
 /*底盘初始化*/
 
 int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
@@ -407,6 +448,8 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
     // 跟随云台模式
     c->move_vec.Vx =-c_cmd->Vy;
     c->move_vec.Vy =-c_cmd->Vx;
+    if (fabsf(c->move_vec.Vx) < CHASSIS_FOLLOW_TRANS_DEADBAND) c->move_vec.Vx = 0.0f;
+    if (fabsf(c->move_vec.Vy) < CHASSIS_FOLLOW_TRANS_DEADBAND) c->move_vec.Vy = 0.0f;
     {
       const float follow_target = c->mech_zero_4310;
       const float follow_err = CircleError(follow_target,
@@ -427,8 +470,6 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
         c->move_vec.Vw = follow_vw;
       }
     }
-
-    // c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
     break;
 
 
diff --git a/User/module/config.c b/User/module/config.c
index edad7ea..0e97a96 100644
--- a/User/module/config.c
+++ b/User/module/config.c
@@ -101,7 +101,7 @@ Config_Param_t config = {
         /*这两个数据是云台6020的零点和机械限位*/
         .mech_zero = 1.31f,
         .travel = 1.4f,
-        .mech_zero_4310=2.06f,
+        .mech_zero_4310=4.51735544f,
 
 
     },