.vscode/settings.json

@@ -4,10 +4,6 @@
         "ahrs.h": "c",
         "string.h": "c",
         "user_math.h": "c",
-        "__locale": "c",
+        "__locale": "c"
-        "kinematics.h": "c",
-        "bezier_curve.h": "c",
-        "queue": "c",
-        "stack": "c"
     }
 }

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -183,8 +183,23 @@
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
+          <ItemText>angle_pose</ItemText>
+        </Ww>
+        <Ww>
+          <count>6</count>
+          <WinNumber>1</WinNumber>
           <ItemText>chassis_cmd</ItemText>
         </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>target_pose</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>c-&gt;param-&gt;mech_param.sign.leg[i]</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -1283,32 +1298,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\component\kinematics.c</PathWithFileName>
+      <PathWithFileName>..\User\component\bezier_curve.c</PathWithFileName>
-      <FilenameWithoutPath>kinematics.c</FilenameWithoutPath>
+      <FilenameWithoutPath>bezier_curve.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>10</GroupNumber>
-      <FileNumber>82</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\User\component\path.c</PathWithFileName>
-      <FilenameWithoutPath>path.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>10</GroupNumber>
-      <FileNumber>83</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\User\bsp\mm.c</PathWithFileName>
-      <FilenameWithoutPath>mm.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1322,7 +1313,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>82</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1334,7 +1325,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>83</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1346,7 +1337,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>84</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1358,7 +1349,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>85</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1378,7 +1369,19 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>12</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>86</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\component\kinematics.c</PathWithFileName>
+      <FilenameWithoutPath>kinematics.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>12</GroupNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1390,7 +1393,7 @@
     </File>
     <File>
       <GroupNumber>12</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1410,7 +1413,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1422,7 +1425,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1434,7 +1437,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1446,7 +1449,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1458,7 +1461,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1470,7 +1473,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>95</FileNumber>
+      <FileNumber>94</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1482,7 +1485,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>96</FileNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1494,7 +1497,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>97</FileNumber>
+      <FileNumber>96</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1506,7 +1509,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>98</FileNumber>
+      <FileNumber>97</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1518,7 +1521,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>99</FileNumber>
+      <FileNumber>98</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1530,7 +1533,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>100</FileNumber>
+      <FileNumber>99</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1550,7 +1553,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>14</GroupNumber>
-      <FileNumber>101</FileNumber>
+      <FileNumber>100</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1562,7 +1565,7 @@
     </File>
     <File>
       <GroupNumber>14</GroupNumber>
-      <FileNumber>102</FileNumber>
+      <FileNumber>101</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

MDK-ARM/CtrBoard-H7_ALL.uvprojx

@@ -1459,19 +1459,9 @@
               <FilePath>..\User\component\limiter.c</FilePath>
             </File>
             <File>
-              <FileName>kinematics.c</FileName>
+              <FileName>bezier_curve.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\User\component\kinematics.c</FilePath>
+              <FilePath>..\User\component\bezier_curve.c</FilePath>
-            </File>
-            <File>
-              <FileName>path.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\component\path.c</FilePath>
-            </File>
-            <File>
-              <FileName>mm.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\bsp\mm.c</FilePath>
             </File>
           </Files>
         </Group>
@@ -1503,6 +1493,11 @@
         <Group>
           <GroupName>User/module</GroupName>
           <Files>
+            <File>
+              <FileName>kinematics.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\kinematics.c</FilePath>
+            </File>
             <File>
               <FileName>chassis.c</FileName>
               <FileType>1</FileType>

User/bsp/mm.c

@@ -1,14 +0,0 @@
-/* Includes ----------------------------------------------------------------- */
-#include "bsp/mm.h"
-
-#include "FreeRTOS.h"
-
-/* Private define ----------------------------------------------------------- */
-/* Private macro ------------------------------------------------------------ */
-/* Private typedef ---------------------------------------------------------- */
-/* Private variables -------------------------------------------------------- */
-/* Private function  -------------------------------------------------------- */
-/* Exported functions ------------------------------------------------------- */
-inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); }
-
-inline void BSP_Free(void *pv) { vPortFree(pv); }

User/bsp/mm.h

@@ -1,20 +0,0 @@
-#pragma once
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Includes ----------------------------------------------------------------- */
-#include <stddef.h>
-#include <stdint.h>
-
-/* Exported constants ------------------------------------------------------- */
-/* Exported macro ----------------------------------------------------------- */
-/* Exported types ----------------------------------------------------------- */
-/* Exported functions prototypes -------------------------------------------- */
-void *BSP_Malloc(size_t size);
-void BSP_Free(void *pv);
-
-#ifdef __cplusplus
-}
-#endif

User/component/bezier_curve.c

@@ -0,0 +1,24 @@
+#include "bezier_curve.h"
+
+// 三次贝塞尔曲线，二维
+void bezier_curve_3rd(const float p0[2], const float p1[2], const float p2[2], const float p3[2], float t, float out[2]) {
+    float u = 1.0f - t;
+    float tt = t * t;
+    float uu = u * u;
+    float uuu = uu * u;
+    float ttt = tt * t;
+    out[0] = uuu * p0[0] + 3 * uu * t * p1[0] + 3 * u * tt * p2[0] + ttt * p3[0];
+    out[1] = uuu * p0[1] + 3 * uu * t * p1[1] + 3 * u * tt * p2[1] + ttt * p3[1];
+}
+
+// 三次贝塞尔曲线，三维
+void bezier_curve_3rd_3d(const float p0[3], const float p1[3], const float p2[3], const float p3[3], float t, float out[3]) {
+    float u = 1.0f - t;
+    float tt = t * t;
+    float uu = u * u;
+    float uuu = uu * u;
+    float ttt = tt * t;
+    for (int i = 0; i < 3; ++i) {
+        out[i] = uuu * p0[i] + 3 * uu * t * p1[i] + 3 * u * tt * p2[i] + ttt * p3[i];
+    }
+}

User/component/bezier_curve.h

@@ -0,0 +1,20 @@
+/*
+  各类滤波器。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// 三次贝塞尔曲线，二维
+void bezier_curve_3rd(const float p0[2], const float p1[2], const float p2[2], const float p3[2], float t, float out[2]);
+
+// 三次贝塞尔曲线，三维
+void bezier_curve_3rd_3d(const float p0[3], const float p1[3], const float p2[3], const float p3[3], float t, float out[3]);
+
+
+#ifdef __cplusplus
+}
+#endif

User/component/cmd.c

@@ -20,80 +20,38 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
       /* 左拨杆相应行为选择和解析 */
     case CMD_SW_UP:
       cmd->chassis.mode = CHASSIS_MODE_RELAX;
-      switch (rc->sw_r) {
-      /* 右拨杆相应行为选择和解析*/ 
-      case CMD_SW_UP:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      case CMD_SW_MID:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      case CMD_SW_DOWN:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      case CMD_SW_ERR:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      }
       break;
 
     case CMD_SW_MID:
       cmd->chassis.mode = CHASSIS_MODE_DAMP;
-      switch (rc->sw_r) {
-      /* 右拨杆相应行为选择和解析*/ 
-      case CMD_SW_UP:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      case CMD_SW_MID:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      case CMD_SW_DOWN:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      case CMD_SW_ERR:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      }
       break;
 
     case CMD_SW_DOWN:
-      cmd->chassis.mode = CHASSIS_MODE_POSITION;
+      cmd->chassis.mode = CHASSIS_MODE_RECOVER;
-      switch (rc->sw_r) {
-      /* 右拨杆相应行为选择和解析*/ 
-      case CMD_SW_UP:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      case CMD_SW_MID:
-        cmd->chassis.action = CHASSIS_ACTION_STAND;
-        break;
-      case CMD_SW_DOWN:
-        cmd->chassis.action = CHASSIS_ACTION_TROT;
-        break;
-      case CMD_SW_ERR:
-        cmd->chassis.action = CHASSIS_ACTION_NONE;
-        break;
-      }
       break;
 
     case CMD_SW_ERR:
       cmd->chassis.mode = CHASSIS_MODE_RELAX;
+      break;
+  }
   switch (rc->sw_r) {
       /* 右拨杆相应行为选择和解析*/ 
     case CMD_SW_UP:
       cmd->chassis.action = CHASSIS_ACTION_NONE;
       break;
+
     case CMD_SW_MID:
       cmd->chassis.action = CHASSIS_ACTION_NONE;
       break;
+
     case CMD_SW_DOWN:
       cmd->chassis.action = CHASSIS_ACTION_NONE;
       break;
+
     case CMD_SW_ERR:
       cmd->chassis.action = CHASSIS_ACTION_NONE;
       break;
   }
-      break;
-  }
   /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
   cmd->chassis.ctrl_vec.vx = rc->ch_l_y;
   cmd->chassis.ctrl_vec.vy = rc->ch_l_x;

User/component/cmd.h

@@ -18,7 +18,6 @@ typedef enum {
   CHASSIS_MODE_DAMP, /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */
   CHASSIS_MODE_RECOVER, /* 恢复模式，电机闭环控制恢复到初始位置。用于机器人上电后恢复到初始位置 */
   CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
-  CHASSIS_MODE_POSITION, /* 位置模式，电机闭环控制保持在指定位置 */
   CHASSIS_MODE_FOLLOW, /* 通过闭环控制使车头方向跟随遥控器 */
 } CMD_ChassisMode_t;
 
@@ -26,9 +25,6 @@ typedef enum {
   CHASSIS_ACTION_NONE, /* 无动作 */
   CHASSIS_ACTION_STAND, /* 站立动作 */
   CHASSIS_ACTION_WALK, /* 行走动作 */
-  CHASSIS_ACTION_TROT, /* 小跑动作 */
-  CHASSIS_ACTION_JUMP, /* 跳跃动作 */
-  CHASSIS_ACTION_RUN,
   CHASSIS_ACTION_NUM,
 }CMD_ChassisAction_t;
 

User/component/kinematics.c

@@ -83,11 +83,7 @@ void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_Link
  */
 int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_LinkLength_t *leg_params, float theta_out[3], Kinematics_Sign_t *sign) {
     float px = foot_in[0];
-    float py = -foot_in[1];
+    float py = foot_in[1];
-    if (sign->hip != sign->thigh){
-        py = foot_in[1];
-    }
-    
     float pz = foot_in[2];
 
     float b2y = leg_params->hip * sign->hip;

User/component/limiter.h

@@ -11,11 +11,10 @@ extern "C" {
 #include <stdbool.h>
 #include <stdint.h>
 
+
 int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, 
   float max_speed, float max_angle, float min_angle);
 
-
-
 #ifdef __cplusplus
 }
 #endif

User/component/path.c

@@ -1,62 +0,0 @@
-/*
-  一些路径生成器
-*/
-
-#include "path.h"
-
-/**
- * @brief 生成一条直线段的路径点
- * @param start 起始点
- * @param end 结束点
- * @param t 时间路径点，范围[0, 1]
- * @param out 输出路径点
- */
-void Path_Straight2d(float start[2], float end[2], float t, float out[2]){
-    out[0] = start[0] + (end[0] - start[0]) * t; // x坐标插值
-    out[1] = start[1] + (end[1] - start[1]) * t; // y坐标插值
-}
-
-/**
- * @brief 生成一条三维直线段的路径点
- * @param start 起始点
- * @param end 结束点
- * @param t 时间路径点，范围[0, 1]
- * @param out 输出路径点
- */
-void Path_straight3d(float start[3], float end[3], float t, float out[3]){
-    out[0] = start[0] + (end[0] - start[0]) * t; // x坐标插值
-    out[1] = start[1] + (end[1] - start[1]) * t; // y坐标插值
-    out[2] = start[2] + (end[2] - start[2]) * t; // z坐标插值
-}
-
-/**
- * @brief 生成一条二次贝塞尔曲线的路径点
- * @param start 起始点
- * @param mid 中间控制点
- * @param end 结束点
- * @param t 时间路径点，范围[0, 1]
- * @param out 输出路径点
- */
-void Path_Bezier2d(float start[2], float mid[2], float end[2], float t, float out[2]){
-    float t1 = 1 - t; // 计算1-t
-    out[0] = t1 * t1 * start[0] + 2 * t1 * t * mid[0] + t * t * end[0]; // x坐标插值
-    out[1] = t1 * t1 * start[1] + 2 * t1 * t * mid[1] + t * t * end[1]; // y坐标插值
-}
-
-/**
- * @brief 生成一条三次贝塞尔曲线的路径点
- * @param start 起始点
- * @param mid1 中间控制点1
- * @param mid2 中间控制点2
- * @param end 结束点
- * @param t 时间路径点，范围[0, 1]
- * @param out 输出路径点
- */
-void Path_Bezier3d(float start[3], float mid1[3], float mid2[3], float end[3], float t, float out[3]){
-    float t1 = 1 - t; // 计算1-t
-    out[0] = t1 * t1 * t1 * start[0] + 3 * t1 * t1 * t * mid1[0] + 3 * t1 * t * t * mid2[0] + t * t * t * end[0]; // x坐标插值
-    out[1] = t1 * t1 * t1 * start[1]
-                + 3 * t1 * t1 * t * mid1[1] + 3 * t1 * t * t * mid2[1] + t * t * t * end[1]; // y坐标插值
-    out[2] = t1 * t1 * t1 * start[2]
-                + 3 * t1 * t1 * t * mid1[2] + 3 * t1 * t * t * mid2[2] + t * t * t * end[2]; // z坐标插值
-}

User/component/path.h

@@ -1,58 +0,0 @@
-/*
-  一些路径生成器
-*/
-
-#pragma once
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdbool.h>
-#include <stdint.h>
-
-/**
- * @brief 生成一条直线段的路径点
- * @param start 起始点
- * @param end 结束点
- * @param t 时间路径点，范围[0, 1]
- * @param out 输出路径点
- */
-void Path_Straight2d(float start[2], float end[2], float t, float out[2]);
-
-/**
- * @brief 生成一条三维直线段的路径点
- * @param start 起始点
- * @param end 结束点
- * @param t 时间路径点，范围[0, 1]
- * @param out 输出路径点
- */
-void Path_straight3d(float start[3], float end[3], float t, float out[3]);
-
-/**
- * @brief 生成一条二次贝塞尔曲线的路径点
- * @param start 起始点
- * @param mid 中间控制点
- * @param end 结束点
- * @param t 时间路径点，范围[0, 1]
- * @param out 输出路径点
- */
-void Path_Bezier2d(float start[2], float mid[2], float end[2], float t, float out[2]);
-
-/**
- * @brief 生成一条三次贝塞尔曲线的路径点
- * @param start 起始点
- * @param mid1 中间控制点1
- * @param mid2 中间控制点2
- * @param end 结束点
- * @param t 时间路径点，范围[0, 1]
- * @param out 输出路径点
- */
-void Path_Bezier3d(float start[3], float mid1[3], float mid2[3], float end[3], float t, float out[3]);
-
-
-
-
-#ifdef __cplusplus
-}
-#endif

User/module/chassis.c

@@ -8,16 +8,15 @@
 #include <stdlib.h>
 
 #include "cmsis_os2.h"
+// #include "component/limiter.h"
 #include "device/go.h"
 #include "component/limiter.h"
-#include "component/kinematics.h"
-#include "component/path.h"
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
 
+/*底盘默认高度*/
 #define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */
-
+#define CHASSIS_MAX_SPEED (0.03f)
-#define CHASSIS_MAX_SPEED (0.03f) /*调试用速度限幅*/
 /* Private macro ------------------------------------------------------------ */
 
 
@@ -43,40 +42,40 @@ static Kinematics_JointCMD_t position_mode = {
     .T = 0.0f, /* 零力矩 */
     .W = 0.0f, /* 零速度 */
     .Pos = 0.0f, /* 零位置 */
-    .K_P = 3.0f, /* 刚度系数 */
+    .K_P = 1.0f, /* 刚度系数 */
-    .K_W = 0.2f, /* 速度系数 */
+    .K_W = 0.1f, /* 速度系数 */
 };
 
-// static uint8_t chassis_mode_states = 0;
+static uint8_t chassis_mode_states = 0;
-// static uint8_t chassis_action_states = 0;
+static uint8_t chassis_action_states = 0;
-static uint8_t stage = 0;
 /* Private function  -------------------------------------------------------- */
 /**
- * \brief 设置底盘运行模式
+ * \brief 设置底盘模式
  *
  * \param c 包含底盘数据的结构体
  * \param mode 要设置的模式
  *
  * \return 函数运行结果
  */
-static int8_t Chassis_SetMode(Chassis_t *c, CMD_ChassisCmd_t *c_cmd) {
+// static int8_t Chassis_SetMode(Chassis_t *c, CMD_ChassisMode_t mode,
-  if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
+//                               uint32_t now) {
-  if (c_cmd == NULL) return CHASSIS_ERR_NULL; /* 控制指令不能为空 */
+//   if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
+//   if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
 
-  if (c->mode != c_cmd->mode) { /* 如果当前模式和要设置的模式不同 */
+//   if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
-    c->mode = c_cmd->mode; /* 更新底盘模式 */
+//     srand(now);
-    stage = 0; /* 重置阶段 */
+//     c->wz_multi = (rand() % 2) ? -1 : 1;
-    c->time = 0.0f; /* 重置时间 */
+//   }
-    c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
+//   /* 切换模式后重置PID和滤波器 */
-  }
+//   for (uint8_t i = 0; i < c->num_wheel; i++) {
-  if (c->action != c_cmd->action) { /* 如果当前动作和要设置的动作不同 */
+//     PID_Reset(c->pid.motor + i);
-    c->action = c_cmd->action; /* 更新底盘动作 */
+//     LowPassFilter2p_Reset(c->filter.in + i, 0.0f);
-    stage = 0; /* 重置阶段 */
+//     LowPassFilter2p_Reset(c->filter.out + i, 0.0f);
-    c->time = 0.0f; /* 重置时间 */
+//   }
-    c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
+//   c->mode = mode;
-  }
+
-  return CHASSIS_OK;
+//   return CHASSIS_OK;
-}
+// }
 
 /* Exported functions ------------------------------------------------------- */
 
@@ -95,25 +94,10 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
 
   c->param = 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));
-  if (c->pid.motor_id == NULL) goto error;
-
-  c->filter.in = BSP_Malloc(GO_MOTOR_MODE_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));
-  if (c->filter.out == NULL) goto error;
-
 return CHASSIS_OK; /* 返回成功 */
-
-error:
-  /* 动态内存分配错误时，释放已经分配的内存，返回错误值 */
-  BSP_Free(c->pid.motor_id);
-  BSP_Free(c->filter.in);
-  BSP_Free(c->filter.out);
-  return CHASSIS_ERR_NULL;
 }
 
 /**
@@ -139,163 +123,92 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
 
   c->dt = (float)(now - c->lask_wakeup) / 1000.0f;
   c->lask_wakeup = now;
-  c->time += c->dt; /* 更新用于生成动作的时间 */
+  c->mode = c_cmd->mode; /* 设置底盘模式 */
-  // while (c->time > 1.0f) { c ->time -= 1.0f; } /* 保持时间在0-1秒之间 */
+
 
-  Chassis_SetMode(c, c_cmd); /* 设置底盘模式和动作 */
 
   switch (c->mode) {
     case CHASSIS_MODE_RELAX: /* 放松模式，电机不输出 */
       c->output = (GO_ChassisCMD_t){0}; /* 清空输出 */
-      c->mode = CHASSIS_MODE_RELAX; /* 更新状态 */
+      chassis_mode_states = CHASSIS_MODE_RELAX; /* 更新状态 */
-      c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
-      c->time = 0.0f; /* 重置时间 */
       break;
     case CHASSIS_MODE_DAMP: /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */
       for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
         c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
       }
-      c->mode = CHASSIS_MODE_DAMP; /* 更新状态 */
+      chassis_mode_states = CHASSIS_MODE_DAMP; /* 更新状态 */
-      c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
-      c->time = 0.0f; /* 重置时间 */
       break;
-    case CHASSIS_MODE_POSITION:{
+    case CHASSIS_MODE_RECOVER: /* 恢复模式，电机闭环控制恢复到初始位置。用于机器人上电后恢复到初始位置 */
+      if (chassis_mode_states != CHASSIS_MODE_RECOVER){
         for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
-              c->output.id[i] = position_mode; /* 设置位置模式 */}
+          if (i % 3 == 0) { /* 每三个电机一个组 */
-
+            c->output.id[i].K_P = 1.0;
-      switch (c->action) {
+            c->output.id[i].K_W = 0.1; /* 设置刚度系数和速度系数 */
-        case CHASSIS_ACTION_NONE:{
+            c->output.id[i].Pos = 0.0f; /* 设置位置为零 */
-          for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
+            c->output.id[i].T = 0.0f; /* 设置力矩为零 */
-            c->output.id[i].Pos = c->feedback.id[i].Pos;}
+            c->output.id[i].W = 0.0f; /* 设置速度为零 */
-          c->mode = CHASSIS_MODE_POSITION;
+          } 
-          c->action = CHASSIS_ACTION_NONE;
+          else {
-          c->time = 0.0f; /* 重置时间 */
+            // c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
-          break;
+            c->output.id[i] = position_mode; /* 设置位置模式 */
           }
 
-        case CHASSIS_ACTION_STAND:{
+          // c->output.named.lf_thigh.Pos = 1.0f; /* 左前腿大腿位置 */
+          // c->output.named.lf_calf.Pos = 2.0f; /* 左前腿小腿位置 */
+          // c->output.named.rf_thigh.Pos = -1.0f; /* 右前腿大腿位置 */
+          // c->output.named.rf_calf.Pos = -2.0f; /* 右前腿小腿位置 */
+          // c->output.named.lr_thigh.Pos = 1.0f; /* 左后腿大腿位置 */
+          // c->output.named.lr_calf.Pos = 2.0f; /* 左后腿小腿位置 */
+          // c->output.named.rr_thigh.Pos = -1.0f; /* 右后腿大腿位置 */
+          // c->output.named.rr_calf.Pos = -2.0f; /* 右后腿小腿位置 */
+          c->output.named.lf_thigh.Pos = c->height * 5;
+          c->output.named.lf_calf.Pos = c->height * 10;
+          c->output.named.rf_thigh.Pos = -c->height * 5;
+          c->output.named.rf_calf.Pos = -c->height * 10;
+          c->output.named.lr_thigh.Pos = c->height * 5;
+          c->output.named.lr_calf.Pos = c->height * 10;
+          c->output.named.rr_thigh.Pos = -c->height * 5;
+          c->output.named.rr_calf.Pos = -c->height * 10;
+          chassis_mode_states = CHASSIS_MODE_RECOVER; /* 更新状态 */
+        }
+        break;
+      }
+      c->output.named.lf_hip.Pos = 0.0f; /* 左前腿髋关节位置 */
+      c->output.named.lr_hip.Pos = 0.0f; /* 左后腿髋关节位置 */
+      c->output.named.rf_hip.Pos = 0.0f; /* 右前腿髋关节位置 */
+      c->output.named.rr_hip.Pos = 0.0f; /* 右后腿髋关节位置 */
+
       c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
-          for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
+      c->output.named.lf_thigh.Pos = c->height * 5;
-            if (i % 2 == 0) { /* 左前腿和右后腿 */
+      c->output.named.lf_calf.Pos = c->height * 10;
-              float target_pose[3] = {0.0, -0.0861, -c->height};
+      c->output.named.rf_thigh.Pos = -c->height * 5;
+      c->output.named.rf_calf.Pos = -c->height * 10;
+      c->output.named.lr_thigh.Pos = c->height * 5;
+      c->output.named.lr_calf.Pos = c->height * 10;
+      c->output.named.rr_thigh.Pos = -c->height * 5;
+      c->output.named.rr_calf.Pos = -c->height * 10;
+      chassis_mode_states = CHASSIS_MODE_RECOVER; /* 更新状态 */
+
+      for (uint8_t i = 0; i < 4; i++) {
+        float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
         float angle_pose[3];
-              Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
+        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].Pos = angle_pose[0]; /* 髋关节位置 */
-              c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
+        c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 大腿位置 */
-              c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
+        c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 小腿位置 */
-            } else {
-              float target_pose[3] = {0.0, 0.0861, -c->height}; /* 右前腿和左后腿 */
-              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]; /* 右前腿小腿 */
       }
-          }
+      // float target_pose[3] = {c_cmd->ctrl_vec.vx/10.0f, c_cmd->ctrl_vec.vy/10.0f, -0.2f}; /* 目标位置 */
-          break;
+      // float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
-        } /* 站立动作 */
+      // float angle_pose[3];
+      // Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[0]); /* 逆运动学计算 */
+      // c->output.named.lf_hip.Pos = angle_pose[0]; /* 左前腿髋关节位置 */
+      // c->output.named.lf_thigh.Pos = angle_pose[1]; /* 左前腿大腿位置 */
+      // c->output.named.lf_calf.Pos = angle_pose[2]; /* 左前腿小腿位置 */
 
-        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;
   }
 
-
+  // Limit_ChassicOutput(&c->feedback, &c->output, 0.5, 0.0, 0.0);
-        case CHASSIS_ACTION_TROT:{
-          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; // 摆动腿抬高高度
-
-          // 对角腿分组：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 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 mid1[3] = {0, base_y, -c->height + swing_height};
-              float mid2[3] = {0, base_y, -c->height + swing_height};
-              float end[3] = {stride_x, base_y + stride_y, -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 end[3]   = {-stride_x, base_y - stride_y, -c->height};   // 下一个摆动相起点
-              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;
-        }
-
-
-      }
-    }   
-    
-  }
-
   for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
     /* 限制输出 */
     Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos,

User/module/chassis.h

@@ -98,7 +98,7 @@ typedef struct{
   joint_params ratio;      /* 关节减速比 */
   joint_limits limit; /* 关节的最小和最大角度，单位：弧度 */
   Kinematics_LinkLength_t length; /* 关节长度，单位：米 */
-  Kinematics_LegOffset_t leg_offset; /* 关节偏移，单位：米 */
+  // Kinematics_LegOffset_t offset; /* 关节偏移，单位：米 */
   Kinematics_DirectionSign_t sign; /* 关节侧向标志（左前/左后为-1，右前/右后为1） */
     
 }Chassis_Mech_Params_t;
@@ -126,7 +126,6 @@ typedef struct {
 typedef struct {
   uint32_t lask_wakeup;
   float dt;
-  float time;
 
   const Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
 
@@ -135,7 +134,6 @@ typedef struct {
   float height; /* 底盘高度，单位：米 */
   /* 模块通用 */
   CMD_ChassisMode_t mode; /* 底盘模式 */
-  CMD_ChassisAction_t action; /* 底盘模式 */
 
   /* 底盘设计 */
   int8_t num_joint; /* 关节数量 */
@@ -144,7 +142,8 @@ typedef struct {
 
   /* 反馈控制用的PID */
   struct {
-    KPID_t *motor_id; /* 控制轮子电机用的PID的动态数组 */
+    KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */
+    KPID_t follow; /* 跟随云台用的PID */
   } pid;
 
   /* 滤波器 */

User/module/config.c

@@ -19,13 +19,11 @@ Config_t param_default = {
         .left_leg = BSP_UART_LEFT_LEG,
         .right_leg = BSP_UART_RIGHT_LEG,
     },
-    .chassis_imu_type = IMU_WHEELREC_N100,
     .cali = {
         .bmi088 = {
             .gyro_offset = {0.0f, 0.0f, 0.0f},
         },
     },
-    
     .chassis = {
         .type = CHASSIS_TYPE_QUADRUPED,
 
@@ -47,74 +45,66 @@ Config_t param_default = {
 
         .mech_param = {
             .length = {
-                .hip = 0.0861f,
+                .hip = 0.05f,
                 .thigh = 0.20f,
                 .calf = 0.20f, 
             },
             .limit = {
                 .max = {
                     .named = {
-                        .lf_hip = 0.50f,   /* 左前腿髋关节最大角度，单位：弧度 */
+                        .lf_hip = 1.57f,   /* 左前腿髋关节最大角度，单位：弧度 */
-                        .lf_thigh = 2.50f, /* 左前腿大腿最大角度，单位：弧度 */
+                        .lf_thigh = 1.57f, /* 左前腿大腿最大角度，单位：弧度 */
-                        .lf_calf = 2.60f,  /* 左前腿小腿最大角度，单位：弧度 */
+                        .lf_calf = 1.57f,  /* 左前腿小腿最大角度，单位：弧度 */
-                        .rf_hip = 0.50f,   /* 右前腿髋关节最大角度，单位：弧度 */
+                        .rf_hip = 1.57f,   /* 右前腿髋关节最大角度，单位：弧度 */
-                        .rf_thigh = 0.0f, /* 右前腿大腿最大角度，单位：弧度 */
+                        .rf_thigh = 1.57f, /* 右前腿大腿最大角度，单位：弧度 */
-                        .rf_calf = -1.2f,  /* 右前腿小腿最大角度，单位：弧度 */
+                        .rf_calf = 1.57f,  /* 右前腿小腿最大角度，单位：弧度 */
-                        .lr_hip = 0.50f,   /* 左后腿髋关节最大角度，单位：弧度 */
+                        .lr_hip = 1.57f,   /* 左后腿髋关节最大角度，单位：弧度 */
-                        .lr_thigh = 2.50f, /* 左后腿大腿最大角度，单位：弧度 */
+                        .lr_thigh = 1.57f, /* 左后腿大腿最大角度，单位：弧度 */
-                        .lr_calf = 2.60f,  /* 左后腿小腿最大角度，单位：弧度 */
+                        .lr_calf = 1.57f,  /* 左后腿小腿最大角度，单位：弧度 */
-                        .rr_hip = 0.50f,   /* 右后腿髋关节最大角度，单位：弧度 */
+                        .rr_hip = 1.57f,   /* 右后腿髋关节最大角度，单位：弧度 */
-                        .rr_thigh = 0.0f, /* 右后腿大腿最大角度，单位：弧度 */
+                        .rr_thigh = 1.57f, /* 右后腿大腿最大角度，单位：弧度 */
-                        .rr_calf = -1.2f,  /* 右后腿小腿最大角度，单位：弧度 */
+                        .rr_calf = 1.57f,  /* 右后腿小腿最大角度，单位：弧度 */
                     }
                 },
                 .min = {
                     .named = {
-                        .lf_hip = -0.50f,   /* 左前腿髋关节最大角度，单位：弧度 */
+                        .lf_hip = 1.57f,   /* 左前腿髋关节最大角度，单位：弧度 */
-                        .lf_thigh = 0.0f, /* 左前腿大腿最大角度，单位：弧度 */
+                        .lf_thigh = 1.57f, /* 左前腿大腿最大角度，单位：弧度 */
-                        .lf_calf = 1.20f,  /* 左前腿小腿最大角度，单位：弧度 */
+                        .lf_calf = 1.57f,  /* 左前腿小腿最大角度，单位：弧度 */
-                        .rf_hip = -0.50f,   /* 右前腿髋关节最大角度，单位：弧度 */
+                        .rf_hip = 1.57f,   /* 右前腿髋关节最大角度，单位：弧度 */
-                        .rf_thigh = -2.50f, /* 右前腿大腿最大角度，单位：弧度 */
+                        .rf_thigh = 1.57f, /* 右前腿大腿最大角度，单位：弧度 */
-                        .rf_calf = -2.6f,  /* 右前腿小腿最大角度，单位：弧度 */
+                        .rf_calf = 1.57f,  /* 右前腿小腿最大角度，单位：弧度 */
-                        .lr_hip = -0.50f,   /* 左后腿髋关节最大角度，单位：弧度 */
+                        .lr_hip = 1.57f,   /* 左后腿髋关节最大角度，单位：弧度 */
-                        .lr_thigh = 0.0f, /* 左后腿大腿最大角度，单位：弧度 */
+                        .lr_thigh = 1.57f, /* 左后腿大腿最大角度，单位：弧度 */
-                        .lr_calf = 1.20f,  /* 左后腿小腿最大角度，单位：弧度 */
+                        .lr_calf = 1.57f,  /* 左后腿小腿最大角度，单位：弧度 */
-                        .rr_hip = -0.50f,   /* 右后腿髋关节最大角度，单位：弧度 */
+                        .rr_hip = 1.57f,   /* 右后腿髋关节最大角度，单位：弧度 */
-                        .rr_thigh = -2.50f, /* 右后腿大腿最大角度，单位：弧度 */
+                        .rr_thigh = 1.57f, /* 右后腿大腿最大角度，单位：弧度 */
-                        .rr_calf = -2.6f,  /* 右后腿小腿最大角度，单位：弧度 */
+                        .rr_calf = 1.57f,  /* 右后腿小腿最大角度，单位：弧度 */
                     }
                 }
             },
-
-
             .zero_point = {
                 .named = {
-                    .lf_hip = 0.02f,   /* 左前腿髋关节零点角度，单位：弧度 */
+                    .lf_hip = 1.98f,   /* 左前腿髋关节零点角度，单位：弧度 */
-                    .lf_thigh = -3.17f, /* 左前腿大腿零点角度，单位：弧度 */
+                    .lf_thigh = -2.86f, /* 左前腿大腿零点角度，单位：弧度 */
-                    .lf_calf = 2.84f - JOINT_CALF_OFFSET,  /* 左前腿小腿零点角度，单位：弧度 */
+                    .lf_calf = -32.367f,  /* 左前腿小腿零点角度，单位：弧度 */
-
+                    .rf_hip = 5.56f,   /* 右前腿髋关节零点角度，单位：弧度 */
-                    .rf_hip = 5.37f,   /* 右前腿髋关节零点角度，单位：弧度 */
+                    .rf_thigh = 12.92f, /* 右前腿大腿零点角度，单位：弧度 */
-                    .rf_thigh = 9.38f, /* 右前腿大腿零点角度，单位：弧度 */
+                    .rf_calf = 37.047f,  /* 右前腿小腿零点角度，单位：弧度 */
-                    .rf_calf = 4.45f + JOINT_CALF_OFFSET,  /* 右前腿小腿零点角度，单位：弧度 */
+                    .lr_hip = 4.76f,   /* 左后腿髋关节零点角度，单位：弧度 */
-
+                    .lr_thigh = -4.57f, /* 左后腿大腿零点角度，单位：弧度 */
-                    .lr_hip = 4.70f,   /* 左后腿髋关节零点角度，单位：弧度 */
+                    .lr_calf = -35.847f,  /* 左后腿小腿零点角度，单位：弧度 */
-                    .lr_thigh = -4.11f, /* 左后腿大腿零点角度，单位：弧度 */
+                    .rr_hip = 2.42f,   /* 右后腿髋关节零点角度，单位：弧度 */
-                    .lr_calf = 2.56f - JOINT_CALF_OFFSET,  /* 左后腿小腿零点角度，单位：弧度 */
+                    .rr_thigh = 8.85f, /* 右后腿大腿零点角度，单位：弧度 */
-
+                    .rr_calf = 38.55f,  /* 右后腿小腿零点角度，单位：弧度 */
-                    .rr_hip = 2.85f,   /* 右后腿髋关节零点角度，单位：弧度 */
-                    .rr_thigh = 10.81f, /* 右后腿大腿零点角度，单位：弧度 */
-                    .rr_calf = 3.59f + JOINT_CALF_OFFSET,  /* 右后腿小腿零点角度，单位：弧度 */
                 }
             },
-
-
-
             .sign = {
                 .leg = {
-                    [0] = { .hip = 1, .thigh = 1, .calf = -1 }, /* 左前腿 */
+                    [0] = { .hip = -1, .thigh = -1, .calf = -1 }, /* 左前腿 */
-                    [1] = { .hip = -1, .thigh = -1, .calf = 1 },   /* 右前腿 */
+                    [1] = { .hip = 1, .thigh = 1, .calf = 1 },   /* 右前腿 */
-                    [2] = { .hip = -1, .thigh = 1, .calf = -1 }, /* 左后腿 */
+                    [2] = { .hip = -1, .thigh = -1, .calf = -1 }, /* 左后腿 */
-                    [3] = { .hip = 1, .thigh = -1, .calf = 1 },   /* 右后腿 */
+                    [3] = { .hip = 1, .thigh = 1, .calf = 1 },   /* 右后腿 */
                 }
             },
             .ratio = {
@@ -133,11 +123,6 @@ Config_t param_default = {
                     .rr_calf = 12.66f, /* 右后腿小腿减速比 */
                 }
             },
-            .leg_offset = {
-                .x = 0.19625f, /* 腿偏移X轴，单位：米 */
-                .y = 0.0825f, /* 腿偏移Y轴，单位：米 */
-                .z = 0.0f, /* 腿偏移Z轴，单位：米 */
-            }
 
         },