平衡控制有了，但是一坨狗屎

.vscode/settings.json

@@ -8,6 +8,8 @@
         "kinematics.h": "c",
         "bezier_curve.h": "c",
         "queue": "c",
-        "stack": "c"
+        "stack": "c",
+        "buzzer.h": "c",
+        "user_task.h": "c"
     }
 }

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -158,32 +158,22 @@
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>dr16</ItemText>
+          <ItemText>chassis</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>chassis</ItemText>
+          <ItemText>go_feedback</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>n100</ItemText>
+          <ItemText>go</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>go_feedback</ItemText>
+          <ItemText>n100_cali</ItemText>
-        </Ww>
-        <Ww>
-          <count>4</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>param_default</ItemText>
-        </Ww>
-        <Ww>
-          <count>5</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>chassis_cmd</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>

User/bsp/buzzer.c

@@ -3,6 +3,7 @@
 
 #include <main.h>
 #include <tim.h>
+#include "bsp/delay.h"
 
 /* Private define ----------------------------------------------------------- */
 /* Private macro ------------------------------------------------------------ */
@@ -10,6 +11,9 @@
 /* Private variables -------------------------------------------------------- */
 /* Private function  -------------------------------------------------------- */
 /* Exported functions ------------------------------------------------------- */
+
+
+
 int8_t BSP_Buzzer_Start(void) {
   if (HAL_TIM_PWM_Start(&htim12, TIM_CHANNEL_2) == HAL_OK) return BSP_OK;
   return BSP_ERR;
@@ -30,6 +34,22 @@ int8_t BSP_Buzzer_Set(float freq, float duty_cycle) {
   return BSP_OK;
 }
 
+int8_t BSP_Buzzer_Set_Note(BSP_Buzzer_Note_t note, float delay_sec) {
+  if (note <= 0 || delay_sec < 0.0f) return BSP_ERR;
+
+  uint32_t timer_clk = HAL_RCC_GetPCLK1Freq(); // 具体时钟频率请根据你的芯片和配置调整
+  uint32_t prescaler = htim12.Init.Prescaler + 1;
+  uint32_t period = (uint32_t)((float)timer_clk / (prescaler * note)) - 1;
+
+  __HAL_TIM_SET_AUTORELOAD(&htim12, period);
+
+  uint32_t pulse = (uint32_t)(0.5f * (float)(period + 1)); // 设置50%的占空比
+  __HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_2, pulse);
+
+  BSP_Delay((uint32_t)(delay_sec * 1000.0f)); // 延时指定秒数
+  return BSP_OK;
+}
+
 int8_t BSP_Buzzer_Stop(void) {
   if (HAL_TIM_PWM_Stop(&htim12, TIM_CHANNEL_2) == HAL_OK) return BSP_OK;
   return BSP_ERR;

User/bsp/buzzer.h

@@ -13,8 +13,43 @@ extern "C" {
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
 /* Exported functions prototypes -------------------------------------------- */
+/*预设音符列表*/
+typedef enum {
+    BSP_BUZZER_NOTE_C4 = 261,   // C4音符频率
+    BSP_BUZZER_NOTE_D4 = 294,   // D4音符频率
+    BSP_BUZZER_NOTE_E4 = 329,   // E4音符频率
+    BSP_BUZZER_NOTE_F4 = 349,   // F4音符频率
+    BSP_BUZZER_NOTE_G4 = 392,   // G4音符频率
+    BSP_BUZZER_NOTE_A4 = 440,   // A4音符频率
+    BSP_BUZZER_NOTE_B4 = 494,   // B4音符频率
+    BSP_BUZZER_NOTE_C5 = 523,   // C5音符频率
+    BSP_BUZZER_NOTE_D5 = 587,   // D5音符频率
+    BSP_BUZZER_NOTE_E5 = 659,   // E5音符频率
+    BSP_BUZZER_NOTE_F5 = 698,   // F5音符频率
+    BSP_BUZZER_NOTE_G5 = 784,   // G5音符频率
+    BSP_BUZZER_NOTE_A5 = 880,   // A5音符频率
+    BSP_BUZZER_NOTE_B5 = 988,   // B5音符频率
+    BSP_BUZZER_NOTE_C6 = 1047,  // C6音符频率
+    BSP_BUZZER_NOTE_D6 = 1175,  // D6音符频率
+    BSP_BUZZER_NOTE_E6 = 1319,  // E6音符频率
+    BSP_BUZZER_NOTE_F6 = 1397,  // F6音符频率
+    BSP_BUZZER_NOTE_G6 = 1568,  // G6音符频率
+    BSP_BUZZER_NOTE_A6 = 1760,  // A6音符频率
+    BSP_BUZZER_NOTE_B6 = 1976,  // B6音符频率
+    BSP_BUZZER_NOTE_C7 = 2093,  // C7音符频率
+    BSP_BUZZER_NOTE_D7 = 2349,  // D7音符频率
+    BSP_BUZZER_NOTE_E7 = 2637,  // E7音符频率
+    BSP_BUZZER_NOTE_F7 = 2794,  // F7音符频率
+    BSP_BUZZER_NOTE_G7 = 3136,  // G7音符频率
+    BSP_BUZZER_NOTE_A7 = 3520,  // A7音符频率
+    BSP_BUZZER_NOTE_B7 = 3951,  // B7音符频率
+    BSP_BUZZER_NOTE_C8 = 4186   // C8音符频率
+} BSP_Buzzer_Note_t;
+
+
 int8_t BSP_Buzzer_Start(void);
 int8_t BSP_Buzzer_Set(float freq, float duty_cycle);
+int8_t BSP_Buzzer_Set_Note(BSP_Buzzer_Note_t note, float delay_sec);
 int8_t BSP_Buzzer_Stop(void);
 
 #ifdef __cplusplus

User/component/cmd.c

@@ -99,7 +99,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
   cmd->chassis.ctrl_vec.vy = rc->ch_l_x;
   cmd->chassis.ctrl_vec.wz = rc->ch_r_x;
   cmd->chassis.delta_hight = rc->ch_res;
-
+  cmd->chassis.delta_eulr.pit = rc->ch_r_y;
 }
 
 // /**

User/component/cmd.h

@@ -38,6 +38,7 @@ typedef struct {
   CMD_ChassisAction_t action; /* 底盘动作 */
   float delta_hight;          /* 底盘高度，单位：米 */
   MoveVector_t ctrl_vec;      /* 底盘控制向量 */
+  AHRS_Eulr_t delta_eulr;     /* 云台欧拉角变化量 */
 } CMD_ChassisCmd_t;
 
 typedef struct {

User/component/kinematics.c

@@ -49,31 +49,6 @@ int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_Joint
     return 0;
 }
 
-void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3],  Kinematics_Sign_t *sign) {
-    float l1 = leg_params->hip * sign->hip;
-    float l2 = -leg_params->thigh;
-    float l3 = -leg_params->calf;
-
-    float q1 = theta_in[0];
-    float q2 = theta_in[1] * sign->thigh;
-    float q3 = theta_in[2] * sign->calf;
-
-    float s1 = sinf(q1);
-    float s2 = sinf(q2);
-    float s3 = sinf(q3);
-
-    float c1 = cosf(q1);
-    float c2 = cosf(q2);
-    float c3 = cosf(q3);
-
-    float c23 = c2 * c3 - s2 * s3;
-    float s23 = s2 * c3 + c2 * s3;
-
-    foot_out[0] = l3 * s23 + l2 * s2;
-    foot_out[1] = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
-    foot_out[2] =  l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
-}
-
 /**
  * @brief 单腿逆运动学
  * @param foot_in 足端目标位置 [x, y, z]（单位：米）
@@ -110,3 +85,38 @@ int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_Lin
     theta_out[2] = q3 * sign->calf;
     return 0;
 }
+
+
+void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3],  Kinematics_Sign_t *sign) {
+    float l1 = leg_params->hip * sign->hip;
+    float l2 = -leg_params->thigh;
+    float l3 = -leg_params->calf;
+
+    float q1 = theta_in[0];
+    float q2 = theta_in[1] * sign->thigh;
+    float q3 = theta_in[2] * sign->calf;
+
+    float s1 = sinf(q1);
+    float s2 = sinf(q2);
+    float s3 = sinf(q3);
+
+    float c1 = cosf(q1);
+    float c2 = cosf(q2);
+    float c3 = cosf(q3);
+
+    float c23 = c2 * c3 - s2 * s3;
+    float s23 = s2 * c3 + c2 * s3;
+
+    float px = l3 * s23 + l2 * s2;
+    float py = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
+    float pz =  l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
+
+    // 配合逆运动学的符号处理
+    if (sign->hip != sign->thigh) {
+        foot_out[1] = py;
+    } else {
+        foot_out[1] = -py;
+    }
+    foot_out[0] = px;
+    foot_out[2] = pz;
+}

User/component/limiter.c

@@ -9,19 +9,45 @@
 
 #include "component/user_math.h"
 
-int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, float max_speed, float max_angle, float min_angle){
+int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos,float *out_troque, float max_speed, float max_angle, float min_angle, float max_torque) {
 
   // 限制位置变化速度
-  if (*out_pos - feedback_pos > max_speed) {
+  // if (*out_pos - feedback_pos > max_speed) {
-    *out_pos = feedback_pos + max_speed;
+  //   *out_pos = feedback_pos + max_speed;
-  } else if (*out_pos - feedback_pos < -max_speed) {
+  // } else if (*out_pos - feedback_pos < -max_speed) {
-    *out_pos = feedback_pos - max_speed;
+  //   *out_pos = feedback_pos - max_speed;
+  // }
+  if (fabsf(*out_pos - feedback_pos) > max_speed) {
+    if (*out_pos > feedback_pos) {
+      *out_pos = feedback_pos + max_speed;
+    } else {
+      *out_pos = feedback_pos - max_speed;
+    }
   }
+
   // 限制角度范围
   if (*out_pos > max_angle) {
     *out_pos = max_angle;
   } else if (*out_pos < min_angle) {
     *out_pos = min_angle;
   }
+
+  /*限制前馈力矩*/
+  // if (fabsf(*out_pos) > max_torque) {
+  //   if (*out_pos > 0) {
+  //     *out_pos = max_torque;
+  //   } else {
+  //     *out_pos = -max_torque;
+  //   }
+  // }
+  if (fabsf(*out_troque) > max_torque) {
+    if (*out_troque > 0) {
+      *out_troque = max_torque;
+    } else {
+      *out_troque = -max_torque;
+    }
+  }
+
+
   return 0; // 成功
 }

User/component/limiter.h

@@ -11,8 +11,8 @@ extern "C" {
 #include <stdbool.h>
 #include <stdint.h>
 
-int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, 
+int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, float *out_torque,
-  float max_speed, float max_angle, float min_angle);
+  float max_speed, float max_angle, float min_angle, float max_torque);
 
 
 

User/component/path.c

@@ -4,6 +4,10 @@
 
 #include "path.h"
 
+float Path_Straight1d(float start, float end, float t) {
+    return start + (end - start) * t;
+}
+
 /**
  * @brief 生成一条直线段的路径点
  * @param start 起始点

User/component/path.h

@@ -11,6 +11,9 @@ extern "C" {
 #include <stdbool.h>
 #include <stdint.h>
 
+
+float Path_Straight1d(float start, float end, float t);
+
 /**
  * @brief 生成一条直线段的路径点
  * @param start 起始点

User/device/n100.c

@@ -133,7 +133,7 @@ bool N100_WaitDmaCplt(void) {
           SIGNAL_N100_NEW_DATA);
 }
 
-int8_t N100_ParseData (N100_t *n100) {
+int8_t N100_ParseData (N100_t *n100, N100_Cali_t *cali) {
     if (n100 == NULL) return DEVICE_ERR_NULL;
     // 先校验原始数据
     if (!N100_Verify(rxbuf)) {
@@ -144,7 +144,9 @@ int8_t N100_ParseData (N100_t *n100) {
     n100->eulr = n100->rx_raw.data.eulr;
     n100->gyro = n100->rx_raw.data.gyro;
     n100->quat = n100->rx_raw.data.quat;
-
+    n100->eulr.rol += cali->offset_x; /* 偏航角校准 */
+    n100->eulr.pit += cali->offset_y; /* 俯仰角校准 */
+    n100->eulr.yaw += cali->offset_z; /* 翻滚角校准 */
     return DEVICE_OK;
 }
 

User/device/n100.h

@@ -37,6 +37,11 @@ typedef struct {
     uint8_t end;
 } N100_AHRS_t;
 
+typedef struct {
+  float offset_x;
+  float offset_y;
+  float offset_z;
+} N100_Cali_t;
 
 typedef struct {
   osThreadId_t thread_alert;
@@ -52,7 +57,7 @@ int8_t N100_Restart(void);
 
 int8_t N100_StartReceiving(N100_t *n100);
 bool N100_WaitDmaCplt(void);
-int8_t N100_ParseData (N100_t *n100);
+int8_t N100_ParseData (N100_t *n100, N100_Cali_t *cali);
 int8_t N100_HandleOffline(N100_t *n100);
 
 #ifdef __cplusplus

User/module/chassis.c

@@ -15,9 +15,10 @@
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
 
-#define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */
+#define CHASSIS_DEFAULT_HEIGHT (0.22f) /* 底盘默认高度，单位：米 */
 
-#define CHASSIS_MAX_SPEED (0.03f) /*调试用速度限幅*/
+#define CHASSIS_MAX_SPEED (0.02f) /*调试用速度限幅*/
+#define CHASSIS_MAX_TORQUE (1.0f) /*调试用力矩限幅*/
 /* Private macro ------------------------------------------------------------ */
 
 
@@ -43,13 +44,14 @@ static Kinematics_JointCMD_t position_mode = {
     .T = 0.0f, /* 零力矩 */
     .W = 0.0f, /* 零速度 */
     .Pos = 0.0f, /* 零位置 */
-    .K_P = 3.0f, /* 刚度系数 */
+    .K_P = 1.2f, /* 刚度系数 */
-    .K_W = 0.2f, /* 速度系数 */
+    .K_W = 0.05f, /* 速度系数 */
 };
 
 // static uint8_t chassis_mode_states = 0;
 // static uint8_t chassis_action_states = 0;
 static uint8_t stage = 0;
+static float T = 0.0f; /* 记录当前时间 */
 /* Private function  -------------------------------------------------------- */
 /**
  * \brief 设置底盘运行模式
@@ -62,19 +64,26 @@ static uint8_t stage = 0;
 static int8_t Chassis_SetMode(Chassis_t *c, CMD_ChassisCmd_t *c_cmd) {
   if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
   if (c_cmd == NULL) return CHASSIS_ERR_NULL; /* 控制指令不能为空 */
+  if (c->mode == c_cmd->mode && c->action == c_cmd->action) {
+    return CHASSIS_OK; /* 如果当前模式和动作与要设置的相同，直接返回 */
+  }
+  stage = 0;
+  c->mode = c_cmd->mode;
+  c->action = c_cmd->action; /* 更新底盘模式和动作 */
+  c->time = 0.0f; /* 重置时间 */
+  c->eulr_setpoint.pit = 0;
+  c->eulr_setpoint.rol = 0.0f; /* 重置期望的俯仰角和翻滚角 */
+  c->eulr_setpoint.yaw = c->eulr_imu.yaw; /* 重置期望的偏航角为当前IMU偏航角 */
+  if (c->mode != CHASSIS_MODE_POSITION) {
+    c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
+  }
+  for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
+    PID_Reset(c->pid.motor_id + i);
+    LowPassFilter2p_Reset(c->filter.in + i, 0.0f);
+    LowPassFilter2p_Reset(c->filter.out + i, 0.0f);
+  }
+  PID_Reset(&c->pid.blance); /* 重置平衡PID */
 
-  if (c->mode != c_cmd->mode) { /* 如果当前模式和要设置的模式不同 */
-    c->mode = c_cmd->mode; /* 更新底盘模式 */
-    stage = 0; /* 重置阶段 */
-    c->time = 0.0f; /* 重置时间 */
-    c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
-  }
-  if (c->action != c_cmd->action) { /* 如果当前动作和要设置的动作不同 */
-    c->action = c_cmd->action; /* 更新底盘动作 */
-    stage = 0; /* 重置阶段 */
-    c->time = 0.0f; /* 重置时间 */
-    c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
-  }
   return CHASSIS_OK;
 }
 
@@ -97,15 +106,27 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
   c->mode = CHASSIS_MODE_RELAX; /* 设置上电后底盘默认模式 */
   c->height = CHASSIS_DEFAULT_HEIGHT; /* 设置底盘默认高度为0.2米 */
 
-  c->pid.motor_id = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->pid.motor_id));
+  c->pid.motor_id = BSP_Malloc(GO_MOTOR_NUM * sizeof(*c->pid.motor_id));
   if (c->pid.motor_id == NULL) goto error;
 
-  c->filter.in = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->filter.in));
+  c->filter.in = BSP_Malloc(GO_MOTOR_NUM * sizeof(*c->filter.in));
   if (c->filter.in == NULL) goto error;
 
-  c->filter.out = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->filter.out));
+  c->filter.out = BSP_Malloc(GO_MOTOR_NUM * sizeof(*c->filter.out));
   if (c->filter.out == NULL) goto error;
 
+  for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
+    PID_Init(c->pid.motor_id + i, KPID_MODE_NO_D, target_freq,
+             &(c->param->torque_pid_param));
+
+    LowPassFilter2p_Init(c->filter.in + i, target_freq,
+                         c->param->low_pass_cutoff_freq.in);
+    LowPassFilter2p_Init(c->filter.out + i, target_freq,
+                         c->param->low_pass_cutoff_freq.out);
+  }
+  PID_Init(&c->pid.blance, KPID_MODE_NO_D, target_freq,
+           &(c->param->blance_pid_param));
+
 return CHASSIS_OK; /* 返回成功 */
 
 error:
@@ -132,6 +153,15 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const GO_ChassisFeedback_t *go){
     }
 }
 
+int8_t Chassis_UpdateImu(Chassis_t *c, const AHRS_Eulr_t *eulr){
+  if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
+  if (eulr == NULL) return CHASSIS_ERR_NULL; /* IMU数据不能为空 */
+  
+  c->eulr_imu.yaw = eulr->yaw;
+  c->eulr_imu.pit = eulr->pit;
+  c->eulr_imu.rol = eulr->rol;
+}
+
 int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now){
   /* 底盘数据和控制指令结构体不能为空 */
   if (c == NULL) return CHASSIS_ERR_NULL;
@@ -159,10 +189,21 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
       c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
       c->time = 0.0f; /* 重置时间 */
       break;
-    case CHASSIS_MODE_POSITION:{
+    case CHASSIS_MODE_FOLLOW:{
       for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
-              c->output.id[i] = position_mode; /* 设置位置模式 */}
+          c->output.id[i] = position_mode; /* 设置位置模式 */
-
+      }
+      c->delta_eulr.pit = PID_Calc(&c->pid.blance, 0.0f, c->eulr_imu.pit, 0.0f, c->dt);
+      c->eulr_setpoint.pit += c->delta_eulr.pit; /* 更新期望的俯仰角 */
+      //限制c->eulr_setpoint.pit在-0.1f到0.1f之间
+      if (c->eulr_setpoint.pit > 0.05f) {
+        c->eulr_setpoint.pit = 0.05f;
+      }
+      else if (c->eulr_setpoint.pit < -0.05f) {
+        c->eulr_setpoint.pit = -0.05f;
+      }
+      // c->delta_eulr.pit = 0;
+       /* 计算俯仰角变化量 */
       switch (c->action) {
         case CHASSIS_ACTION_NONE:{
           for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
@@ -175,110 +216,107 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
 
         case CHASSIS_ACTION_STAND:{
           c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
+          float pitch_correction = PID_Calc(&c->pid.blance, 0.0f, c->eulr_imu.pit, 0.0f, c->dt);
           for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
-            if (i % 2 == 0) { /* 左前腿和右后腿 */
+            if (c->eulr_setpoint.pit > 0.0f) {
-              float target_pose[3] = {0.0, -0.0861, -c->height};
+              if (i == 0) { /* 左前腿 */
-              float angle_pose[3];
+                float target_pose[3] = {-0.015, -0.0861, -c->height - c->eulr_setpoint.pit};
-              Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                float angle_pose[3];
-              c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
-              c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
-              c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
+              } else if (i == 1) { /* 右前腿 */
+                float target_pose[3] = {-0.015, 0.0861, -c->height - c->eulr_setpoint.pit}; /* 右前腿和左后腿 */
+                float angle_pose[3];
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
+              } else if (i == 2) { /* 左后腿 */
+                float target_pose[3] = {-0.015, -0.0861, -c->height + c->eulr_setpoint.pit}; /* 左前腿和右后腿 */
+                float angle_pose[3];
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
+              } else if (i == 3) { /* 右后腿 */
+                float target_pose[3] = {-0.015, 0.0861, -c->height + c->eulr_setpoint.pit}; /* 右前腿和左后腿 */
+                float angle_pose[3];
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
+              }
             } else {
-              float target_pose[3] = {0.0, 0.0861, -c->height}; /* 右前腿和左后腿 */
+              if (i == 0) { /* 左前腿 */
-              float angle_pose[3];
+                float target_pose[3] = {-0.015, -0.0861, -c->height - c->eulr_setpoint.pit};
-              Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                float angle_pose[3];
-              c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
-              c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
-              c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
+              } else if (i == 1) { /* 右前腿 */
+                float target_pose[3] = {-0.015, 0.0861, -c->height - c->eulr_setpoint.pit}; /* 右前腿和左后腿 */
+                float angle_pose[3];
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
+              } else if (i == 2) { /* 左后腿 */
+                float target_pose[3] = {-0.015, -0.0861, -c->height + c->eulr_setpoint.pit};
+                float angle_pose[3];
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
+              } else if (i == 3) { /* 右后腿 */
+                float target_pose[3] = {-0.015, 0.0861, -c->height + c->eulr_setpoint.pit}; /* 右前腿和左后腿 */
+                float angle_pose[3];
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
+              }
             }
           }
-          break;
+          // break;
-        } /* 站立动作 */
+          if (c_cmd->ctrl_vec.vx == 0.0f && c_cmd->ctrl_vec.vy == 0.0f && c_cmd->ctrl_vec.wz == 0.0f) {
-
-        case CHASSIS_ACTION_WALK:{
-            c->height = c->height + c_cmd->delta_hight * c->dt;
-            float T = 2.0f; // 步态周期(s)
-            float swing_height = 0.15f; // 摆动腿抬高高度
-            float stride_x = c_cmd->ctrl_vec.vx * T; // x方向步幅
-            float stride_y = c_cmd->ctrl_vec.vy * T; // y方向步幅
-            float wz = c_cmd->ctrl_vec.wz; // 旋转速度
-
-            uint8_t swing_leg = (uint8_t)(c->time / (T / 4)) % 4; // 当前摆动腿编号
-            float t_phase = fmodf(c->time, T / 4) / (T / 4); // 当前腿相内归一化时间
-
-            for (uint8_t leg = 0; leg < 4; leg++) {
-                float target_pose[3];
-                float base_y = (leg % 2 == 0) ? -0.0861f : 0.0861f;
-                float base_x = 0.0f;
-
-                if (leg == swing_leg) {
-                    // 摆动腿：贝塞尔轨迹，抬高并前移
-                    float start[3] = {base_x - stride_x/2, base_y - stride_y/2, -c->height};
-                    float mid1[3] = {base_x, base_y, -c->height + swing_height};
-                    float mid2[3] = {base_x, base_y, -c->height + swing_height};
-                    float end[3] = {base_x + stride_x/2, base_y + stride_y/2, -c->height};
-                    Path_Bezier3d(start, mid1, mid2, end, t_phase, target_pose);
-                } else {
-                    // 支撑腿：3/4周期内走完整步幅的1/3
-                    // 计算支撑腿在支撑相内的归一化时间
-                    uint8_t support_leg_index = (leg + 4 - swing_leg) % 4;
-                    float support_phase = fmodf(c->time + (T / 4) * support_leg_index, T) / (T * 3.0f / 4.0f);
-                    if (support_phase > 1.0f) support_phase = 1.0f;
-
-                    // 支撑腿分三段，每段1/3步幅
-                    float seg = support_phase * 3.0f; // [0,3)
-                    int seg_idx = (int)seg; // 0,1,2
-                    float seg_phase = seg - seg_idx; // [0,1)
-
-                    // 每段起止点
-                    float seg_start_x = base_x + stride_x/2 - stride_x * seg_idx / 3.0f;
-                    float seg_start_y = base_y + stride_y/2 - stride_y * seg_idx / 3.0f;
-                    float seg_end_x = base_x + stride_x/2 - stride_x * (seg_idx+1) / 3.0f;
-                    float seg_end_y = base_y + stride_y/2 - stride_y * (seg_idx+1) / 3.0f;
-
-                    Path_straight3d(
-                        (float[3]){seg_start_x, seg_start_y, -c->height},
-                        (float[3]){seg_end_x, seg_end_y, -c->height},
-                        seg_phase, target_pose
-                    );
-                }
-                float angle_pose[3];
-                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[leg]);
-                c->output.id[leg * 3 + 0].Pos = angle_pose[0];
-                c->output.id[leg * 3 + 1].Pos = angle_pose[1];
-                c->output.id[leg * 3 + 2].Pos = angle_pose[2];
-            }
             break;
           }
+        } /* 站立动作 */
 
           
         case CHASSIS_ACTION_TROT:{
+          c->height = c->height + c_cmd->delta_hight * c->dt;
+          
           float T = 0.6f; // 步态周期(s)
           if (c->time > T) {
             c->time -= T; // 保持时间在0-T之间
           }
           float stride_x = c_cmd->ctrl_vec.vx / 10.0f;
           float stride_y = c_cmd->ctrl_vec.vy / 10.0f;
-          float swing_height = 0.15f; // 摆动腿抬高高度
+          float swing_height = 0.12f; // 摆动腿抬高高度
 
           // 对角腿分组：0-3一组，1-2一组
           for (uint8_t leg = 0; leg < 4; leg++) {
             float t_leg = fmodf(c->time + ((leg == 0 || leg == 3) ? 0.0f : T/2), T) / T; // 对角腿相差半周期
             float base_y = (leg % 2 == 0) ? -0.0861f : 0.0861f;
+            float wz = (leg < 2) ? c_cmd->ctrl_vec.wz/5 : -c_cmd->ctrl_vec.wz/5; // 前两条腿正向，后两条腿反向
             float target_pose[3];
             if (t_leg < 0.5f) {
               // 摆动相，贝塞尔插值
               float t_bezier = t_leg / 0.5f;
-              float start[3] = {-stride_x, base_y - stride_y, -c->height};
+              float start[3] = {-stride_x -0.015, base_y - stride_y - wz, -c->height};
-              float mid1[3] = {0, base_y, -c->height + swing_height};
+              float mid1[3] = {-0.015 - stride_x/2, base_y, -c->height + swing_height};
-              float mid2[3] = {0, base_y, -c->height + swing_height};
+              float mid2[3] = {-0.015 + stride_x/2, base_y, -c->height + swing_height};
-              float end[3] = {stride_x, base_y + stride_y, -c->height};
+              float end[3] = {stride_x -0.015, base_y + stride_y + wz, -c->height};
               Path_Bezier3d(start, mid1, mid2, end, t_bezier, target_pose);
             } else {
               // 支撑相，首尾相连，起点为上一个摆动相终点，终点为下一个摆动相起点
               float t_line = (t_leg - 0.5f) / 0.5f;
-              float start[3] = {stride_x, base_y + stride_y, -c->height};    // 摆动相终点
+              float start[3] = {stride_x-0.015, base_y + stride_y + wz, -c->height};    // 摆动相终点
-              float end[3]   = {-stride_x, base_y - stride_y, -c->height};   // 下一个摆动相起点
+              float end[3]   = {-stride_x-0.015, base_y - stride_y - wz, -c->height};   // 下一个摆动相起点
               Path_straight3d(start, end, t_line, target_pose);
             }
             float angle_pose[3];
@@ -292,16 +330,176 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
 
 
       }
+      for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
+      /*PID计算力矩*/
+      c->output.id[i].T = PID_Calc(c->pid.motor_id, c->output.id[i].Pos, c->feedback.id[i].Pos, 0.0f, c->dt);
+      }
+      break;
     }   
   
+    case CHASSIS_MODE_POSITION:{
+      /*设置pd参数为位控参数，获取遥控器控制命令*/
+      for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
+        c->output.id[i] = position_mode;} /* 设置位置模式 */
+      c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
+      c->eulr_setpoint.pit += c_cmd->delta_eulr.pit* c->dt; /* 更新期望的俯仰角 */
+      c->eulr_setpoint.rol = 0.0f; /* 期望的俯仰角和翻滚角 */
+      c->eulr_setpoint.yaw += c_cmd->delta_eulr.yaw * c->dt; /* 期望的偏航角 */
+
+      /*pitch轴软件限位*/
+      if (c->eulr_setpoint.pit > 0.2f) {
+        c->eulr_setpoint.pit = 0.2f; /* 限制俯仰角在-0.05到0.05之间 */
+      } else if (c->eulr_setpoint.pit < -0.2f) {
+        c->eulr_setpoint.pit = -0.2f; /* 限制俯仰角在-0.05到0.05之间 */
+      }
+
+      switch (c->action) 
+      {
+        case CHASSIS_ACTION_NONE:{
+          for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
+            c->output.id[i].Pos = c->feedback.id[i].Pos;}
+          c->mode = CHASSIS_MODE_POSITION;
+          c->action = CHASSIS_ACTION_NONE;
+          c->time = 0.0f; /* 重置时间 */
+          break;
+        }
+        case CHASSIS_ACTION_STAND:{
+          switch (stage) {
+            /* 重置时间记录当前位子 */
+            case 0:{
+              c->time = 0.0f; /* 重置时间 */
+              T = 0.2f; /* 设置站立动作的时间周期 */
+              for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
+              c->leg_pos.start_pos.leg[i].x = c->feedback.id[i * 3].Pos; /* 四条腿基坐标x轴位置 */
+              c->leg_pos.start_pos.leg[i].y = c->feedback.id[i * 3 + 1].Pos;
+              c->leg_pos.start_pos.leg[i].z = c->feedback.id[i * 3 + 2].Pos;
+
+              c->leg_pos.end_pos.leg[i].x = -0.015f; /* 四条腿基坐标x轴位置 */
+              c->leg_pos.end_pos.leg[i].y = (i % 2 == 0) ? -c->param->mech_param.length.hip : c->param->mech_param.length.hip; /* 左前腿和右后腿y轴位置，右前腿和左后腿y轴位置 */
+              c->leg_pos.end_pos.leg[i].z = -c->height; /* 四条腿基坐标z轴位置 */
+              float angle_pose[3];
+              Kinematics_InverseKinematics(&c->leg_pos.end_pos.leg[i].x, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+              c->leg_pos.end_pos.leg[i].x = angle_pose[0]; /* 髋关节 */
+              c->leg_pos.end_pos.leg[i].y = angle_pose[1]; /* 大腿 */
+              c->leg_pos.end_pos.leg[i].z = angle_pose[2]; /* 小腿 */
+
+              /*设置四腿基坐标*/
+              c->foot_base[i].x = -0.015f; /* 四条腿基坐标x轴位置 */
+              c->foot_base[i].y = (i % 2 == 0) ? -c->param->mech_param.length.hip : c->param->mech_param.length.hip; /* 左前腿和右后腿y轴位置，右前腿和左后腿y轴位置 */
+              c->foot_base[i].z = -c->height;
+
+              }
+              stage ++;
+              break;
+            }
+            case 1:{
+              /*在周期内对12个关节完成一次直线插补*/
+              for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
+                float t = c->time / T; /* 计算当前时间在周期中的比例 */
+                c->output.id[i * 3].Pos = Path_Straight1d(c->leg_pos.start_pos.leg[i].x, c->leg_pos.end_pos.leg[i].x, t); /* 髋关节 */
+                c->output.id[i * 3 + 1].Pos = Path_Straight1d(c->leg_pos.start_pos.leg[i].y, c->leg_pos.end_pos.leg[i].y, t); /* 大腿 */
+                c->output.id[i * 3 + 2].Pos = Path_Straight1d(c->leg_pos.start_pos.leg[i].z, c->leg_pos.end_pos.leg[i].z, t); /* 小腿 */
+              }
+              if (c->time >= T) {
+                stage ++;
+              }
+              break;
+            }
+            case 2:{
+              /*计算pit和rol的平衡pid*/
+              float pitch_correction = PID_Calc(&c->pid.blance, c->eulr_setpoint.pit, c->eulr_imu.pit, 0.0f, c->dt);
+              float roll_correction = PID_Calc(&c->pid.blance, c->eulr_setpoint.rol, c->eulr_imu.rol, 0.0f, c->dt);
+              
+              /*计算为了补偿平衡的腿长*/
+              c->foot_base[0].z = -c->height - pitch_correction - roll_correction + c->delta_eulr.pit; /* 左前腿 */
+              c->foot_base[1].z = -c->height - pitch_correction + roll_correction + c->delta_eulr.pit; /* 右前腿 */
+              c->foot_base[2].z = -c->height + pitch_correction - roll_correction - c->delta_eulr.pit; /* 左后腿 */
+              c->foot_base[3].z = -c->height + pitch_correction + roll_correction - c->delta_eulr.pit; /* 右后腿 */
+              float target_pose[3];
+              float angle_pose[3];
+              /*将足端坐标转换为关节角度*/
+              for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
+                target_pose[0] = c->foot_base[i].x; /* 髋关节 */
+                target_pose[1] = c->foot_base[i].y; /* 大腿 */
+                target_pose[2] = c->foot_base[i].z;
+                Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+                c->output.id[i * 3].Pos = angle_pose[0]; /* 髋关节 */
+                c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 大腿 */
+                c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 小腿 */
+              }
+              break;
+            }
+          }
+        if (c_cmd->ctrl_vec.vx == 0.0f && c_cmd->ctrl_vec.vy == 0.0f && c_cmd->ctrl_vec.wz == 0.0f){
+          break;
+        }
+        } /* 站立动作 */
+
+        case CHASSIS_ACTION_TROT:{
+          if( c->action == CHASSIS_ACTION_TROT){
+              float pitch_correction = PID_Calc(&c->pid.blance, c->eulr_setpoint.pit, c->eulr_imu.pit, 0.0f, c->dt);
+              float roll_correction = PID_Calc(&c->pid.blance, c->eulr_setpoint.rol, c->eulr_imu.rol, 0.0f, c->dt);
+              
+              /*计算为了补偿平衡的腿长*/
+              c->foot_base[0].z = -c->height - pitch_correction - roll_correction; /* 左前腿 */
+              c->foot_base[1].z = -c->height - pitch_correction + roll_correction; /* 右前腿 */
+              c->foot_base[2].z = -c->height + pitch_correction - roll_correction; /* 左后腿 */
+              c->foot_base[3].z = -c->height + pitch_correction + roll_correction; /* 右后腿 */
+          }
+          T = 0.6f; /* 设置trot动作的时间周期 */
+          if (c->time > T) {
+            c->time -= T; // 保持时间在0-T之间
+          }
+          float stride_x = c_cmd->ctrl_vec.vx / 10.0f; /* 步幅x轴 */
+          float stride_y = c_cmd->ctrl_vec.vy / 10.0f; /* 步幅y轴 */
+          float swing_height = 0.12f; // 摆动腿默认抬高高度
+
+          // 对角腿分组：0-3一组，1-2一组
+          for (uint8_t leg = 0; leg < 4; leg++) {
+            float t_leg = fmodf(c->time + ((leg == 0 || leg == 3) ? 0.0f : T/2), T) / T; // 对角腿相差半周期
+            float wz = (leg < 2) ? c_cmd->ctrl_vec.wz/5 : -c_cmd->ctrl_vec.wz/5; // 前两条腿正向，后两条腿反向
+            float target_pose[3];
+            if (t_leg < 0.5f) {
+              // 摆动相，贝塞尔插值
+              float t_bezier = t_leg / 0.5f;
+              float start[3] = {c->foot_base[leg].x -stride_x, c->foot_base[leg].y - stride_y - wz, c->foot_base[leg].z};
+              float mid1[3] = {c->foot_base[leg].x - stride_x/2, c->foot_base[leg].y, c->foot_base[leg].z + swing_height};
+              float mid2[3] = {c->foot_base[leg].x + stride_x/2, c->foot_base[leg].y, c->foot_base[leg].z + swing_height};
+              float end[3] = {c->foot_base[leg].x +stride_x , c->foot_base[leg].y + stride_y + wz, c->foot_base[leg].z};
+              Path_Bezier3d(start, mid1, mid2, end, t_bezier, target_pose);
+            } else {
+              // 支撑相，首尾相连，起点为上一个摆动相终点，终点为下一个摆动相起点
+              float t_line = (t_leg - 0.5f) / 0.5f;
+              float start[3] = {stride_x+c->foot_base[leg].x, c->foot_base[leg].y + stride_y + wz, c->foot_base[leg].z};    // 摆动相终点
+              float end[3]   = {-stride_x+c->foot_base[leg].x, c->foot_base[leg].y - stride_y - wz, c->foot_base[leg].z};   // 下一个摆动相起点
+              Path_straight3d(start, end, t_line, target_pose);
+            }
+            float angle_pose[3];
+            Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[leg]);
+            c->output.id[leg * 3 + 0].Pos = angle_pose[0];
+            c->output.id[leg * 3 + 1].Pos = angle_pose[1];
+            c->output.id[leg * 3 + 2].Pos = angle_pose[2];
+          }
+
+          break;
+        } /* trot动作 */
+      }
+      for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
+        /*PID计算力矩*/
+        c->output.id[i].T = PID_Calc(c->pid.motor_id, c->output.id[i].Pos, c->feedback.id[i].Pos, 0.0f, c->dt);
+      }
+    }
   }
 
+  
+
   for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
-    /* 限制输出 */
+
-    Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos,
+    Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos, &c->output.id[i].T,
                         c->param->mech_param.ratio.id[i] * CHASSIS_MAX_SPEED,
                         c->param->mech_param.limit.max.id[i],
-                        c->param->mech_param.limit.min.id[i]);
+                        c->param->mech_param.limit.min.id[i],
+                        c->param->mech_param.ratio.id[i] * CHASSIS_MAX_TORQUE);
   }
 
 }

User/module/chassis.h

@@ -38,24 +38,6 @@ typedef enum {
   IMU_ERROR,
 } Chassis_ImuType_t;
 
-// typedef union {
-//     float id[GO_MOTOR_NUM]; /* 零点角度，单位：弧度 */
-//     struct {
-//         float lf_hip;   /* 左前腿髋关节零点 */
-//         float lf_thigh; /* 左前腿大腿零点 */
-//         float lf_calf;  /* 左前腿小腿零点 */
-//         float rf_hip;   /* 右前腿髋关节零点 */
-//         float rf_thigh; /* 右前腿大腿零点 */  
-//         float rf_calf;  /* 右前腿小腿零点 */
-//         float lr_hip;   /* 左后腿髋关节零点 */
-//         float lr_thigh; /* 左后腿大腿零点 */
-//         float lr_calf;  /* 左后腿小腿零点 */
-//         float rr_hip;   /* 右后腿髋关节零点 */
-//         float rr_thigh; /* 右后腿大腿零点 */
-//         float rr_calf;  /* 右后腿小腿零点 */
-//     } named;
-// } Chassis_MotorZeroPoint_t;
-
 typedef union {
   float id[GO_MOTOR_NUM];
   struct{
@@ -74,9 +56,14 @@ typedef union {
   } named;
 } joint_params;
 
+typedef struct {
+  float x;
+  float y;
+  float z;
+} joint_pos;
 typedef struct {
   joint_params min;
-  joint_params max; /* 关节的最小和最大角度，单位：弧度 */
+  joint_params max;
 } joint_limits;
 
 typedef union {
@@ -97,6 +84,7 @@ typedef struct{
  
   joint_params ratio;      /* 关节减速比 */
   joint_limits limit; /* 关节的最小和最大角度，单位：弧度 */
+  
   Kinematics_LinkLength_t length; /* 关节长度，单位：米 */
   Kinematics_LegOffset_t leg_offset; /* 关节偏移，单位：米 */
   Kinematics_DirectionSign_t sign; /* 关节侧向标志（左前/左后为-1，右前/右后为1） */
@@ -110,7 +98,7 @@ typedef struct {
   Chassis_Mech_Params_t mech_param; /* 机械参数 */
 
   KPID_Params_t torque_pid_param; /* 力矩矩控制PID的参数 */
-
+  KPID_Params_t blance_pid_param; /* 平衡PID的参数 */
   /* 低通滤波器截止频率 */
   struct {
     float in;  /* 输入 */
@@ -133,18 +121,45 @@ typedef struct {
   GO_ChassisFeedback_t feedback; /* 底盘反馈信息 */
   GO_ChassisCMD_t output;
   float height; /* 底盘高度，单位：米 */
+
+  struct {
+    float x;
+    float y;
+    float z;
+  } foot_base[GO_MOTOR_NUM/3]; /* 四个足端的位置，单位：米 */
+
+  struct{
+    struct{
+      joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
+    }start_pos;
+    struct{
+      joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
+    }mid1_pos;
+    struct{
+      joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
+    }mid2_pos;
+    struct{
+      joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
+    }end_pos;  
+  } leg_pos; /* 四条腿的关节位置，单位：米 */
+
   /* 模块通用 */
   CMD_ChassisMode_t mode; /* 底盘模式 */
+  
   CMD_ChassisAction_t action; /* 底盘模式 */
 
-  /* 底盘设计 */
+  AHRS_Eulr_t eulr_imu; /* 欧拉角，单位：弧度 */
-  int8_t num_joint; /* 关节数量 */
+
+  AHRS_Eulr_t delta_eulr; /* 欧拉角变化量，单位：弧度 */
+
+  AHRS_Eulr_t eulr_setpoint; /* 期望的欧拉角，单位：弧度 */
   
   MoveVector_t move_vec; /* 底盘实际的运动向量 */
 
   /* 反馈控制用的PID */
   struct {
     KPID_t *motor_id; /* 控制轮子电机用的PID的动态数组 */
+    KPID_t blance;    /* 平衡PID */
   } pid;
 
   /* 滤波器 */
@@ -178,6 +193,14 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,float target_fre
  */
 int8_t Chassis_UpdateFeedback(Chassis_t *c, const GO_ChassisFeedback_t *go);
 
+/**
+ * @brief 更新底盘的IMU信息
+ * @param c 包含底盘数据的结构体
+ * @param eulr 包含IMU欧拉角的结构体
+ * @return 
+ */
+int8_t Chassis_UpdateImu(Chassis_t *c, const AHRS_Eulr_t *eulr);
+
 /**
  * \brief 运行底盘控制逻辑
  *

User/module/config.c

@@ -30,13 +30,24 @@ Config_t param_default = {
         .type = CHASSIS_TYPE_QUADRUPED,
 
         .torque_pid_param = {
-            .k = 1.0f, /* 控制器增益 */
+            .k = 5.0f, /* 控制器增益 */
-            .p = 1.0f, /* 比例项增益 */
+            .p = 20.0f, /* 比例项增益 */
-            .i = 0.0f, /* 积分项增益 */
+            .i = 1.0f, /* 积分项增益 */
             .d = 0.0f, /* 微分项增益 */
-            .i_limit = 0.0f, /* 积分项上限 */
+            .i_limit = 100.0f, /* 积分项上限 */
-            .out_limit = 0.0f, /* 输出绝对值限制 */
+            .out_limit = 100.0f, /* 输出绝对值限制 */
-            .d_cutoff_freq = 10.0f, /* D项低通截止频率 */
+            .d_cutoff_freq = -1.0f, /* D项低通截止频率 */
+            .range = -1.0f, /* 计算循环误差时使用，大于0时启用 */
+        },
+
+        .blance_pid_param = {
+            .k = 1.00f, /* 控制器增益 */
+            .p = 0.08f, /* 比例项增益 */
+            .i = 0.08f, /* 积分项增益 */
+            .d = 0.0f, /* 微分项增益 */
+            .i_limit = 0.05f, /* 积分项上限 */
+            .out_limit = 0.1f, /* 输出绝对值限制 */
+            .d_cutoff_freq = -1.0f, /* D项低通截止频率 */
             .range = -1.0f, /* 计算循环误差时使用，大于0时启用 */
         },
         
@@ -91,19 +102,19 @@ Config_t param_default = {
                 .named = {
                     .lf_hip = 0.02f,   /* 左前腿髋关节零点角度，单位：弧度 */
                     .lf_thigh = -3.17f, /* 左前腿大腿零点角度，单位：弧度 */
-                    .lf_calf = 2.84f - JOINT_CALF_OFFSET,  /* 左前腿小腿零点角度，单位：弧度 */
+                    .lf_calf = 0.72f - JOINT_CALF_OFFSET,  /* 左前腿小腿零点角度，单位：弧度 */
 
                     .rf_hip = 5.37f,   /* 右前腿髋关节零点角度，单位：弧度 */
                     .rf_thigh = 9.38f, /* 右前腿大腿零点角度，单位：弧度 */
-                    .rf_calf = 4.45f + JOINT_CALF_OFFSET,  /* 右前腿小腿零点角度，单位：弧度 */
+                    .rf_calf = 4.96f + JOINT_CALF_OFFSET,  /* 右前腿小腿零点角度，单位：弧度 */
 
-                    .lr_hip = 4.70f,   /* 左后腿髋关节零点角度，单位：弧度 */
+                    .lr_hip = 4.5f,   /* 左后腿髋关节零点角度，单位：弧度 */
-                    .lr_thigh = -4.11f, /* 左后腿大腿零点角度，单位：弧度 */
+                    .lr_thigh = -3.2f, /* 左后腿大腿零点角度，单位：弧度 */
-                    .lr_calf = 2.56f - JOINT_CALF_OFFSET,  /* 左后腿小腿零点角度，单位：弧度 */
+                    .lr_calf = 1.73f - JOINT_CALF_OFFSET,  /* 左后腿小腿零点角度，单位：弧度 */
 
-                    .rr_hip = 2.85f,   /* 右后腿髋关节零点角度，单位：弧度 */
+                    .rr_hip = 2.7f,   /* 右后腿髋关节零点角度，单位：弧度 */
-                    .rr_thigh = 10.81f, /* 右后腿大腿零点角度，单位：弧度 */
+                    .rr_thigh = 10.58f, /* 右后腿大腿零点角度，单位：弧度 */
-                    .rr_calf = 3.59f + JOINT_CALF_OFFSET,  /* 右后腿小腿零点角度，单位：弧度 */
+                    .rr_calf = 3.58f + JOINT_CALF_OFFSET,  /* 右后腿小腿零点角度，单位：弧度 */
                 }
             },
 

User/task/atti_esti.c

@@ -26,7 +26,12 @@ N100_t n100;
 BMI088_Cali_t bmi088_cali = {
   .gyro_offset = {0.0f, 0.0f, 0.0f},
 }; /* BMI088校准数据 */
-AHRS_Magn_t mage = {0.0f, 0.0f, 0.0f}; /* 磁力计数据，未使用 */
+N100_Cali_t n100_cali = {
+  .offset_x = 0.0f,
+  .offset_y = 0.0f,
+  .offset_z = 0.0f,
+};
+AHRS_Magn_t mage = {-0.01f, 0.025f, 0.0f}; /* 磁力计数据，未使用 */
 AHRS_t gimbal_ahrs;
 AHRS_Eulr_t eulr_to_send;
 
@@ -109,14 +114,15 @@ void Task_atti_esti(void *argument) {
         N100_StartReceiving(&n100);
         if (N100_WaitDmaCplt()) {
           osKernelLock();
-          N100_ParseData(&n100);
+          N100_ParseData(&n100, &n100_cali); /* 解析N100接收到的数据 */
           osKernelUnlock();
         } else {
           N100_HandleOffline(&n100);
         }
         osMessageQueueReset(task_runtime.msgq.body.eulr_imu); /* 重置消息队列 */
         osMessageQueuePut(task_runtime.msgq.body.eulr_imu, &n100.eulr, 0, 0); /* 将欧拉角数据放入消息队列 */
-        
+        osMessageQueueReset(task_runtime.msgq.chassis.eulr_imu); /* 重置消息队列 */
+        osMessageQueuePut(task_runtime.msgq.chassis.eulr_imu, &n100.eulr, 0, 0); /* 将欧拉角数据放入消息队列 */
         break;
       default:
         

User/task/ctrl_chassis.c

@@ -16,6 +16,7 @@
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN*/
 Chassis_t chassis;
+AHRS_Eulr_t chassis_eulr; /* 底盘IMU欧拉角数据 */
 GO_ChassisFeedback_t chassis_feedback; /* 底盘反馈数据 */
 GO_ChassisCMD_t chassis_output; /* 底盘输出数据 */
 CMD_ChassisCmd_t chassis_cmd; /* 底盘控制命令 */
@@ -43,6 +44,9 @@ void Task_ctrl_chassis(void *argument) {
     if(osMessageQueueGet(task_runtime.msgq.chassis.feefback, &chassis_feedback, NULL, 0) == osOK) { /* 从消息队列中获取底盘反馈数据 */
       Chassis_UpdateFeedback(&chassis, &chassis_feedback); /* 更新底盘反馈数据 */
     }
+    if(osMessageQueueGet(task_runtime.msgq.chassis.eulr_imu, &chassis_eulr, NULL, 0) == osOK) { /* 从消息队列中获取IMU欧拉角数据 */
+      Chassis_UpdateImu(&chassis, &chassis_eulr); /* 更新底盘IMU数据 */
+    }
     osMessageQueueGet(task_runtime.msgq.cmd.chassis, &chassis_cmd, NULL, 0);
     
     osKernelLock();

User/task/init.c

@@ -46,6 +46,7 @@ void Task_Init(void *argument) {
   task_runtime.msgq.chassis.feefback = osMessageQueueNew(2u, sizeof(GO_ChassisFeedback_t), NULL);
   task_runtime.msgq.chassis.output = osMessageQueueNew(2u, sizeof(GO_ChassisCMD_t), NULL);
   task_runtime.msgq.body.eulr_imu = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
+  task_runtime.msgq.chassis.eulr_imu = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核

User/task/test.c

@@ -6,8 +6,7 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN*/
-#include "bsp/uart.h"
+#include "bsp/buzzer.h"
-#include "gom_protocol.h"
 /* USER INCLUDE END*/
 
 /* Private typedef ---------------------------------------------------------- */
@@ -15,10 +14,8 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN*/
-MotorCmd_t mcmd = {0};
+// const BSP_Buzzer_Note_t music_notes[] = { BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_F5, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_G5, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_F4, BSP_BUZZER_NOTE_D4, BSP_BUZZER_NOTE_C6, BSP_BUZZER_NOTE_C4, 0, BSP_BUZZER_NOTE_F5, BSP_BUZZER_NOTE_G5, BSP_BUZZER_NOTE_C7, BSP_BUZZER_NOTE_C6, 0, BSP_BUZZER_NOTE_D6, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_F4, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_A6, 0, BSP_BUZZER_NOTE_C5, BSP_BUZZER_NOTE_D6, BSP_BUZZER_NOTE_C6, 0, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_E4, BSP_BUZZER_NOTE_C6, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_E6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_E4, BSP_BUZZER_NOTE_G4, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_F6, BSP_BUZZER_NOTE_C4, 0 };
-MotorData_t data = {0};
+// uint32_t index = 0; /* 音符索引 */
-
-
 /* USER STRUCT END*/
 
 /* Private function --------------------------------------------------------- */
@@ -33,28 +30,20 @@ void Task_test(void *argument) {
   osDelay(TEST_INIT_DELAY); /* 延时一段时间再开启任务 */
 
   /* USER CODE INIT BEGIN*/
-  mcmd.id = 0;
+  BSP_Buzzer_Start(); /* 启动蜂鸣器 */
-  mcmd.mode = 1;
+  BSP_Buzzer_Set_Note(BSP_BUZZER_NOTE_C4, 0.5f); /* 设置蜂鸣器音符为C4，延时0.5秒 */
-  mcmd.K_P = 0;
+  BSP_Buzzer_Set_Note(BSP_BUZZER_NOTE_E4, 0.5f); /* 设置蜂鸣器音符为E4，延时0.5秒 */
-  mcmd.K_W = 0;
+  BSP_Buzzer_Set_Note(BSP_BUZZER_NOTE_G4, 0.5f); /* 设置蜂鸣器音符为G4，延时0.5秒 */
-  mcmd.Pos = 0;
+  BSP_Buzzer_Stop(); /* 停止蜂鸣器 */
-  mcmd.W = 0;
+  // BSP_Buzzer_Set(2000,0.5f);
-  mcmd.T = 0;
   /* USER CODE INIT END*/
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-
+    // BSP_Buzzer_Set_Note(music_notes[index], 0.02f); /* 设置蜂鸣器音符 */
-    // modify_data(&mcmd);
+    // index++;
-
-    // HAL_UART_Transmit(BSP_UART_GetHandle(BSP_UART_LEFT_LEG), (uint8_t *)&mcmd.motor_send_data, sizeof(mcmd.motor_send_data), 1);
-    
-    // HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_LEFT_LEG), (uint8_t *)&data.motor_recv_data, sizeof(data.motor_recv_data), 1);
-    
-    // extract_data(&data);
-
 
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

User/task/user_task.h

@@ -13,7 +13,7 @@ extern "C" {
 /* USER INCLUDE END */
 /* Exported constants ------------------------------------------------------- */
 /* 任务运行频率 */
-#define TEST_FREQ (1000)
+#define TEST_FREQ (50)
 #define MONITOR_FREQ (100)
 #define CMD_FREQ (500)
 #define CTRL_LEG_FREQ (800)
@@ -67,6 +67,7 @@ typedef struct {
         struct {
             osMessageQueueId_t feefback;
             osMessageQueueId_t output;
+            osMessageQueueId_t eulr_imu;
         }chassis; /* 底盘控制消息队列 */
     } msgq;
     /* USER MESSAGE END */

Utils/222.c

@@ -0,0 +1,4 @@
+// 自动生成的音符数组，每0.2秒检测一次主旋律
+const BSP_Buzzer_Note_t music_notes[] = { BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_F5, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_G5, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_F4, BSP_BUZZER_NOTE_D4, BSP_BUZZER_NOTE_C6, BSP_BUZZER_NOTE_C4, 0, BSP_BUZZER_NOTE_F5, BSP_BUZZER_NOTE_G5, BSP_BUZZER_NOTE_C7, BSP_BUZZER_NOTE_C6, 0, BSP_BUZZER_NOTE_D6, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_F4, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_A6, 0, BSP_BUZZER_NOTE_C5, BSP_BUZZER_NOTE_D6, BSP_BUZZER_NOTE_C6, 0, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_E4, BSP_BUZZER_NOTE_C6, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_E6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_E4, BSP_BUZZER_NOTE_G4, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_F6, BSP_BUZZER_NOTE_C4, 0 };
+const float music_notes_durations[] = { 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.80f, 0.20f, 0.20f, 0.20f, 0.60f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.40f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.40f, 0.80f };
+const unsigned int music_notes_len = 43;

Utils/Kinematics.py

@@ -1,78 +1,177 @@
-# test_kinematics.py
-
 import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
 
-def forward_kinematics(theta, link, side_sign):
+# 定义机械参数
-    l1 = link['hip'] * side_sign
+leg_params = {
-    l2 = -link['thigh']
+    'hip': 0.1,    # 髋关节长度
-    l3 = -link['calf']
+    'thigh': 0.3,   # 大腿长度
+    'calf': 0.3     # 小腿长度
+}
+
+# 定义关节方向符号 (与C代码中一致)
+signs = {
+    'left_front': {'hip': 1, 'thigh': 1, 'calf': -1},
+    'right_front': {'hip': -1, 'thigh': -1, 'calf': 1},
+    'left_rear': {'hip': -1, 'thigh': 1, 'calf': -1},
+    'right_rear': {'hip': 1, 'thigh': -1, 'calf': 1}
+}
+
+def forward_kinematics(theta, leg_params, sign):
+    """正运动学"""
     q1, q2, q3 = theta
+    q2 = q2 * sign['thigh']
+    q3 = q3 * sign['calf']
     
-    s1, s2, s3 = np.sin([q1, q2, q3])
+    l1 = leg_params['hip'] * sign['hip']
-    c1, c2, c3 = np.cos([q1, q2, q3])
+    l2 = leg_params['thigh']
+    l3 = leg_params['calf']
     
-    c23 = c2 * c3 - s2 * s3
+    # 计算足端位置
-    s23 = s2 * c3 + c2 * s3
+    x = l2 * np.cos(q2) + l3 * np.cos(q2 + q3)
+    y = l1 * np.cos(q1) + np.sin(q1) * (l2 * np.sin(q2) + l3 * np.sin(q2 + q3))
+    z = l1 * np.sin(q1) - np.cos(q1) * (l2 * np.sin(q2) + l3 * np.sin(q2 + q3))
     
-    x = l3 * s23 + l2 * s2
+    foot_pos = np.array([x, -y if sign['hip'] == sign['thigh'] else y, z])
-    y = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1
+    return foot_pos
-    z =  l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2
-    return np.array([x, y, z])
 
-def q1_ik(py, pz, l1):
+def inverse_kinematics(foot_pos, leg_params, sign):
-    L = np.sqrt(py*py + pz*pz - l1*l1)
+    """逆运动学"""
-    return np.arctan2(pz*l1 + py*L, py*l1 - pz*L)
+    px = foot_pos[0]
+    py = -foot_pos[1] if sign['hip'] == sign['thigh'] else foot_pos[1]
+    pz = foot_pos[2]
     
-def q3_ik(b3z, b4z, b):
+    b2y = leg_params['hip'] * sign['hip']
-    temp = (b3z**2 + b4z**2 - b**2) / (2.0 * np.abs(b3z * b4z))
+    b3z = -leg_params['thigh']
-    temp = np.clip(temp, -1.0, 1.0)
+    b4z = -leg_params['calf']
-    q3 = np.arccos(temp)
+    a = leg_params['hip']
-    return -(np.pi - q3)
+    c = np.sqrt(px**2 + py**2 + pz**2)
+    b = np.sqrt(c**2 - a**2)
     
-def q2_ik(q1, q3, px, py, pz, b3z, b4z):
+    # 检查可达性
-    a1 = py * np.sin(q1) - pz * np.cos(q1)
-    a2 = px
-    m1 = b4z * np.sin(q3)
-    m2 = b3z + b4z * np.cos(q3)
-    return np.arctan2(m1 * a1 + m2 * a2, m1 * a2 - m2 * a1)
-
-def inverse_kinematics(foot, link, side_sign):
-    px, py, pz = foot
-    b2y = link['hip'] * side_sign
-    b3z = -link['thigh']
-    b4z = -link['calf']
-    a = link['hip']
-    c = np.sqrt(px*px + py*py + pz*pz)
-    b = np.sqrt(c*c - a*a)
     if np.isnan(b) or b > abs(b3z) + abs(b4z):
         return None
+    
+    def q1_ik(py, pz, l1):
+        L = np.sqrt(py**2 + pz**2 - l1**2)
+        return np.arctan2(pz*l1 + py*L, py*l1 - pz*L)
+    
+    def q3_ik(b3z, b4z, b):
+        temp = (b3z**2 + b4z**2 - b**2) / (2.0 * abs(b3z * b4z))
+        temp = np.clip(temp, -1.0, 1.0)
+        q3 = np.arccos(temp)
+        return -(np.pi - q3)
+    
+    def q2_ik(q1, q3, px, py, pz, b3z, b4z):
+        a1 = py * np.sin(q1) - pz * np.cos(q1)
+        a2 = px
+        m1 = b4z * np.sin(q3)
+        m2 = b3z + b4z * np.cos(q3)
+        return np.arctan2(m1*a1 + m2*a2, m1*a2 - m2*a1)
+    
     q1 = q1_ik(py, pz, b2y)
     q3 = q3_ik(b3z, b4z, b)
     q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z)
-    return np.array([q1, q2, q3])
     
-# ...existing code...
+    return np.array([q1, q2 * sign['thigh'], q3 * sign['calf']])
 
-def test():
+def test_kinematics(leg_sign, test_name):
-    link = {'hip': 0.05, 'thigh': 0.2, 'calf': 0.2}
+    print(f"\nTesting {test_name} leg...")
-    side_sign = 1  # 左前腿
     
-    # 输入一个目标点
+    # 生成随机关节角度
-    foot_target = np.array([0.0, 0.06, -0.2])
+    np.random.seed(42)
-    print("目标足端位置:", foot_target)
+    test_angles = np.random.uniform(-np.pi/2, np.pi/2, (10, 3))
     
-    # 逆运动学求解关节角度
+    errors = []
-    theta = inverse_kinematics(foot_target, link, side_sign)
-    if theta is None:
-        print("目标点不可达！")
-        return
-    print("逆运动学解（关节角度，单位：弧度）:", theta)
-    print("逆运动学解（关节角度，单位：度）:", np.degrees(theta))
     
-    # 正运动学验算
+    for angles in test_angles:
-    foot_check = forward_kinematics(theta, link, side_sign)
+        # 正运动学计算足端位置
-    print("正运动学验算足端位置:", foot_check)
+        foot_pos = forward_kinematics(angles, leg_params, leg_sign)
-    print("位置误差:", np.abs(foot_target - foot_check))
         
-if __name__ == "__main__":
+        # 逆运动学计算关节角度
-    test()
+        calculated_angles = inverse_kinematics(foot_pos, leg_params, leg_sign)
+        
+        if calculated_angles is None:
+            print(f"Unreachable position for angles: {angles}")
+            continue
+        
+        # 计算误差
+        error = np.max(np.abs(angles - calculated_angles))
+        errors.append(error)
+        
+        print(f"Original angles: {angles}")
+        print(f"Calculated angles: {calculated_angles}")
+        print(f"Error: {error:.6f} rad\n")
+    
+    avg_error = np.mean(errors)
+    max_error = np.max(errors)
+    print(f"Average error: {avg_error:.6f} rad")
+    print(f"Maximum error: {max_error:.6f} rad")
+    
+    return avg_error, max_error
+
+# 测试所有四种腿型
+results = {}
+for leg_name, leg_sign in signs.items():
+    avg_err, max_err = test_kinematics(leg_sign, leg_name)
+    results[leg_name] = (avg_err, max_err)
+
+# 打印汇总结果
+print("\nSummary of results:")
+for leg_name, (avg_err, max_err) in results.items():
+    print(f"{leg_name}: avg error={avg_err:.6f} rad, max error={max_err:.6f} rad")
+
+# 可视化一个例子
+fig = plt.figure(figsize=(10, 8))
+ax = fig.add_subplot(111, projection='3d')
+
+# 选择左前腿作为示例
+leg_sign = signs['left_front']
+angles = np.array([0.5, -0.3, 0.7])  # 髋关节、大腿、小腿角度
+
+# 计算正运动学
+foot_pos = forward_kinematics(angles, leg_params, leg_sign)
+
+# 绘制
+l1 = leg_params['hip'] * leg_sign['hip']
+l2 = leg_params['thigh']
+l3 = leg_params['calf']
+
+q1, q2, q3 = angles
+q2 = q2 * leg_sign['thigh']
+q3 = q3 * leg_sign['calf']
+
+# 计算各关节位置
+hip_pos = np.array([0, 0, 0])
+knee_pos = np.array([
+    l2 * np.cos(q2),
+    l1 * np.cos(q1) + np.sin(q1) * l2 * np.sin(q2),
+    l1 * np.sin(q1) - np.cos(q1) * l2 * np.sin(q2)
+])
+foot_pos = np.array([
+    l2 * np.cos(q2) + l3 * np.cos(q2 + q3),
+    l1 * np.cos(q1) + np.sin(q1) * (l2 * np.sin(q2) + l3 * np.sin(q2 + q3)),
+    l1 * np.sin(q1) - np.cos(q1) * (l2 * np.sin(q2) + l3 * np.sin(q2 + q3))
+])
+
+# 调整y坐标符号
+if leg_sign['hip'] != leg_sign['thigh']:
+    knee_pos[1] = -knee_pos[1]
+    foot_pos[1] = -foot_pos[1]
+
+# 绘制连杆
+ax.plot([hip_pos[0], knee_pos[0]], [hip_pos[1], knee_pos[1]], [hip_pos[2], knee_pos[2]], 'b-', linewidth=3)
+ax.plot([knee_pos[0], foot_pos[0]], [knee_pos[1], foot_pos[1]], [knee_pos[2], foot_pos[2]], 'r-', linewidth=3)
+
+# 绘制关节
+ax.scatter(hip_pos[0], hip_pos[1], hip_pos[2], c='k', s=100, label='Hip')
+ax.scatter(knee_pos[0], knee_pos[1], knee_pos[2], c='k', s=100, label='Knee')
+ax.scatter(foot_pos[0], foot_pos[1], foot_pos[2], c='g', s=100, label='Foot')
+
+ax.set_xlabel('X (m)')
+ax.set_ylabel('Y (m)')
+ax.set_zlabel('Z (m)')
+ax.set_title('Leg Kinematics Example (Left Front Leg)')
+ax.legend()
+plt.tight_layout()
+plt.show()

Utils/music.py

@@ -0,0 +1,65 @@
+from pydub import AudioSegment
+import numpy as np
+import os
+import librosa
+
+# 音符频率表（与buzzer.h一致）
+NOTE_FREQS = [
+    261, 294, 329, 349, 392, 440, 494, 523, 587, 659, 698, 784, 880, 988, 1047, 1175, 1319, 1397, 1568, 1760, 1976, 2093, 2349, 2637, 2794, 3136, 3520, 3951, 4186
+]
+NOTE_NAMES = [
+    "BSP_BUZZER_NOTE_C4", "BSP_BUZZER_NOTE_D4", "BSP_BUZZER_NOTE_E4", "BSP_BUZZER_NOTE_F4", "BSP_BUZZER_NOTE_G4", "BSP_BUZZER_NOTE_A4", "BSP_BUZZER_NOTE_B4",
+    "BSP_BUZZER_NOTE_C5", "BSP_BUZZER_NOTE_D5", "BSP_BUZZER_NOTE_E5", "BSP_BUZZER_NOTE_F5", "BSP_BUZZER_NOTE_G5", "BSP_BUZZER_NOTE_A5", "BSP_BUZZER_NOTE_B5",
+    "BSP_BUZZER_NOTE_C6", "BSP_BUZZER_NOTE_D6", "BSP_BUZZER_NOTE_E6", "BSP_BUZZER_NOTE_F6", "BSP_BUZZER_NOTE_G6", "BSP_BUZZER_NOTE_A6", "BSP_BUZZER_NOTE_B6",
+    "BSP_BUZZER_NOTE_C7", "BSP_BUZZER_NOTE_D7", "BSP_BUZZER_NOTE_E7", "BSP_BUZZER_NOTE_F7", "BSP_BUZZER_NOTE_G7", "BSP_BUZZER_NOTE_A7", "BSP_BUZZER_NOTE_B7",
+    "BSP_BUZZER_NOTE_C8"
+]
+
+def freq_to_note_name(freq):
+    # 找到最接近的音符
+    idx = np.argmin(np.abs(np.array(NOTE_FREQS) - freq))
+    return NOTE_NAMES[idx], NOTE_FREQS[idx]
+
+def mp3_to_note_array(mp3_path, c_array_name="music_notes", out_path=None, interval_sec=0.02):
+    y, sr = librosa.load(mp3_path, sr=None, mono=True)
+    hop_length = int(sr * interval_sec)
+    pitches, magnitudes = librosa.piptrack(y=y, sr=sr, hop_length=hop_length)
+    notes = []
+    durations = []
+    last_note = None
+    for t in range(pitches.shape[1]):
+        index = magnitudes[:, t].argmax()
+        freq = pitches[index, t]
+        if freq > 0:
+            note_name, note_freq = freq_to_note_name(freq)
+        else:
+            note_name = "0"
+        if note_name == last_note and notes:
+            durations[-1] += interval_sec
+        else:
+            notes.append(note_name)
+            durations.append(interval_sec)
+            last_note = note_name
+
+    # 生成C数组字符串
+    notes_str = ", ".join(notes)
+    durations_str = ", ".join([f"{d:.2f}f" for d in durations])
+    c_code = f"// 自动生成的音符数组，每{interval_sec}秒检测一次主旋律\n"
+    c_code += f"const BSP_Buzzer_Note_t {c_array_name}[] = {{ {notes_str} }};\n"
+    c_code += f"const float {c_array_name}_durations[] = {{ {durations_str} }};\n"
+    c_code += f"const unsigned int {c_array_name}_len = {len(notes)};\n"
+
+    # 自动生成输出路径
+    if out_path is None:
+        base, _ = os.path.splitext(mp3_path)
+        out_path = base + ".c"
+
+    # 写入C文件
+    with open(out_path, "w") as f:
+        f.write(c_code)
+    print(f"音符数组已写入: {out_path}")
+
+if __name__ == "__main__":
+    mp3_path = "/Users/lvzucheng/Documents/R/CM_DOG/Utils/222.mp3"  # 你的mp3文件路径
+    c_array_name = "music_notes"      # C数组名
+    mp3_to_note_array(mp3_path, c_array_name, interval_sec=0.2)