From d5b5c254c499195e5c40b31e4a485d41b4e6f887 Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Sun, 24 Mar 2024 22:34:47 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=80=9F=E5=BA=A6=E8=BE=93?=
 =?UTF-8?q?=E5=85=A5,=20=E8=85=BF=E9=95=BFpid=E5=BE=85=E8=B0=83=E6=95=B4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 28 ++++++++++++++++++++--------
 application/chassis/balance.h |  2 +-
 2 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index c0d82c6..0d63c26 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -55,8 +55,8 @@ void BalanceInit()
             .can_handle = &hcan1},
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 0.3,
-                .Kd = 0.1,
+                .Kp = 0.2,
+                .Kd = 0,
                 .Ki = 0,
                 .DeadBand = 0.0001,
                 .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
@@ -105,10 +105,10 @@ void BalanceInit()
 
     // 腿长控制
     PID_Init_Config_s leg_length_pid_conf = {
-        .Kp = 600,
-        .Kd = 150,
+        .Kp = 800,
+        .Kd = 300,
         .Ki = 0,
-        .MaxOut = 60,
+        .MaxOut = 20,
         .DeadBand = 0.0001f,
         .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
         .CoefA = 0.01,
@@ -148,7 +148,7 @@ static void ControlSwitch()
         {
             chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG;                        // 自由转动&前后
             chassis_cmd_recv.vx = 0.002 * (float)rc_data[TEMP].rc.rocker_r1;           // speed x, unit m/s
-            chassis_cmd_recv.delta_leglen = -0.0000015f * (float)rc_data[TEMP].rc.dial;
+            chassis_cmd_recv.delta_leglen = -0.000005f * (float)rc_data[TEMP].rc.dial;
         }
     }
     else
@@ -161,6 +161,10 @@ static void ResetChassis()
 {
     EnableAllMotor();   // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
 
+    // 复位时清空距离和腿长积累量,保证顺利站起
+    chassis.dist = chassis.target_dist = 0;
+    l_side.target_len = r_side.target_len = 0.12;
+
     // 撞墙时前后移动保证能重新站立,执行速度输入
     LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
     LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
@@ -221,8 +225,16 @@ static void WokingStateSet()
     l_side.target_len += chassis_cmd_recv.delta_leglen;
     r_side.target_len += chassis_cmd_recv.delta_leglen;
     // 腿长限幅
-    VAL_LIMIT(l_side.target_len, 0.12, 0.25);
-    VAL_LIMIT(r_side.target_len, 0.12, 0.25);
+    VAL_LIMIT(l_side.target_len, 0.12, 0.22);
+    VAL_LIMIT(r_side.target_len, 0.12, 0.22);
+
+    // 加速度限幅,防止键盘控制摔倒
+    if (abs(chassis_cmd_recv.vx - chassis.target_v) / del_t < MAX_ACC_REF)
+        chassis.target_v = chassis_cmd_recv.vx;
+    else
+        chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
+    // 模型距离参考输入
+    chassis.target_dist += chassis.target_v * del_t;
 }
 
 
diff --git a/application/chassis/balance.h b/application/chassis/balance.h
index f7d436c..2f7ed74 100644
--- a/application/chassis/balance.h
+++ b/application/chassis/balance.h
@@ -8,7 +8,7 @@
 #define LIMIT_LINK_RAD 0.205467224      // 初始限位角度,见ParamAssemble
 #define BALANCE_GRAVITY_BIAS 0
 #define ROLL_GRAVITY_BIAS 0
-#define MAX_ACC_REF 0.7f
+#define MAX_ACC_REF 0.5f
 #define MAX_DIST_TRACK 0.1f
 #define MAX_VEL_TRACK 0.5f