修改与轮电机固连杆，使用机体速度计算LQR增益

application/chassis/balance.c

@@ -58,7 +58,7 @@ void BalanceInit()
             .can_handle = &hcan1},
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 0.3,
+                .Kp = 0.1,
                 .Kd = 0,
                 .Ki = 0,
                 .DeadBand = 0.0001,
@@ -102,9 +102,9 @@ void BalanceInit()
         .motor_type = LK9025,
     };
     driven_conf.can_init_config.tx_id = 1;
-    driven[RD] = r_driven = LKMotorInit(&driven_conf);
-    driven_conf.can_init_config.tx_id = 2;
     driven[LD] = l_driven = LKMotorInit(&driven_conf);
+    driven_conf.can_init_config.tx_id = 2;
+    driven[RD] = r_driven = LKMotorInit(&driven_conf);
 
     // 腿长控制
     PID_Init_Config_s leg_length_pid_conf = {
@@ -219,21 +219,21 @@ static void ResetChassis()
     chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
 
     // 撞墙时前后移动保证能重新站立,执行速度输入
-    LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
+    LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
-    LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
+    LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
 
     // 若关节完成复位,进入ready态
-    if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.025 &&
+    if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.03 &&
-        abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.025 &&
+        abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.03 &&
-        abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.025 &&
+        abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.03 &&
-        abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.025)
+        abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.03)
     {
         chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
     }
-    else if (abs(lf->measure.total_angle) <= 0.025 &&
+    else if (abs(lf->measure.total_angle) <= 0.03 &&
-             abs(lb->measure.total_angle) <= 0.025 &&
+             abs(lb->measure.total_angle) <= 0.03 &&
-             abs(rf->measure.total_angle) <= 0.025 &&
+             abs(rf->measure.total_angle) <= 0.03 &&
-             abs(rb->measure.total_angle) <= 0.025)
+             abs(rb->measure.total_angle) <= 0.03)
     {                                 // 双阈值保证关节能够复位而不会进入死区
         chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
 
@@ -279,6 +279,9 @@ static void WokingStateSet()
 
     // 运动模式
     EnableAllMotor();
+    // 保证关节电机为开环扭矩控制
+    for (uint8_t i = 0; i < JOINT_CNT; i++)
+        HTMotorOuterLoop(joint[i], OPEN_LOOP);
 
     // 设置目标速度/腿长/距离
     l_side.target_len += chassis_cmd_recv.delta_leglen;
@@ -297,6 +300,13 @@ static void WokingStateSet()
 
     // 角度输入
     chassis.target_yaw = chassis_cmd_recv.offset_angle; 
+
+    // TODO 转向速度限幅
+
+    // TODO 最大dist误差限幅
+
+    // TODO 最大速度误差限幅
+
 }
 
 
@@ -344,11 +354,12 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
     l_side.T_wheel -= steer_v_pid.Output;
     r_side.T_wheel += steer_v_pid.Output;
 
+    // 抗劈叉
     static float swerving_speed_ff, ff_coef = 0;
     swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
     PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
-    l_side.T_hip += anti_crash_pid.Output + swerving_speed_ff;
+    l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
-    r_side.T_hip -= anti_crash_pid.Output + swerving_speed_ff;
+    r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
 }
 
 
@@ -358,8 +369,8 @@ static void LegControl() /* 腿长控制和Roll补偿 */
     l_side.target_len += roll_compensate_pid.Output;
     r_side.target_len -= roll_compensate_pid.Output;
 
-    static float gravity_comp = 57.63;
+    static float gravity_comp = 60;
-    static float roll_extra_comp_p = 400;
+    static float roll_extra_comp_p = 300;
     float roll_comp = roll_extra_comp_p * chassis.roll;
     l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp;
     r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + roll_comp;
@@ -385,13 +396,6 @@ void BalanceTask()
     WokingStateSet();
     // 参数组装
     ParamAssemble();
-
-    // stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
-    if (chassis_status == ROBOT_STOP ||
-        chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
-        chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
-        return; // 复位模态或急停,直接退出
-
     // 将五连杆映射成单杆
     Link2Leg(&l_side, &chassis);
     Link2Leg(&r_side, &chassis);
@@ -407,6 +411,13 @@ void BalanceTask()
     // 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();
 }

application/chassis/balance.h

@@ -32,13 +32,13 @@
 #define RB 3u
 
 #define DRIVEN_CNT 2u
-#define RD 0u
+#define LD 0u
-#define LD 1u
+#define RD 1u
 
 typedef struct
 {
     // joint
-    float phi1_w, phi4_w, phi2_w, phi5_w; // phi2_w used for calc real wheel speed
+    float phi1_w, phi4_w, phi2_w, phi3_w, phi5_w; // phi2_w or phi3_w used for calc real wheel speed
     float T_back, T_front;
 
     // link angle, phi1-ph5, phi5 is pod angle

application/chassis/linkNleg.h

@@ -68,10 +68,12 @@ void Link2Leg(LinkNPodParam *p, ChassisParam *chassis)
     float phi2_pred = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
     xC = xB + CALF_LEN * mcos(phi2_pred);
     yC = yB + CALF_LEN * msin(phi2_pred);
+    float phi3_pred = atan2f(yC - yD, xC - xD);
     float phi5_pred = atan2f(yC, xC - JOINT_DISTANCE / 2);
 
     // 差分计算腿长变化率和腿角速度
-    p->phi2_w = (phi2_pred - p->phi2) / predict_dt; // 稍后用于修正轮速
+    p->phi2_w = (phi2_pred - p->phi2) / predict_dt;
+    p->phi3_w = (phi3_pred - p->phi3) / predict_dt; // 稍后用于修正轮速
     p->phi5_w = (phi5_pred - p->phi5) / predict_dt;
     p->legd = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->leg_len) / predict_dt;
     p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w * predict_dt) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w

application/chassis/lqr_calc.h

@@ -9,18 +9,18 @@
  */
 static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
 {
-    float k[12][3] = {155.616146,-200.327241,-0.278791,
+    float k[12][3] = {62.680622,-74.772126,-13.135672,
-                        -12.564474,-24.018026,0.674594,
+                        1.620796,-4.331826,-0.454705,
-                        145.503085,-87.742553,-5.916048,
+                        32.093563,-25.558681,-16.605856,
-                        97.588867,-70.361944,-4.007305,
+                        19.128242,-17.562747,-10.815380,
-                        303.894243,-240.356086,68.074439,
+                        225.373594,-201.324771,53.236052,
-                        18.743007,-15.738160,4.718314,
+                        13.575849,-13.013546,3.845604,
-                        -170.799127,75.225969,18.998160,
+                        76.704297,-72.201666,20.359891,
-                        -29.312006,20.892957,1.529644,
+                        4.520141,-4.051799,1.189345,
-                        122.553208,-114.023608,38.147691,
+                        139.418565,-125.750074,34.110069,
-                        63.561616,-64.916409,25.861226,
+                        88.295852,-79.170124,21.430975,
-                        -657.233777,402.487049,66.281303,
+                        -163.725633,127.852860,110.004619,
-                        -40.107302,25.117848,1.705173};
+                        -9.557678,7.476790,4.655220};
     float T[2] = {0};       // 0 T_wheel  1 T_hip
     float l = p->leg_len;
     float lsqr = l * l;

application/chassis/speed_estimation.h

@@ -17,8 +17,8 @@
 void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS_t *imu, float delta_t)
 {
     // 修正轮速和距离
-    lp->wheel_w = lp->w_ecd + lp->phi2_w - cp->pitch_w; // 减去和定子固连的phi2_w
+    lp->wheel_w = lp->w_ecd + lp->phi3_w - cp->pitch_w; // 减去和定子固连的phi2_w
-    rp->wheel_w = rp->w_ecd + rp->phi2_w - cp->pitch_w;
+    rp->wheel_w = rp->w_ecd + rp->phi3_w - cp->pitch_w;
 
     // 以轮子为基点,计算机体两侧髋关节处的速度
     lp->body_v = lp->wheel_w * WHEEL_RADIUS + lp->leg_len * lp->theta_w + lp->legd * msin(lp->theta);

application/robot_def.h

@@ -141,14 +141,13 @@ typedef struct
 {
     // 控制部分
     float vx;           // 前进方向速度
-    float rotate_w;      // 旋转速度, 目前仅在复位模式下使用
+    float rotate_w;     // 旋转速度, 目前仅在复位模式下使用
     float delta_leglen; // 腿长
     float offset_angle; // 底盘和归中位置的夹角
     chassis_mode_e chassis_mode;
     chassis_direction_e direction;
 
     // UI部分
-    lid_mode_e lid_mode;
     friction_mode_e friction_mode;
     Target_State_e target_state;
     loader_mode_e loader_mode;

modules/motor/HTmotor/HT04.h

@@ -11,7 +11,7 @@
 #define CURRENT_SMOOTH_COEF 0.9f
 #define SPEED_BUFFER_SIZE 5
 #define HT_SPEED_BIAS -0.0109901428f // 电机速度偏差,单位rad/s
-#define TORQUE_CONST_HT 3.5  // 扭矩常数,单位N.m/A
+#define TORQUE_COEF_HT 3.5  // 扭矩系数,单位N.m/A
 
 #define P_MIN -95.5f // Radians
 #define P_MAX 95.5f

modules/motor/LKmotor/LK9025.h

@@ -15,7 +15,7 @@
 #define SPEED_SMOOTH_COEF 0.85f
 #define REDUCTION_RATIO_DRIVEN 1
 #define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
-#define CURRENT_TORQUE_COEF_LK 0.00512f  // 电流设定值转换成扭矩的系数
+#define CURRENT_TORQUE_COEF_LK 0.00512f  // 电流设定值转换成扭矩的系数，这里对应的是16T
 
 typedef struct // 9025
 {