From 57723a3b67ebc9b0e7b1587398983d6dd94b31f9 Mon Sep 17 00:00:00 2001
From: kai <1797003616@qq.com>
Date: Sun, 24 Mar 2024 13:23:10 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=85=BF=E9=83=A8=E5=85=B3?=
 =?UTF-8?q?=E8=8A=82=E5=A4=8D=E4=BD=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 application/chassis/balance.c | 65 ++++++++++++++++++++++++++++++++---
 1 file changed, 61 insertions(+), 4 deletions(-)

diff --git a/application/chassis/balance.c b/application/chassis/balance.c
index 479b49c..b886c87 100644
--- a/application/chassis/balance.c
+++ b/application/chassis/balance.c
@@ -50,8 +50,19 @@ void BalanceInit()
         // 写一个,剩下的修改方向和id即可
         .can_init_config = {
             .can_handle = &hcan1},
+        .controller_param_init_config = {
+            .angle_PID = {
+                .Kp = 0.3,
+                .Kd = 0.1,
+                .Ki = 0,
+                .DeadBand = 0.0001,
+                .Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
+                .MaxOut = 4,
+                .Derivative_LPF_RC = 0.05,
+            }, // 仅用于复位腿
+        },
         .controller_setting_init_config = {
-            .close_loop_type = OPEN_LOOP,
+            .close_loop_type = ANGLE_LOOP,
             .outer_loop_type = OPEN_LOOP,
             .motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
             .angle_feedback_source = MOTOR_FEED,
@@ -111,7 +122,7 @@ static void ControlSwitch()
         if (rc_data->rc.rocker_l1 < -600)
         {
             chassis_cmd_recv.chassis_mode = CHASSIS_RESET;
-            chassis_cmd_recv.vx = 0.005 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
+            chassis_cmd_recv.vx = 0.05 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
         }
         else
         {
@@ -124,10 +135,56 @@ static void ControlSwitch()
 }
 
 
+/* 腿缩回复位,只允许驱动轮电机移动 */
+static void ResetChassis()
+{
+    EnableAllMotor();   // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
+
+    // // 撞墙时前后移动保证能重新站立,执行速度输入
+    // LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
+    // LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
+
+    // 若关节完成复位,进入ready态
+    if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.02 &&
+        abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.02 &&
+        abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.02 &&
+        abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.02)
+    {
+        chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
+    }
+    else if (abs(lf->measure.total_angle) <= 0.02 &&
+             abs(lb->measure.total_angle) <= 0.02 &&
+             abs(rf->measure.total_angle) <= 0.02 &&
+             abs(rb->measure.total_angle) <= 0.02)
+    {                                 // 双阈值保证关节能够复位而不会进入死区
+        chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
+
+        for (uint8_t i = 0; i < JOINT_CNT; i++)
+            HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
+
+        return;                                    // 退出函数不再执行关节指令
+    }
+    else
+        chassis_status = ROBOT_STOP;
+
+    // 还在复位中,关节改为位置环,执行复位
+    for (uint8_t i = 0; i < JOINT_CNT; i++)
+    {
+        HTMotorOuterLoop(joint[i], ANGLE_LOOP);
+        HTMotorSetRef(joint[i], 0);
+    }
+}
+
+
 // 工作状态设定
 static void WokingStateSet()
 {
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式
+    {
+        ResetChassis();
+        return;
+    }
+    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
     {
         for (uint8_t i = 0; i < JOINT_CNT; i++)
             HTMotorStop(joint[i]);
@@ -198,5 +255,5 @@ void BalanceTask()
     CalcLQR(&r_side, &chassis);
 
     // 运动模态,电机输出映射和限幅
-    WattLimitSet();
+    // WattLimitSet();
 }
\ No newline at end of file