改vmc

User/component/vmc.c

@@ -35,6 +35,7 @@
 #define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)
 
 /* Private variables -------------------------------------------------------- */
+static bool inited = false;
 /* Private function prototypes ---------------------------------------------- */
 
 static int8_t VMC_ValidateParams(const VMC_Param_t *param);
@@ -61,7 +62,7 @@ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
     // 重置状态
     VMC_Reset(vmc);
     
-    vmc->initialized = true;
+    inited = true;
     
     return 0;
 }
@@ -77,7 +78,7 @@ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
  * - 角度: 顺时针为正
  */
 int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) {
-    if (vmc == NULL || !vmc->initialized) {
+    if (vmc == NULL || !inited) {
         return -1;
     }
     body_pitch = -body_pitch; // 机体俯仰角取反
@@ -111,8 +112,8 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
     leg->phi0 = atan2f(leg->foot_y, leg->foot_x);
     
     // 计算等效摆动杆角度(相对于机体坐标系)
-    leg->alpha = VMC_PI_2 - leg->phi0;
+    leg->alpha = M_PI_2 - leg->phi0;
-    leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0);
+    leg->theta = M_PI - (M_PI_2 + body_pitch - leg->phi0);
     
     // 角度归一化
     VMC_ANGLE_NORMALIZE(leg->theta);
@@ -124,9 +125,6 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
     leg->d_theta = body_pitch_rate + leg->d_phi0;
     leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
     
-    // 计算角加速度
-    leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
-    
       VMC_GroundContactDetection(vmc); // 更新地面接触状态
     return 0;
 }
@@ -137,7 +135,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
  * 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩
  */
 int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
-    if (vmc == NULL || !vmc->initialized) {
+    if (vmc == NULL || !inited) {
         return -1;
     }
     
@@ -166,7 +164,7 @@ int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
  * 基于虚拟力和腿部状态估计地面法向力
  */
 float VMC_GroundContactDetection(VMC_t *vmc) {
-    if (vmc == NULL || !vmc->initialized) {
+    if (vmc == NULL || !inited) {
         return 0.0f;
     }
     
@@ -188,7 +186,7 @@ float VMC_GroundContactDetection(VMC_t *vmc) {
  * @brief 获取足端位置
  */
 int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
-    if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) {
+    if (vmc == NULL || !inited || x == NULL || y == NULL) {
         return -1;
     }
     
@@ -202,7 +200,7 @@ int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
  * @brief 获取等效摆动杆参数
  */
 int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) {
-    if (vmc == NULL || !vmc->initialized) {
+    if (vmc == NULL || !inited) {
         return -1;
     }
     
@@ -218,7 +216,7 @@ int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, flo
  * @brief 获取关节输出力矩
  */
 int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) {
-    if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) {
+    if (vmc == NULL || !inited || tau_front == NULL || tau_rear == NULL) {
         return -1;
     }
     
@@ -248,7 +246,7 @@ void VMC_Reset(VMC_t *vmc) {
  * @brief 设置虚拟力和力矩
  */
 void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
-    if (vmc == NULL || !vmc->initialized) {
+    if (vmc == NULL || !inited) {
         return;
     }
     
@@ -262,7 +260,7 @@ void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
  * 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
  */
 int8_t VMC_ComputeJacobian(VMC_t *vmc) {
-    if (vmc == NULL || !vmc->initialized) {
+    if (vmc == NULL || !inited) {
         return -1;
     }
     
@@ -301,7 +299,7 @@ int8_t VMC_ComputeJacobian(VMC_t *vmc) {
  */
 static int8_t VMC_ValidateParams(const VMC_Param_t *param) {
     if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 || 
-        param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) {
+        param->leg_3 <= 0 || param->leg_4 <= 0) {
         return -1;
     }
     

User/component/vmc.h

@@ -8,12 +8,14 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 #include <math.h>
-#include "kinematics.h"
+#include "user_math.h"
 
+/* Exported constants ------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
 
 /**
  * @brief VMC虚拟模型控制参数结构体
+ * 
  */
 typedef struct {
     float hip_length;       // 髋关节间距
@@ -21,8 +23,6 @@ typedef struct {
     float leg_2;           // 大腿后端长度 (L2) 
     float leg_3;           // 小腿长度 (L3)
     float leg_4;           // 小腿前端长度 (L4)
-    float wheel_radius;    // 轮子半径
-    float wheel_mass;      // 轮子质量
 } VMC_Param_t;
 
 /**
@@ -44,7 +44,6 @@ typedef struct {
     float d_L0;            // 等效摆动杆长度变化率
     float theta;           // 等效摆动杆角度
     float d_theta;         // 等效摆动杆角速度
-    float dd_theta;        // 等效摆动杆角加速度
 
     // 虚拟力和力矩
     float F_virtual;       // 虚拟支撑力
@@ -84,23 +83,16 @@ typedef struct {
     VMC_Param_t param;     // VMC参数
     VMC_Leg_t leg;         // 腿部状态
     float dt;              // 控制周期
-    bool initialized;      // 初始化标志
 } VMC_t;
 
-/* Exported constants ------------------------------------------------------- */
-
-#define VMC_PI_2        (1.5707963267948966f)
-#define VMC_PI          (3.1415926535897932f)
-#define VMC_2PI         (6.2831853071795865f)
-
 /* Exported macros ---------------------------------------------------------- */
 
 /**
  * @brief 角度范围限制到[-PI, PI]
  */
 #define VMC_ANGLE_NORMALIZE(angle) do { \
-    while((angle) > VMC_PI) (angle) -= VMC_2PI; \
+    while((angle) > M_PI) (angle) -= M_2PI; \
-    while((angle) < -VMC_PI) (angle) += VMC_2PI; \
+    while((angle) < -M_PI) (angle) += M_2PI; \
 } while(0)
 
 /* Exported functions prototypes -------------------------------------------- */
@@ -109,7 +101,7 @@ typedef struct {
  * @brief 初始化VMC控制器
  * @param vmc VMC控制器实例
  * @param param VMC参数
- * @param sample_freq 采样频率 (Hz)
+ * @param sample_freq 控制频率 (Hz)
  * @return 0:成功, -1:失败
  */
 int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq);
@@ -135,7 +127,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
 int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);
 
 /**
- * @brief 地面接触检测
+ * @brief 离地检测
  * @param vmc VMC控制器实例
  * @return 地面法向力 (N)
  */