User/module/balance_chassis.c

@@ -118,9 +118,20 @@ static void Chassis_ResetControllers(Chassis_t *c) {
  * @param c 底盘结构体指针
  */
 static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
-  /* 双零点已关闭，始终使用主零点 */
-  c->yaw_control.target_yaw = c->param->mech.mech_zero_yaw;
+  float current_yaw = c->feedback.yaw.rotor_abs_angle;
+  float zero_point_1 = c->param->mech.mech_zero_yaw;
+  float zero_point_2 = zero_point_1 + M_PI;
+
+  float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
+  float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
+
+  if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
+    c->yaw_control.target_yaw = zero_point_1;
     c->yaw_control.is_reversed = false;
+  } else {
+    c->yaw_control.target_yaw = zero_point_2;
+    c->yaw_control.is_reversed = true;
+  }
 }
 
 /**
@@ -1241,10 +1252,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   } else {
     c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
 
-    /* 双零点已关闭，始终使用主零点 */
     float manual_offset = c_cmd->move_vec.vy * M_PI_2;
-    c->yaw_control.target_yaw = c->param->mech.mech_zero_yaw + manual_offset;
+    float base_target_1 = c->param->mech.mech_zero_yaw + manual_offset;
+    float base_target_2 = c->param->mech.mech_zero_yaw + M_PI + manual_offset;
+
+    float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
+    float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
+
+    if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
+      c->yaw_control.target_yaw = base_target_1;
       c->yaw_control.is_reversed = false;
+    } else {
+      c->yaw_control.target_yaw = base_target_2;
+      c->yaw_control.is_reversed = true;
+    }
 
     c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
                                         c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);

User/module/cmd/cmd.c

@@ -6,7 +6,6 @@
 #include "module/cmd/cmd_types.h"
 #include "module/gimbal.h"
 #include "module/shoot.h"
-#include "module/gimbal.h"
 #include <stdint.h>
 #include <string.h>
 

User/module/config.c

@@ -545,7 +545,7 @@ Config_RobotParam_t robot_config = {
             //  .balance_sw_mid   = CHASSIS_MODE_RELAX,
             //   .balance_sw_down   = CHASSIS_MODE_RELAX,
             .balance_sw_mid  =CHASSIS_MODE_WHELL_LEG_BALANCE,
-            .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
+            .balance_sw_down = CHASSIS_MODE_LEG_TEST,
 #endif
         },
     },