From cbfb2990d21f84448ab97ba80f188897b7b435b8 Mon Sep 17 00:00:00 2001
From: zcj <2487150395@qq.com>
Date: Thu, 16 May 2024 15:30:08 +0800
Subject: [PATCH] =?UTF-8?q?pitch=E9=99=90=E4=BD=8D=E6=94=B9=E6=88=90?=
 =?UTF-8?q?=E8=81=94=E7=9B=9F=E8=B5=9B=E7=89=88=E6=9C=AC=EF=BC=8C=E7=94=B5?=
 =?UTF-8?q?=E6=9C=BA=E8=A7=92=E8=A7=92=E5=BA=A6=E8=AE=A1=E7=AE=97=E4=B8=8D?=
 =?UTF-8?q?=E5=87=86=EF=BC=8C=E4=BC=9A=E5=87=BA=E9=97=AE=E9=A2=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/chassis.c | 26 +++++++++++++++++++-------
 application/cmd/robot_cmd.c   |  8 +++++---
 modules/super_cap/super_cap.c |  1 +
 modules/super_cap/super_cap.h |  3 +++
 4 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c
index 4bfc4a7..2d1b665 100644
--- a/application/chassis/chassis.c
+++ b/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 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c
index ae2be56..cbb3646 100644
--- a/application/cmd/robot_cmd.c
+++ b/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);
 //    }
diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c
index a75d507..f726bbc 100644
--- a/modules/super_cap/super_cap.c
+++ b/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;
 }
 
diff --git a/modules/super_cap/super_cap.h b/modules/super_cap/super_cap.h
index 171c851..ed1736b 100644
--- a/modules/super_cap/super_cap.h
+++ b/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;
 
 /**