简化了电机测量的命名,完成初版平衡底盘的功能编写,待测试方向和符号的正确性

application/balance_chassis/balance.h

@@ -4,31 +4,30 @@
 #define CALF_LEN 0.25f        // 小腿
 #define THIGH_LEN 0.15f       // 大腿
 #define JOINT_DISTANCE 0.108f // 关节间距
+#define WHEEL_RADIUS 0.032f   // 轮子半径
+#define LIMIT_LINK_RAD 0.358  //
 
-#define VELOCITY_DIFF_VMC //通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
+// 计算速度的方式(五连杆到单杆的映射)
+#define VELOCITY_DIFF_VMC // 通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
 // #define ANGLE_DIFF_VMC //直接保存上一次的值,通过差分计算腿长变化率和腿角速度
 
-
 typedef struct
 {
     // joint
-    float phi1_w, phi4_w;
+    float phi1_w, phi4_w, phi2_w; // phi2_w used for calc real wheel speed
     float T_back, T_front;
-
     // link angle,phi1-ph5, phi5 is pod angle
     float phi1, phi2, phi3, phi4, phi5;
-
     // wheel
-    float wheel_angle;
+    float wheel_dist;
     float wheel_w;
     float T_wheel;
-
     // pod
+    float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一
     float pod_len;
-    float pod_w;
-    float pod_v;
-    float F_pod;
-    float T_pod;
+    float height,height_v;
+    float pod_v, pod_w;
+    float F_pod, T_pod;
 } LinkNPodParam;
 
 /**