zimiaomeil

User/module/balance_chassis.c

@@ -118,20 +118,9 @@ static void Chassis_ResetControllers(Chassis_t *c) {
  * @param c 底盘结构体指针
  */
 static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
-  float current_yaw = c->feedback.yaw.rotor_abs_angle;
+  /* 双零点已关闭，始终使用主零点 */
-  float zero_point_1 = c->param->mech.mech_zero_yaw;
+  c->yaw_control.target_yaw = c->param->mech.mech_zero_yaw;
-  float zero_point_2 = zero_point_1 + M_PI;
+  c->yaw_control.is_reversed = false;
-
-  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;
-  }
 }
 
 /**
@@ -1252,20 +1241,10 @@ 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;
-    float base_target_1 = c->param->mech.mech_zero_yaw + manual_offset;
+    c->yaw_control.target_yaw = c->param->mech.mech_zero_yaw + manual_offset;
-    float base_target_2 = c->param->mech.mech_zero_yaw + M_PI + manual_offset;
+    c->yaw_control.is_reversed = false;
-
-    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

@@ -5,6 +5,7 @@
 #include "bsp/time.h"
 #include "module/cmd/cmd_types.h"
 #include "module/shoot.h"
+#include "module/gimbal.h"
 #include <stdint.h>
 #include <string.h>
 
@@ -654,16 +655,22 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
   if (ctx->input.online[CMD_SRC_NUC]) {
     if (ctx->active_source==CMD_SRC_RC) {
       if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
+        // 用遥控器调自瞄的时候打开下面两行
         ctx->output.gimbal.source = CMD_SRC_NUC;
         ctx->output.shoot.source = CMD_SRC_NUC;
 #if CMD_ENABLE_MODULE_REFUI
         ctx->output.refui.source = CMD_SRC_NUC;
 #endif
-      }
     }
   }
+}
 #endif
-
+      // if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
+      //   ctx->output.balance_chassis.source = CMD_SRC_RC;
+      //   ctx->output.gimbal.source = CMD_SRC_RC;
+      //   ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_LEG_TEST;
+      //   ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RECOVER;
+      // }
   return CMD_OK;
 }
 

User/module/config.c

@@ -538,14 +538,15 @@ Config_RobotParam_t robot_config = {
             .gimbal_sw_up   = GIMBAL_MODE_RELAX,
             .gimbal_sw_mid  = GIMBAL_MODE_ABSOLUTE,
             // .gimbal_sw_down = GIMBAL_MODE_RECOVER,
-            .gimbal_sw_down = GIMBAL_MODE_AI_CONTROL,
+            .gimbal_sw_down = GIMBAL_MODE_RECOVER,
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
             .balance_sw_up   = CHASSIS_MODE_RELAX,
             //  .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,
+                .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
 #endif
         },
     },