添加腿长控制

application/chassis/balance.c

@@ -36,6 +36,9 @@ static LKMotorInstance *l_driven, *r_driven, *driven[2];
 static LinkNPodParam l_side, r_side;
 static ChassisParam chassis;
 
+// 综合运动补偿的PID控制器
+static PIDInstance leglen_pid_l, leglen_pid_r;     // 用PD模拟弹簧, 不要积分(弹簧是无积分二阶系统), 增益不可过大否则抗外界冲击响应时太"硬"
+
 // 底盘状态
 static Robot_Status_e chassis_status;
 
@@ -100,7 +103,23 @@ void BalanceInit()
     driven_conf.can_init_config.tx_id = 2;
     driven[LD] = l_driven = LKMotorInit(&driven_conf);
 
+    // 腿长控制
+    PID_Init_Config_s leg_length_pid_conf = {
+        .Kp = 600,
+        .Kd = 150,
+        .Ki = 0,
+        .MaxOut = 60,
+        .DeadBand = 0.0001f,
+        .Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
+        .CoefA = 0.01,
+        .CoefB = 0.02,
+        .Derivative_LPF_RC = 0.08,
+    };
+    PIDInit(&leglen_pid_l, &leg_length_pid_conf);
+    PIDInit(&leglen_pid_r, &leg_length_pid_conf);
+
     // 状态初始化
+    l_side.target_len = r_side.target_len = 0.12;
     chassis_status = ROBOT_READY;
     DWT_GetDeltaT(&balance_dwt_cnt);
 }
@@ -128,6 +147,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;
         }
     }
     else
@@ -140,9 +160,9 @@ static void ResetChassis()
 {
     EnableAllMotor();   // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
 
-    // // 撞墙时前后移动保证能重新站立,执行速度输入
-    // LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
-    // LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
+    // 撞墙时前后移动保证能重新站立,执行速度输入
+    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 &&
@@ -195,6 +215,13 @@ static void WokingStateSet()
 
     // 运动模式
     EnableAllMotor();
+
+    // 设置目标速度/腿长/距离
+    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);
 }
 
 
@@ -229,8 +256,20 @@ static void ParamAssemble()
     r_side.w_ecd = -r_driven->measure.speed_rads;
 }
 
+
+static void LegControl() /* 腿长控制和Roll补偿 */
+{
+    static float gravity_comp = 54.54;
+    l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp;
+    r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp;
+}
+
 static void WattLimitSet() /* 设定运动模态的输出 */
 {
+    HTMotorSetRef(lf, 0.2857f * -l_side.T_front); // 根据扭矩常数计算得到的系数
+    HTMotorSetRef(lb, 0.2857f * -l_side.T_back);
+    HTMotorSetRef(rf, 0.2857f * r_side.T_front);
+    HTMotorSetRef(rb, 0.2857f * r_side.T_back);
     LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
     LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);
 }
@@ -253,7 +292,18 @@ void BalanceTask()
     // 根据单杆计算处的角度和杆长,计算反馈增益
     CalcLQR(&l_side, &chassis);
     CalcLQR(&r_side, &chassis);
+    // 腿长控制,保持机体水平
+    LegControl();
+    // VMC映射成关节输出
+    VMCProject(&l_side);
+    VMCProject(&r_side);
+
+    // stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
+    if (chassis_status == ROBOT_STOP ||
+        chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
+        chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
+        return; // 复位模态或急停,直接退出
 
     // 运动模态,电机输出映射和限幅
-    // WattLimitSet();
+    WattLimitSet();
 }