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

哎

Browse Source
This commit is contained in:
Robofish 2026-01-21 11:03:24 +08:00
parent 8330656915
commit 29fdd4a2db
10 changed files with 8885 additions and 8859 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1183,6 +1183,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>74</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\component\limiter.c</PathWithFileName>
<FilenameWithoutPath>limiter.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -1193,7 +1205,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>74</FileNumber>
<FileNumber>75</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1205,7 +1217,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>75</FileNumber>
<FileNumber>76</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1217,7 +1229,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>76</FileNumber>
<FileNumber>77</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1229,7 +1241,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>77</FileNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1241,7 +1253,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>78</FileNumber>
<FileNumber>79</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1253,7 +1265,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>79</FileNumber>
<FileNumber>80</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1265,7 +1277,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>80</FileNumber>
<FileNumber>81</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1277,7 +1289,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>81</FileNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1289,7 +1301,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1301,7 +1313,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1321,7 +1333,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1333,7 +1345,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1345,7 +1357,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1357,7 +1369,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1377,7 +1389,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1389,7 +1401,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1401,7 +1413,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1413,7 +1425,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1425,7 +1437,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>93</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1437,7 +1449,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>94</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1449,7 +1461,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>94</FileNumber>
<FileNumber>95</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1461,7 +1473,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>95</FileNumber>
<FileNumber>96</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1473,7 +1485,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>96</FileNumber>
<FileNumber>97</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1485,7 +1497,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>97</FileNumber>
<FileNumber>98</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1497,7 +1509,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>98</FileNumber>
<FileNumber>99</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

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

@ -779,6 +779,11 @@
<FileType>1</FileType>
<FilePath>..\User\component\vmc.c</FilePath>
</File>
<File>
<FileName>limiter.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\limiter.c</FilePath>
</File>
</Files>
</Group>
<Group>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

10
User/module/balance_chassis.c
View File

@ -540,6 +540,12 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
Chassis_LQRControl(c, c_cmd);
Chassis_Output(c);
break;
case CHASSIS_MODE_BALANCE_ROTOR:
Chassis_LQRControl(c, c_cmd);
c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f;
c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f;
Chassis_Output(c);
break;
default:
return CHASSIS_ERR_MODE;
@ -629,13 +635,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_State_t target_state = {
.theta = 0.0f,
.d_theta = 0.0f,
.phi = -0.2f,
.phi = -0.1f,
.d_phi = 0.0f,
.x = c->chassis_state.target_x,
.d_x = c->chassis_state.target_velocity_x,
};
/* ==================== Yaw轴控制 ==================== */
if (c_cmd->mode!= CHASSIS_MODE_BALANCE_ROTOR || c_cmd->move_vec.vy == 0.0f) {
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
@ -655,6 +662,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
}
/* ==================== 左腿LQR控制 ==================== */
if (c->vmc_[0].leg.is_ground_contact) {

3
User/module/balance_chassis.h
View File

@ -40,7 +40,8 @@ typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */
// CHASSIS_MODE_CALIBRATE, /* 校准模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/
} Chassis_Mode_t;
/* 跳跃状态枚举 */

34
User/module/config.c
View File

@ -104,8 +104,8 @@ Config_RobotParam_t robot_config = {
.shoot_param = {
.trig_step_angle=M_2PI/8,
.shot_delay_time=1.0f,
.shot_burst_num=20,
.shot_delay_time=0.05f,
.shot_burst_num=1,
.fric_motor_param[0] = {
.can = BSP_CAN_1,
.id = 0x201,
@ -317,28 +317,28 @@ Config_RobotParam_t robot_config = {
},
.lqr_gains = {
.k11_coeff = { -2.101987239070885e+02f, 2.347095543874707e+02f, -1.159459871283743e+02f, -3.241021388045613e+00f }, // theta
.k12_coeff = { 4.560316971810678e-01f, -4.034884993782992e-01f, -8.609037541931352e+00f, -1.808248933097993e-01f }, // d_theta
.k13_coeff = { -4.398353138550677e+01f, 4.449488274721128e+01f, -1.570406941791402e+01f, -1.480318394233200e+00f }, // x
.k14_coeff = { -4.453434270476863e+01f, 4.584608571495492e+01f, -1.777654668835657e+01f, -2.075482135428575e+00f }, // d_x
.k15_coeff = { -6.235526028409054e+01f, 7.713157115257103e+01f, -3.685375309973255e+01f, 8.389295941174636e+00f }, // phi
.k16_coeff = { -1.616998524208392e+01f, 2.002347187804290e+01f, -9.587206376699182e+00f, 2.246428025510658e+00f }, // d_phi
.k21_coeff = { 1.125565146862570e+02f, -7.099628921833802e+01f, -3.539441069640339e+00f, 1.352123626909457e+01f }, // theta
.k22_coeff = { 5.499043468869487e+00f, -4.098738780201080e+00f, 3.543127660296783e-01f, 1.459732446406883e+00f }, // d_theta
.k23_coeff = { -5.544629627706907e+01f, 6.998139961765887e+01f, -3.384603004971483e+01f, 7.457602036770122e+00f }, // x
.k24_coeff = { -7.795712085138642e+01f, 9.384282192103521e+01f, -4.331019925766177e+01f, 9.314619186867809e+00f }, // d_x
.k25_coeff = { 2.096775452842998e+02f, -2.114480157454946e+02f, 7.456308940238735e+01f, 4.018425949787725e+00f }, // phi
.k26_coeff = { 5.608234904658202e+01f, -5.668241813181834e+01f, 2.007882601644766e+01f, 9.024904615903195e-01f }, // d_phi
.k11_coeff = { -2.210764178888050e+02f, 2.426363711747753e+02f, -1.195592871161101e+02f, -3.943946668753056e+00f }, // theta
.k12_coeff = { -2.137164311515632e+00f, 1.217737477437830e+00f, -9.525046061205586e+00f, -3.204960462460905e-01f }, // d_theta
.k13_coeff = { -5.403421185903361e+01f, 5.400834583224349e+01f, -1.871421701200579e+01f, -2.187399624044644e+00f }, // x
.k14_coeff = { -4.592744044784984e+01f, 4.660766512779469e+01f, -1.805265440365850e+01f, -2.699994690713614e+00f }, // d_x
.k15_coeff = { -9.685784551581214e+01f, 1.159388980795003e+02f, -5.296171221139453e+01f, 1.123906253873898e+01f }, // phi
.k16_coeff = { -2.294801139621741e+01f, 2.749641396600526e+01f, -1.259823164155369e+01f, 2.761284676172917e+00f }, // d_phi
.k21_coeff = { 7.727438740554770e+01f, -3.291782819657940e+01f, -1.757884718209812e+01f, 1.497962178015115e+01f }, // theta
.k22_coeff = { -4.313876591098404e-01f, 3.014310593976116e+00f, -2.775571423570345e+00f, 1.949396772465259e+00f }, // d_theta
.k23_coeff = { -8.163301739920055e+01f, 9.889264843312036e+01f, -4.547892032101780e+01f, 9.369462147227104e+00f }, // x
.k24_coeff = { -1.025410522837800e+02f, 1.187387974704523e+02f, -5.202150278239223e+01f, 1.038835798060997e+01f }, // d_x
.k25_coeff = { 2.742584002836992e+02f, -2.735816033702905e+02f, 9.479793291043821e+01f, 7.314831370012082e+00f }, // phi
.k26_coeff = { 6.734357804702088e+01f, -6.735254573603545e+01f, 2.346143287673895e+01f, 1.501777400084277e+00f }, // d_phi
},
.jump_params = {
.crouch_time_ms = 300,
.launch_time_ms = 128,
.retract_time_ms = 100,
.launch_time_ms = 120,
.retract_time_ms = 80,
.land_time_ms = 300,
.crouch_leg_length = 0.14f,
.launch_force = 200.0f,
.retract_leg_length = 0.16f, /* 收腿目标更短 */
.retract_force = -60.0f, /* 收腿前馈力加大 */
.retract_force = -120.0f, /* 收腿前馈力加大 */
},
.theta = 0.0f,
.x = 0.0f,

4
User/module/shoot.c
View File

@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
memset(&s->output,0,sizeof(s->output));
s->target_variable.target_rpm=4000.0f; /* 归一化目标转速 */
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */
return 0;
}
@ -257,7 +257,6 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
}
else if(last_firecmd==false&&cmd->firecmd==true)
{
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_FIRE;
s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num;
@ -281,7 +280,6 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
if(!cmd->firecmd)
{
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_READY;
}

2
User/task/rc.c
View File

@ -137,7 +137,7 @@ switch (dr16.data.sw_l) {
break;
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
break;

4
utils/Simulation-master/balance/series_legs/get_k_length.m
View File

@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([1500 100 2500 1500 8000 500]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[200 0;0 50]; %T Tp
Q=diag([1500 200 5000 2000 20000 1000]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[240 0;0 60]; %T Tp
K=lqr(A,B,Q,R);