TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

取消重力前馈变化

Browse Source
This commit is contained in:
kai
2024-05-17 23:11:58 +08:00
parent 987d7c9b68
commit 51c92b423a
2 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
application/chassis/balance.c
View File

@@ -173,7 +173,6 @@ void BalanceInit()
// 状态初始化 // 状态初始化
l_side.target_len = r_side.target_len = 0.12; l_side.target_len = r_side.target_len = 0.12;
l_side.gravity_ff = r_side.gravity_ff = 60.0f;
chassis.vel_cov = 100; // 速度协方差初始化 chassis.vel_cov = 100; // 速度协方差初始化
chassis_status = ROBOT_READY; chassis_status = ROBOT_READY;
DWT_GetDeltaT(&balance_dwt_cnt); DWT_GetDeltaT(&balance_dwt_cnt);
@@ -384,10 +383,11 @@ static void LegControl() /* 腿长控制和Roll补偿 */
l_side.target_len += roll_compensate_pid.Output; l_side.target_len += roll_compensate_pid.Output;
r_side.target_len -= roll_compensate_pid.Output; r_side.target_len -= roll_compensate_pid.Output;
static float gravity_ff = 60;
static float roll_extra_comp_p = 400; 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) + l_side.gravity_ff - roll_comp; l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_ff - roll_comp;
r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + r_side.gravity_ff + roll_comp; r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_ff + roll_comp;
} }
static void WattLimitSet() /* 设定运动模态的输出 */ static void WattLimitSet() /* 设定运动模态的输出 */

1
application/chassis/balance.h
View File

@@ -57,7 +57,6 @@ typedef struct
float T_wheel; float T_wheel;
float zw_ddot; // 驱动轮竖直方向加速度 float zw_ddot; // 驱动轮竖直方向加速度
float normal_force; // 支持力 float normal_force; // 支持力
float gravity_ff; // 重力前馈
uint8_t fly_flag; // 离地标志位 uint8_t fly_flag; // 离地标志位
// pod // pod