pitch限位改成联盟赛版本，电机角角度计算不准，会出问题

application/chassis/chassis.c

@@ -60,7 +60,7 @@ float chassis_power = 0;
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
 float P_cmdall = 0;
-static uint8_t last_chassis_power_limit=0;
+static uint8_t input=0;
 void ChassisInit()
 {
 
@@ -119,7 +119,14 @@ void ChassisInit()
                     .can_handle = &hcan1,
                     .tx_id = 0x210,
                     .rx_id = 0x211,
-            }};
+            },
+            .buffer_pid_config = {
+                    .Kp = 0.5f,
+                    .Ki = 0,
+                    .Kd = 0,
+                    .MaxOut = 500,
+            }
+    };
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
     SuperCapSetPower(cap,70);  // 超级电容限制功率
 
@@ -218,11 +225,16 @@ static void LimitChassisOutput()
 static void SuperCapSetUpdate()
 {
 
-    if(last_chassis_power_limit != chassis_cmd_recv.chassis_power_limit)
-    {
-        SuperCapSetPower(cap,chassis_cmd_recv.chassis_power_limit);  // 超级电容限制功率
-        last_chassis_power_limit = chassis_cmd_recv.chassis_power_limit;
-    }
+//    PIDCalculate(&cap->buffer_pid, chassis_cmd_recv.chassis_power_buffer,30);
+//    input = chassis_cmd_recv.chassis_power_limit - cap->buffer_pid.Output;
+//    SuperCapSetPower(cap,input);
+
+
+//    if(last_chassis_power_limit != chassis_cmd_recv.chassis_power_limit)
+//    {
+//        SuperCapSetPower(cap,chassis_cmd_recv.chassis_power_limit);  // 超级电容限制功率
+//        last_chassis_power_limit = chassis_cmd_recv.chassis_power_limit;
+//    }
 }
 /**
  * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算

application/cmd/robot_cmd.c

@@ -64,6 +64,9 @@ static referee_info_t* referee_data; // 用于获取裁判系统的数据
 static referee_info_vt_t* referee_vt_data;
 static uint8_t Pitch_Limit_Flag=1;
 
+float PitchMotorAngle;
+float DeltaPitchAngle;
+
 void RobotCMDInit()
 {
     rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
@@ -157,9 +160,8 @@ static void pitch_limit()
 
 //    if(!Pitch_Limit_Flag)
 //    {
-//        float Delta_total_angle;//pitch电机自水平位置转过的角度
-//        Delta_total_angle = gimbal_fetch_data.pitch_motor_total_angle-Horizontal_Pitch_total_angle;
-//        PitchMotorAngle = -0.0019f * Delta_total_angle - 0.4059;//电机旋转导致的云台相对底盘角度
+//
+//        PitchMotorAngle = -0.0019f * gimbal_fetch_data.pitch_motor_total_angle - 0.4059;//电机旋转导致的云台相对底盘角度
 //        DeltaPitchAngle = gimbal_fetch_data.gimbal_imu_data.Pitch - PitchMotorAngle;//陀螺仪和电机角的误差
 //        gimbal_cmd_send.pitch = LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE-DeltaPitchAngle, PITCH_MAX_ANGLE-DeltaPitchAngle);
 //    }

modules/super_cap/super_cap.c

@@ -24,6 +24,7 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
 
     supercap_config->can_config.can_module_callback = SuperCapRxCallback;
     super_cap_instance->can_ins = CANRegister(&supercap_config->can_config);
+    PIDInit(&super_cap_instance->buffer_pid, &supercap_config->buffer_pid_config);
     return super_cap_instance;
 }
 

modules/super_cap/super_cap.h

@@ -2,6 +2,7 @@
 #define SUPER_CAP_H
 
 #include "bsp_can.h"
+#include "controller.h"
 
 #pragma pack(1)
 typedef struct
@@ -18,12 +19,14 @@ typedef struct
 {
     CANInstance *can_ins; // CAN实例
     SuperCap_Msg_s cap_msg; // 超级电容信息
+    PIDInstance buffer_pid; //缓冲功率闭环控制参数
 } SuperCapInstance;
 
 /* 超级电容初始化配置 */
 typedef struct
 {
     CAN_Init_Config_s can_config;
+    PID_Init_Config_s buffer_pid_config;
 } SuperCap_Init_Config_s;
 
 /**