添加腿部关节复位

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();
 }