add_gimbal_recover

CMakeLists.txt

@@ -83,6 +83,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/device/vofa.c
     User/device/mrobot.c
     User/device/referee.c
+    User/device/supercap.c
     # User/module sources
     User/module/balance_chassis.c
     User/module/config.c

User/device/supercap.h

@@ -98,6 +98,8 @@ uint32_t get_chassis_energy_from_supercap(void);
 
 int8_t SuperCap_Init(void);
 int8_t SuperCap_Update(void);
+int8_t PowerLimit_Output_by_cap(float power_limit, float *motor_out, uint32_t len);
+int8_t PowerLimit_Output(float power_limit, float *motor_out, uint32_t len);
 
 /* UI 导出结构（供 referee 系统绘制） */
 typedef struct {

User/module/aimbot.h

@@ -6,7 +6,7 @@ extern "C" {
 #endif
 
 /* Includes ----------------------------------------------------------------- */
-#include "component\user_math.h"
+#include "component/user_math.h"
 #include "module/gimbal.h"
 #include "bsp/fdcan.h"
 #include <stdint.h>

User/module/balance_chassis.c

@@ -11,6 +11,7 @@
 #include "component/user_math.h"
 #include "device/motor_lk.h"
 #include "device/motor_lz.h"
+#include "device/supercap.h"
 #include <math.h>
 #include <stddef.h>
 #include <string.h>
@@ -1363,4 +1364,4 @@ void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui) {
   ui->mode = c->mode;
   // ui->angle = c->feedback.yaw.rotor_abs_angle - c->mech_zero;
   // #error "右边那个mech_zero应该是跟随云台的那个角,我没找着在哪"
-}
+}

User/module/cap.c

@@ -4,8 +4,8 @@
 
 /* Includes ----------------------------------------------------------------- */
 #include "cap.h"
-#include "component\limiter.h"
+#include "component/limiter.h"
-#include "device\referee.h"
+#include "device/referee.h"
 
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */

User/module/cap.h

@@ -9,8 +9,8 @@ extern "C" {
 #endif
 
 /* Includes ----------------------------------------------------------------- */
-#include "device\supercap.h"
+#include "device/supercap.h"
-#include "device\referee.h"
+#include "device/referee.h"
 
 /* Exported constants ------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */

User/module/config.c

@@ -80,6 +80,9 @@ 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,

User/module/gimbal.c

@@ -50,7 +50,6 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
   LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
 
   MOTOR_DM_Enable(&(g->param->yaw_motor));
-  MOTOR_DM_Enable(&(g->param->pit_motor));
 
   AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
 
@@ -256,6 +255,34 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
                           g->feedback.imu.gyro.x, 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,7 +30,8 @@ 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 系统绘制） */
@@ -74,10 +75,10 @@ typedef struct {
     KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
     KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
     struct {
-      KPID_t yaw_angle; /* yaw轴角位置环PID */
+      KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
-      KPID_t yaw_omega; /* yaw轴角速度环PID */
+      KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
-      KPID_t pit_angle; /* pitch轴角位置环PID */
+      KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
-      KPID_t pit_omega; /* pitch轴角速度环PID */
+      KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
     } aimbot;
   } pid;
 
@@ -97,6 +98,11 @@ typedef struct {
     float pit; /* pitch轴机械限位行程 -1表示无限位*/
   } travel;
 
+  /* Recover模式参数 */
+  struct {
+    float yaw_mech_zero;       /* yaw机械零点（编码器角度） */
+  } recover;
+
 } Gimbal_Params_t;
 
 /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */