noaddrecover

User/module/balance_chassis.c

@@ -1050,7 +1050,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    * 无速度指令时：位移+速度双环控制，积分保持位置锁定
    * 位移误差钳位到±2m，防止积分漂移过大 */
   if (fabsf(c->chassis_state.target_velocity_x) > 0.01f) {
-    c->chassis_state.target_x = c->chassis_state.position_x;
+    c->chassis_state.target_x = c->chassis_state.position_x+0.8f;  /* 直接锁定在当前位置，消除位移误差 */
   } else {
     c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
   }

User/module/config.c

@@ -5,6 +5,7 @@
 /* Includes ----------------------------------------------------------------- */
 #include "module/config.h"
 #include "module/balance_chassis.h"
+#include "module/gimbal.h"
 #include <stdbool.h>
 
 
@@ -81,9 +82,6 @@ Config_RobotParam_t robot_config = {
             .yaw = -1.0f,
             .pit = 0.85f,
         },
-        .recover = {
-            .yaw_mech_zero = 0.0f,  /* yaw编码器机械零点，需要实测标定 */
-        },
         .low_pass_cutoff_freq = {
             .out = -1.0f,
             .gyro = 1000.0f,
@@ -502,13 +500,14 @@ Config_RobotParam_t robot_config = {
         .rc_mode_map = {
 #if CMD_ENABLE_MODULE_GIMBAL
             .gimbal_sw_up   = GIMBAL_MODE_RELAX,
-            .gimbal_sw_mid  = GIMBAL_MODE_RELATIVE,
+            .gimbal_sw_mid  = GIMBAL_MODE_ABSOLUTE,
             .gimbal_sw_down = GIMBAL_MODE_RELATIVE,
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
             .balance_sw_up   = CHASSIS_MODE_RELAX,
-            .balance_sw_mid  = CHASSIS_MODE_WHELL_LEG_BALANCE,
+            // .balance_sw_mid  = CHASSIS_MODE_WHELL_LEG_BALANCE,
-            .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
+            .balance_sw_mid = CHASSIS_MODE_RELAX, // 先关闭轮腿平衡，避免不熟悉操作导致摔倒
+            .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
 #endif
         },
     },

User/module/gimbal.c

@@ -218,7 +218,6 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
   case GIMBAL_MODE_RELAX:
     g->out.yaw = 0.0f;
     g->out.pit = 0.0f;
-    /* fallthrough - AI控制模式也需要执行PID计算 */
     break;
   case GIMBAL_MODE_ABSOLUTE:
   case GIMBAL_MODE_RELATIVE:
@@ -233,7 +232,6 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
                           g->feedback.imu.gyro.y, 0.f, g->dt);
 
     break;
-        break;
   case GIMBAL_MODE_AI_CONTROL:
     /* --- YAW --- */
     // 位置环: Kp * (pos_ref - pos_fdb)
@@ -246,43 +244,15 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
                           g->feedback.imu.gyro.z, 0.0f, g->dt)
                + g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA;  // 加速度前馈: J * acc
 
-    /* --- PITCH --- */
+    /* --- PITCH --- (反馈轴与 ABSOLUTE 模式一致: eulr.rol / gyro.y) */
     pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle),
                                    g->setpoint.eulr.pit,
-                                   g->feedback.imu.eulr.pit, 0.0f, g->dt);
+                                   g->feedback.imu.eulr.rol, 0.0f, g->dt);
     g->out.pit = PID_Calc(&(g->pid.aimbot.pit_omega),
                           pit_omega_set_point + g_cmd->ff_vel_pit,
-                          g->feedback.imu.gyro.x, 0.0f, g->dt)
+                          g->feedback.imu.gyro.y, 0.0f, g->dt)
                + g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA;  // 加速度前馈: J * acc
     break;
-
-  case GIMBAL_MODE_RECOVER: {
-    /* --- YAW: 编码器双环PID，双零点选近 --- */
-    float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
-    float zero0 = g->param->recover.yaw_mech_zero;
-    float zero1 = zero0 + M_PI;
-    if (zero1 >= M_2PI) zero1 -= M_2PI;
-
-    /* 计算到两个零点的圆周距离，选近的 */
-    float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
-    float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
-    float yaw_target = (err0 <= err1) ? zero0 : zero1;
-
-    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), yaw_target,
-                                   yaw_enc, 0.0f, g->dt);
-    g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
-                          g->feedback.motor.yaw.rotor_speed, 0.0f, g->dt);
-
-    /* --- PITCH: 编码器双环PID，锁定限位中心 --- */
-    float pit_center = 0.5f * (g->limit.pit.max + g->limit.pit.min);
-    float pit_enc = g->feedback.motor.pit.rotor_abs_angle;
-
-    pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), pit_center,
-                                   pit_enc, 0.0f, g->dt);
-    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
-                          g->feedback.motor.pit.rotor_speed, 0.0f, g->dt);
-    break;
-  }
   }
   
   /* 输出滤波 */

User/module/gimbal.h

@@ -30,8 +30,7 @@ typedef enum {
   GIMBAL_MODE_RELAX,    /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
   GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
   GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
-  GIMBAL_MODE_AI_CONTROL, /* AI控制模式，直接接受AI下发的目标角度 */
+  GIMBAL_MODE_AI_CONTROL  /* AI控制模式，直接接受AI下发的目标角度 */
-  GIMBAL_MODE_RECOVER   /* 回中模式，使用编码器双环PID控制yaw回零点，pitch锁定限位中心 */
 } Gimbal_Mode_t;
 
 /* UI 导出结构（供 referee 系统绘制） */
@@ -98,11 +97,6 @@ typedef struct {
     float pit; /* pitch轴机械限位行程 -1表示无限位*/
   } travel;
 
-  /* Recover模式参数 */
-  struct {
-    float yaw_mech_zero;       /* yaw机械零点（编码器角度） */
-  } recover;
-
 } Gimbal_Params_t;
 
 /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */