robofish/CM_DOG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

能走了

Browse Source
This commit is contained in:
Robofish 2025-06-27 13:46:55 +08:00
parent 6ced1fd904
commit 07fd423f20
18 changed files with 4959 additions and 4586 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.vscode/settings.json vendored
View File

@ -4,6 +4,10 @@
"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"
}
}

88
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -183,18 +183,8 @@
<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>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
@ -1293,8 +1283,32 @@
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\component\bezier_curve.c</PathWithFileName>
<FilenameWithoutPath>bezier_curve.c</FilenameWithoutPath>
<PathWithFileName>..\User\component\kinematics.c</PathWithFileName>
<FilenameWithoutPath>kinematics.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>
@ -1308,7 +1322,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1320,7 +1334,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1332,7 +1346,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1344,7 +1358,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1364,19 +1378,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>12</GroupNumber>
<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>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1388,7 +1390,7 @@
</File>
<File>
<GroupNumber>12</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1408,7 +1410,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1420,7 +1422,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1432,7 +1434,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1444,7 +1446,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>93</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1456,7 +1458,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>94</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1468,7 +1470,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>94</FileNumber>
<FileNumber>95</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1480,7 +1482,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>95</FileNumber>
<FileNumber>96</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1492,7 +1494,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>96</FileNumber>
<FileNumber>97</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1504,7 +1506,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>97</FileNumber>
<FileNumber>98</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1516,7 +1518,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>98</FileNumber>
<FileNumber>99</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1528,7 +1530,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>99</FileNumber>
<FileNumber>100</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1548,7 +1550,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>14</GroupNumber>
<FileNumber>100</FileNumber>
<FileNumber>101</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1560,7 +1562,7 @@
</File>
<File>
<GroupNumber>14</GroupNumber>
<FileNumber>101</FileNumber>
<FileNumber>102</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

19
MDK-ARM/CtrBoard-H7_ALL.uvprojx
View File

@ -1459,9 +1459,19 @@
<FilePath>..\User\component\limiter.c</FilePath>
</File>
<File>
<FileName>bezier_curve.c</FileName>
<FileName>kinematics.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\bezier_curve.c</FilePath>
<FilePath>..\User\component\kinematics.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>
@ -1493,11 +1503,6 @@
<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>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

8848
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

14
User/bsp/mm.c Normal file
View File

@ -0,0 +1,14 @@
/* 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); }

20
User/bsp/mm.h Normal file
View File

@ -0,0 +1,20 @@
#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

24
User/component/bezier_curve.c
View File

@ -1,24 +0,0 @@
#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];
}
}

20
User/component/bezier_curve.h
View File

@ -1,20 +0,0 @@
/*
各类滤波器。
*/
#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

78
User/component/cmd.c
View File

@ -20,36 +20,78 @@ 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_RECOVER;
cmd->chassis.mode = CHASSIS_MODE_POSITION;
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) {
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;
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;
}
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */

4
User/component/cmd.h
View File

@ -18,6 +18,7 @@ typedef enum {
CHASSIS_MODE_DAMP, /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */
CHASSIS_MODE_RECOVER, /* 恢复模式，电机闭环控制恢复到初始位置。用于机器人上电后恢复到初始位置 */
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
CHASSIS_MODE_POSITION, /* 位置模式，电机闭环控制保持在指定位置 */
CHASSIS_MODE_FOLLOW, /* 通过闭环控制使车头方向跟随遥控器 */
} CMD_ChassisMode_t;
@ -25,6 +26,9 @@ 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;

11
User/component/kinematics.c
View File

@ -84,6 +84,10 @@ 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];
if (sign->hip != sign->thigh){
py = foot_in[1];
}
float pz = foot_in[2];
float b2y = leg_params->hip * sign->hip;
@ -101,12 +105,7 @@ int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_Lin
float q3 = q3_ik(b3z, b4z, b);
float q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z);
// theta_out[0] = q1;
if (sign->hip == sign->thigh){
theta_out[0] = q1;
} else {
theta_out[0] = -q1;
}
theta_out[0] = q1;
theta_out[1] = q2 * sign->thigh;
theta_out[2] = q3 * sign->calf;
return 0;

3
User/component/limiter.h
View File

@ -11,10 +11,11 @@ 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

62
User/component/path.c Normal file
View File

@ -0,0 +1,62 @@
/*
一些路径生成器
*/
#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坐标插值
}

58
User/component/path.h Normal file
View File

@ -0,0 +1,58 @@
/*
一些路径生成器
*/
#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

279
User/module/chassis.c
View File

@ -8,15 +8,16 @@
#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 ------------------------------------------------------------ */
@ -42,40 +43,40 @@ static Kinematics_JointCMD_t position_mode = {
.T = 0.0f, /* 零力矩 */
.W = 0.0f, /* 零速度 */
.Pos = 0.0f, /* 零位置 */
.K_P = 1.0f, /* 刚度系数 */
.K_W = 0.1f, /* 速度系数 */
.K_P = 3.0f, /* 刚度系数 */
.K_W = 0.2f, /* 速度系数 */
};
static uint8_t chassis_mode_states = 0;
static uint8_t chassis_action_states = 0;
// static uint8_t chassis_mode_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_ChassisMode_t mode,
// uint32_t now) {
// if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
// if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
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 (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
// srand(now);
// c->wz_multi = (rand() % 2) ? -1 : 1;
// }
// /* 切换模式后重置PID和滤波器 */
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// PID_Reset(c->pid.motor + i);
// LowPassFilter2p_Reset(c->filter.in + i, 0.0f);
// LowPassFilter2p_Reset(c->filter.out + i, 0.0f);
// }
// c->mode = mode;
// return CHASSIS_OK;
// }
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;
}
/* Exported functions ------------------------------------------------------- */
@ -94,10 +95,25 @@ 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;
}
/**
@ -123,92 +139,163 @@ 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->mode = c_cmd->mode; /* 设置底盘模式 */
c->time += c->dt; /* 更新用于生成动作的时间 */
// 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}; /* 清空输出 */
chassis_mode_states = CHASSIS_MODE_RELAX; /* 更新状态 */
c->mode = 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; /* 设置阻尼力矩 */
}
chassis_mode_states = CHASSIS_MODE_DAMP; /* 更新状态 */
c->mode = CHASSIS_MODE_DAMP; /* 更新状态 */
c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
c->time = 0.0f; /* 重置时间 */
break;
case CHASSIS_MODE_RECOVER: /* 恢复模式，电机闭环控制恢复到初始位置。用于机器人上电后恢复到初始位置 */
if (chassis_mode_states != CHASSIS_MODE_RECOVER){
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
if (i % 3 == 0) { /* 每三个电机一个组 */
c->output.id[i].K_P = 1.0;
c->output.id[i].K_W = 0.1; /* 设置刚度系数和速度系数 */
c->output.id[i].Pos = 0.0f; /* 设置位置为零 */
c->output.id[i].T = 0.0f; /* 设置力矩为零 */
c->output.id[i].W = 0.0f; /* 设置速度为零 */
}
else {
// c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
c->output.id[i] = position_mode; /* 设置位置模式 */
}
case CHASSIS_MODE_POSITION:{
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
c->output.id[i] = position_mode; /* 设置位置模式 */}
// 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; /* 更新状态 */
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;
}
break;
case CHASSIS_ACTION_STAND:{
c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
if (i % 2 == 0) { /* 左前腿和右后腿 */
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]; /* 左前腿小腿 */
} 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]; /* 右前腿小腿 */
}
}
break;
} /* 站立动作 */
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:{
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;
}
}
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; /* 更新底盘高度 */
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; /* 更新状态 */
for (uint8_t i = 0; i < 4; i++) {
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}; /* 目标位置 */
// 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]; /* 左前腿小腿位置 */
break;
}
// Limit_ChassicOutput(&c->feedback, &c->output, 0.5, 0.0, 0.0);
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
/* 限制输出 */
Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos,

5
User/module/chassis.h
View File

@ -126,6 +126,7 @@ typedef struct {
typedef struct {
uint32_t lask_wakeup;
float dt;
float time;
const Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
@ -134,6 +135,7 @@ typedef struct {
float height; /* 底盘高度，单位：米 */
/* 模块通用 */
CMD_ChassisMode_t mode; /* 底盘模式 */
CMD_ChassisAction_t action; /* 底盘模式 */
/* 底盘设计 */
int8_t num_joint; /* 关节数量 */
@ -142,8 +144,7 @@ typedef struct {
/* 反馈控制用的PID */
struct {
KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */
KPID_t follow; /* 跟随云台用的PID */
KPID_t *motor_id; /* 控制轮子电机用的PID的动态数组 */
} pid;
/* 滤波器 */

2
User/module/config.c
View File

@ -103,7 +103,7 @@ Config_t param_default = {
.rr_hip = 2.85f, /* 右后腿髋关节零点角度，单位：弧度 */
.rr_thigh = 10.81f, /* 右后腿大腿零点角度，单位：弧度 */
.rr_calf = 4.83f + JOINT_CALF_OFFSET, /* 右后腿小腿零点角度，单位：弧度 */
.rr_calf = 3.59f + JOINT_CALF_OFFSET, /* 右后腿小腿零点角度，单位：弧度 */
}
},