TuxMonkey/bf_original_balance_chassis
1
1
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-25 11:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

修改增益K

Browse Source
This commit is contained in:
kai
2024-04-03 22:34:36 +08:00
parent 9f97a793ce
commit 41e13aee44
3 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
application/chassis/lqr_calc.h
View File

@@ -9,18 +9,18 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
float k[12][3] = {85.365277,-164.978797,-4.643325,
-7.817260,-26.353895,0.955791,
43.461413,-37.225135,-12.040913,
34.470271,-39.045724,-7.813613,
171.639863,-172.946723,59.632812,
11.281864,-11.926075,4.239721,
-19.517972,1.032904,27.896023,
-10.715152,11.663766,2.651705,
96.039929,-99.227575,36.121143,
56.371947,-60.347262,25.166639,
-213.707672,181.799382,93.194337,
-14.287060,12.217074,3.285836};
float k[12][3] = {155.616146,-200.327241,-0.278791,
-12.564474,-24.018026,0.674594,
145.503085,-87.742553,-5.916048,
97.588867,-70.361944,-4.007305,
303.894243,-240.356086,68.074439,
18.743007,-15.738160,4.718314,
-170.799127,75.225969,18.998160,
-29.312006,20.892957,1.529644,
122.553208,-114.023608,38.147691,
63.561616,-64.916409,25.861226,
-657.233777,402.487049,66.281303,
-40.107302,25.117848,1.705173};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;