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

测位移

Browse Source
This commit is contained in:
Robofish 2026-03-15 10:00:03 +08:00
parent bc9801a60a
commit 0ab60f81dd
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
User/module/balance_chassis.c
View File

@ -762,11 +762,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 目标状态 ==================== */ /* ==================== 目标状态 ==================== */
LQR_State_t target_state = { LQR_State_t target_state = {
.theta = 0.0f, .theta = 0.0f+c->param->lqr_offset.theta,
.d_theta = 0.0f, .d_theta = 0.0f,
.phi = 0.0f, .phi = 0.0f+c->param->lqr_offset.phi,
.d_phi = 0.0f, .d_phi = 0.0f,
.x = c->chassis_state.target_x, .x = c->chassis_state.target_x+c->param->lqr_offset.x,
.d_x = c->chassis_state.target_velocity_x, .d_x = c->chassis_state.target_velocity_x,
}; };

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=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); B=double(B);
Q=diag([4000 200 500 50 50000 10]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 Q=diag([4000 200 1000 500 50000 10]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[150 0;0 5]; %T Tp R=[150 0;0 25]; %T Tp
K=lqr(A,B,Q,R); K=lqr(A,B,Q,R);