TuxMonkey/bf_original_balance_chassis
1
1
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-25 03:47:47 +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

4
application/chassis/balance.c
View File

@@ -199,7 +199,7 @@ static void ControlSwitch()
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
chassis_cmd_recv.vx = 0.002 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s chassis_cmd_recv.vx = 0.002 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.delta_leglen = -0.000001f * (float)rc_data[TEMP].rc.dial; chassis_cmd_recv.delta_leglen = -0.000001f * (float)rc_data[TEMP].rc.dial;
chassis_cmd_recv.offset_angle -= 0.00001 * (float)rc_data[TEMP].rc.rocker_r_; chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
} }
} }
else else
@@ -359,7 +359,7 @@ static void LegControl() /* 腿长控制和Roll补偿 */
r_side.target_len -= roll_compensate_pid.Output; r_side.target_len -= roll_compensate_pid.Output;
static float gravity_comp = 57.63; static float gravity_comp = 57.63;
static float roll_extra_comp_p = 300; static float roll_extra_comp_p = 400;
float roll_comp = roll_extra_comp_p * chassis.roll; float roll_comp = roll_extra_comp_p * chassis.roll;
l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp; l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp;
r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + roll_comp; r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + roll_comp;

2
application/chassis/balance.h
View File

@@ -9,6 +9,8 @@
#define BALANCE_GRAVITY_BIAS 0 #define BALANCE_GRAVITY_BIAS 0
#define ROLL_GRAVITY_BIAS 0 #define ROLL_GRAVITY_BIAS 0
#define MAX_ACC_REF 0.5f #define MAX_ACC_REF 0.5f
#define MAX_DIST_TRACK 1.0f
#define MAX_VEL_TRACK 0.5f
// IMU距离中心的距离 // IMU距离中心的距离
#define CENTER_IMU_W 0 #define CENTER_IMU_W 0

24
application/chassis/lqr_calc.h
View File

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