From 89609e884a15b21188d3ff28ae117efb3ce2f441 Mon Sep 17 00:00:00 2001
From: zcj <2487150395@qq.com>
Date: Thu, 16 May 2024 22:15:26 +0800
Subject: [PATCH] =?UTF-8?q?=E5=8A=9F=E7=8E=87=E6=8E=A7=E5=88=B6=E5=8A=A0?=
 =?UTF-8?q?=E8=B6=85=E7=94=B5=EF=BC=8C=E5=8F=82=E6=95=B0=E5=8F=AF=E4=BB=A5?=
 =?UTF-8?q?=E6=9B=B4=E6=BF=80=E8=BF=9B=E4=B8=80=E7=82=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/chassis.c | 33 ++++++++++++++++++++-------------
 application/cmd/robot_cmd.c   |  6 +++---
 2 files changed, 23 insertions(+), 16 deletions(-)

diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c
index 2d1b665..482cce8 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 input=0;
+static float input=0;
 void ChassisInit()
 {
 
@@ -121,10 +121,10 @@ void ChassisInit()
                     .rx_id = 0x211,
             },
             .buffer_pid_config = {
-                    .Kp = 0.5f,
+                    .Kp = 0.7f,
                     .Ki = 0,
                     .Kd = 0,
-                    .MaxOut = 500,
+                    .MaxOut = 300,
             }
     };
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
@@ -183,6 +183,7 @@ static void MecanumCalculate()
  */
 static void LimitChassisOutput()
 {
+    static float P_max=0;
     float initial_total_power[4]={motor_rf->motor_controller.motor_power_predict ,
                                   motor_rb->motor_controller.motor_power_predict ,
                                   motor_lb->motor_controller.motor_power_predict ,
@@ -195,11 +196,14 @@ static void LimitChassisOutput()
         P_cmdall += initial_total_power[i];
     }
 
-
-    float P_max = (float)(chassis_cmd_recv.chassis_power_limit + 5 + chassis_cmd_recv.P_SuperCap) ;
-
-    if(chassis_cmd_recv.chassis_power_buffer<10)//当缓冲功率过小时，限制功率给小；
-        P_max = chassis_cmd_recv.chassis_power_limit;
+    if (cap->cap_msg.cap_vol>1800)
+    {
+         P_max = input + chassis_cmd_recv.P_SuperCap ;
+    }
+    else
+    {
+        P_max = input;
+    }
     float K = P_max/P_cmdall;
 
 
@@ -225,9 +229,10 @@ static void LimitChassisOutput()
 static void SuperCapSetUpdate()
 {
 
-//    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);
+    PIDCalculate(&cap->buffer_pid, chassis_cmd_recv.chassis_power_buffer,30);
+    input = chassis_cmd_recv.chassis_power_limit - cap->buffer_pid.Output;
+    LIMIT_MIN_MAX(input, 30, 130);
+    SuperCapSetPower(cap,input);
 
 
 //    if(last_chassis_power_limit != chassis_cmd_recv.chassis_power_limit)
@@ -325,13 +330,15 @@ void ChassisTask()
     MecanumCalculate();
     //读取底盘功率，方便功率控制
 //    chassis_power = PowerMeterGet(power_meter);
+
+    //更新超电设定值
+    SuperCapSetUpdate();
     // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
     LimitChassisOutput();
     // 根据电机的反馈速度和IMU(如果有)计算真实速度
     EstimateSpeed();
 
-    //更新超电设定值
-    SuperCapSetUpdate();
+
 
     // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
     // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c
index cbb3646..a65d755 100644
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@@ -156,7 +156,7 @@ static void update_ui_data()
 }
 static void pitch_limit()
 {
-    gimbal_cmd_send.pitch = LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
+    LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
 
 //    if(!Pitch_Limit_Flag)
 //    {
@@ -433,10 +433,10 @@ static void MouseKeySet()
     switch (rc_data[TEMP].key[KEY_PRESS].shift) //shift开关超电//
     {
         case 0:
-            chassis_cmd_send.P_SuperCap=0;
+            chassis_cmd_send.P_SuperCap=5;
             break;
         case 1:
-            chassis_cmd_send.P_SuperCap=195;
+            chassis_cmd_send.P_SuperCap=200;
             break;
     }
     pitch_limit();