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

Compare commits

base: robofish:07fd423f20e0fa9efb20539268f7ba95cab0b4ea
Branches Tags
robofish:main
robofish:stm32
robofish:pos_control
robofish:ros2_control
..
compare: robofish:cc4d62efd5b04842ba9f6d70a21436dfe51b2934
Branches Tags
robofish:stm32
robofish:pos_control
robofish:ros2_control
robofish:main

No commits in common. "07fd423f20e0fa9efb20539268f7ba95cab0b4ea" and "cc4d62efd5b04842ba9f6d70a21436dfe51b2934" have entirely different histories.
07fd423f20 ... cc4d62efd5

18 changed files with 4623 additions and 5014 deletions
Show Stats Expand all files Collapse all files

6
.vscode/settings.json vendored
View File

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

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

@ -183,8 +183,23 @@
<Ww> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>angle_pose</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>chassis_cmd</ItemText> <ItemText>chassis_cmd</ItemText>
</Ww> </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> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -1283,32 +1298,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <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> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@ -1322,7 +1313,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>84</FileNumber> <FileNumber>82</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1334,7 +1325,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>85</FileNumber> <FileNumber>83</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1346,7 +1337,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>86</FileNumber> <FileNumber>84</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1358,7 +1349,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber> <FileNumber>85</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1378,7 +1369,19 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>12</GroupNumber> <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> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1390,7 +1393,7 @@
</File> </File>
<File> <File>
<GroupNumber>12</GroupNumber> <GroupNumber>12</GroupNumber>
<FileNumber>89</FileNumber> <FileNumber>88</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1410,7 +1413,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber> <FileNumber>89</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1422,7 +1425,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber> <FileNumber>90</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1434,7 +1437,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber> <FileNumber>91</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1446,7 +1449,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber> <FileNumber>92</FileNumber>
<FileType>5</FileType> <FileType>5</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1458,7 +1461,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>94</FileNumber> <FileNumber>93</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1470,7 +1473,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>95</FileNumber> <FileNumber>94</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1482,7 +1485,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>96</FileNumber> <FileNumber>95</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1494,7 +1497,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>97</FileNumber> <FileNumber>96</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1506,7 +1509,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>98</FileNumber> <FileNumber>97</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1518,7 +1521,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>99</FileNumber> <FileNumber>98</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1530,7 +1533,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>100</FileNumber> <FileNumber>99</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1550,7 +1553,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>14</GroupNumber> <GroupNumber>14</GroupNumber>
<FileNumber>101</FileNumber> <FileNumber>100</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1562,7 +1565,7 @@
</File> </File>
<File> <File>
<GroupNumber>14</GroupNumber> <GroupNumber>14</GroupNumber>
<FileNumber>102</FileNumber> <FileNumber>101</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>

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

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

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

Binary file not shown.

8843
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
View File

@ -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); }

20
User/bsp/mm.h
View File

@ -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

24
User/component/bezier_curve.c Normal file
View File

@ -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];
}
}

20
User/component/bezier_curve.h Normal file
View File

@ -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

78
User/component/cmd.c
View File

@ -20,78 +20,36 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
/* 左拨杆相应行为选择和解析 */ /* 左拨杆相应行为选择和解析 */
case CMD_SW_UP: case CMD_SW_UP:
cmd->chassis.mode = CHASSIS_MODE_RELAX; 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; break;
case CMD_SW_MID: case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_DAMP; 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; break;
case CMD_SW_DOWN: 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; break;
case CMD_SW_ERR: case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX; cmd->chassis.mode = CHASSIS_MODE_RELAX;
switch (rc->sw_r) { break;
}
switch (rc->sw_r) {
/* 右拨杆相应行为选择和解析*/ /* 右拨杆相应行为选择和解析*/
case CMD_SW_UP: case CMD_SW_UP:
cmd->chassis.action = CHASSIS_ACTION_NONE; cmd->chassis.action = CHASSIS_ACTION_NONE;
break; break;
case CMD_SW_MID:
cmd->chassis.action = CHASSIS_ACTION_NONE; case CMD_SW_MID:
break; cmd->chassis.action = CHASSIS_ACTION_NONE;
case CMD_SW_DOWN: break;
cmd->chassis.action = CHASSIS_ACTION_NONE;
break; case CMD_SW_DOWN:
case CMD_SW_ERR: cmd->chassis.action = CHASSIS_ACTION_NONE;
cmd->chassis.action = CHASSIS_ACTION_NONE; break;
break;
} case CMD_SW_ERR:
cmd->chassis.action = CHASSIS_ACTION_NONE;
break; break;
} }
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */

4
User/component/cmd.h
View File

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

6
User/component/kinematics.c
View File

@ -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) { 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 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 pz = foot_in[2];
float b2y = leg_params->hip * sign->hip; float b2y = leg_params->hip * sign->hip;

3
User/component/limiter.h
View File

@ -11,11 +11,10 @@ extern "C" {
#include <stdbool.h> #include <stdbool.h>
#include <stdint.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 max_speed, float max_angle, float min_angle); float max_speed, float max_angle, float min_angle);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

62
User/component/path.c
View File

@ -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坐标插值
}

58
User/component/path.h
View File

@ -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

279
User/module/chassis.c
View File

@ -8,16 +8,15 @@
#include <stdlib.h> #include <stdlib.h>
#include "cmsis_os2.h" #include "cmsis_os2.h"
// #include "component/limiter.h"
#include "device/go.h" #include "device/go.h"
#include "component/limiter.h" #include "component/limiter.h"
#include "component/kinematics.h"
#include "component/path.h"
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/*底盘默认高度*/
#define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */ #define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */
#define CHASSIS_MAX_SPEED (0.03f)
#define CHASSIS_MAX_SPEED (0.03f) /*调试用速度限幅*/
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
@ -43,40 +42,40 @@ static Kinematics_JointCMD_t position_mode = {
.T = 0.0f, /* 零力矩 */ .T = 0.0f, /* 零力矩 */
.W = 0.0f, /* 零速度 */ .W = 0.0f, /* 零速度 */
.Pos = 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 -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
/** /**
* \brief 设置底盘运行模式 * \brief 设置底盘模式
* *
* \param c 包含底盘数据的结构体 * \param c 包含底盘数据的结构体
* \param mode 要设置的模式 * \param mode 要设置的模式
* *
* \return 函数运行结果 * \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 ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
@ -95,25 +94,10 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
c->param = param; /* 初始化参数 */ c->param = param; /* 初始化参数 */
c->mode = CHASSIS_MODE_RELAX; /* 设置上电后底盘默认模式 */ c->mode = CHASSIS_MODE_RELAX; /* 设置上电后底盘默认模式 */
c->height = CHASSIS_DEFAULT_HEIGHT; /* 设置底盘默认高度为0.2米 */ 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; /* 返回成功 */ 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->dt = (float)(now - c->lask_wakeup) / 1000.0f;
c->lask_wakeup = now; 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) { switch (c->mode) {
case CHASSIS_MODE_RELAX: /* 放松模式，电机不输出 */ case CHASSIS_MODE_RELAX: /* 放松模式，电机不输出 */
c->output = (GO_ChassisCMD_t){0}; /* 清空输出 */ 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; break;
case CHASSIS_MODE_DAMP: /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */ case CHASSIS_MODE_DAMP: /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) { for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
c->output.id[i] = damping_torque; /* 设置阻尼力矩 */ 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; break;
case CHASSIS_MODE_POSITION:{ case CHASSIS_MODE_RECOVER: /* 恢复模式，电机闭环控制恢复到初始位置。用于机器人上电后恢复到初始位置 */
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) { if (chassis_mode_states != CHASSIS_MODE_RECOVER){
c->output.id[i] = position_mode; /* 设置位置模式 */} 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; /* 设置位置模式 */
}
switch (c->action) { // c->output.named.lf_thigh.Pos = 1.0f; /* 左前腿大腿位置 */
case CHASSIS_ACTION_NONE:{ // c->output.named.lf_calf.Pos = 2.0f; /* 左前腿小腿位置 */
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) { // c->output.named.rf_thigh.Pos = -1.0f; /* 右前腿大腿位置 */
c->output.id[i].Pos = c->feedback.id[i].Pos;} // c->output.named.rf_calf.Pos = -2.0f; /* 右前腿小腿位置 */
c->mode = CHASSIS_MODE_POSITION; // c->output.named.lr_thigh.Pos = 1.0f; /* 左后腿大腿位置 */
c->action = CHASSIS_ACTION_NONE; // c->output.named.lr_calf.Pos = 2.0f; /* 左后腿小腿位置 */
c->time = 0.0f; /* 重置时间 */ // c->output.named.rr_thigh.Pos = -1.0f; /* 右后腿大腿位置 */
break; // 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;
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.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]); /* 逆运动学计算 */
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++) { 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,

7
User/module/chassis.h
View File

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

97
User/module/config.c
View File

@ -19,13 +19,11 @@ Config_t param_default = {
.left_leg = BSP_UART_LEFT_LEG, .left_leg = BSP_UART_LEFT_LEG,
.right_leg = BSP_UART_RIGHT_LEG, .right_leg = BSP_UART_RIGHT_LEG,
}, },
.chassis_imu_type = IMU_WHEELREC_N100,
.cali = { .cali = {
.bmi088 = { .bmi088 = {
.gyro_offset = {0.0f, 0.0f, 0.0f}, .gyro_offset = {0.0f, 0.0f, 0.0f},
}, },
}, },
.chassis = { .chassis = {
.type = CHASSIS_TYPE_QUADRUPED, .type = CHASSIS_TYPE_QUADRUPED,
@ -47,74 +45,66 @@ Config_t param_default = {
.mech_param = { .mech_param = {
.length = { .length = {
.hip = 0.0861f, .hip = 0.05f,
.thigh = 0.20f, .thigh = 0.20f,
.calf = 0.20f, .calf = 0.20f,
}, },
.limit = { .limit = {
.max = { .max = {
.named = { .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 = { .min = {
.named = { .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 = { .zero_point = {
.named = { .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 = { .sign = {
.leg = { .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 = { .ratio = {
@ -133,11 +123,6 @@ Config_t param_default = {
.rr_calf = 12.66f, /* 右后腿小腿减速比 */ .rr_calf = 12.66f, /* 右后腿小腿减速比 */
} }
}, },
.leg_offset = {
.x = 0.19625f, /* 腿偏移X轴，单位：米 */
.y = 0.0825f, /* 腿偏移Y轴，单位：米 */
.z = 0.0f, /* 腿偏移Z轴，单位：米 */
}
}, },