添加驱动轮支持力解算

application/chassis/balance.c

@@ -18,6 +18,7 @@
 #include "bsp_log.h"
 #include "lqr_calc.h"
 #include "speed_estimation.h"
+#include "fly_detection.h"
 
 // 计时变量
 static uint32_t balance_dwt_cnt;
@@ -160,7 +161,7 @@ void BalanceInit()
 
     // 抗劈叉
     PID_Init_Config_s anti_crash_pid_conf = {
-        .Kp = 10,
+        .Kp = 15,
         .Kd = 1,
         .Ki = 0.0,
         .MaxOut = 10,
@@ -420,6 +421,9 @@ void BalanceTask()
     // VMC映射成关节输出
     VMCProject(&l_side);
     VMCProject(&r_side);
+    // 驱动轮支持力解算
+    NormalForceSolve(&l_side, Chassis_IMU_data, del_t);
+    NormalForceSolve(&r_side, Chassis_IMU_data, del_t);
 
     // stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
     if (chassis_status == ROBOT_STOP ||

application/chassis/balance.h

@@ -12,6 +12,9 @@
 #define MAX_DIST_TRACK 1.0f
 #define MAX_VEL_TRACK 0.5f
 
+// 驱动轮质量
+#define WHEEL_MASS 0.58f
+
 // IMU距离中心的距离
 #define CENTER_IMU_W 0
 #define CENTER_IMU_L 0.1f
@@ -50,6 +53,8 @@ typedef struct
     float wheel_w;    // 单侧轮子的速度
     float body_v;     // 髋关节速度
     float T_wheel;
+    float zw_ddot;    // 驱动轮竖直方向加速度
+    float normal_force;  // 支持力
 
     // pod
     float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一

application/chassis/fly_detection.h

@@ -0,0 +1,25 @@
+#include "balance.h"
+#include "arm_math.h"
+#include "math.h"
+#include "user_lib.h"
+
+// 驱动轮支持力解算
+void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt)
+{
+    static float accx, accy, accz;
+    accx = imu->MotionAccel_b[X];
+    accy = imu->MotionAccel_b[Y];
+    accz = imu->MotionAccel_b[Z];
+
+    static float pitch, roll;
+    pitch = imu->Pitch;
+    roll = imu->Roll;
+
+    // 驱动轮竖直方向加速度
+    p->zw_ddot = -msin(roll) * accx + mcos(roll) * msin(pitch) * accy + mcos(pitch) * mcos(roll) * accz;
+
+    // 驱动轮支持力解算
+    static float P;
+    P = p->F_leg * mcos(p->theta) + p->T_hip * msin(p->theta) / p->leg_len;
+    p->normal_force = P + WHEEL_MASS * (p->zw_ddot + 9.81f); 
+}