From 51c92b423a9b1dfe99896d307ba8e4f8f0a044eb Mon Sep 17 00:00:00 2001 From: kai <1797003616@qq.com> Date: Fri, 17 May 2024 23:11:58 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=96=E6=B6=88=E9=87=8D=E5=8A=9B=E5=89=8D?= =?UTF-8?q?=E9=A6=88=E5=8F=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/balance.c | 6 +++--- application/chassis/balance.h | 1 - 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 55218c7..5b9a3f0 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -173,7 +173,6 @@ void BalanceInit() // 状态初始化 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_status = ROBOT_READY; DWT_GetDeltaT(&balance_dwt_cnt); @@ -384,10 +383,11 @@ static void LegControl() /* 腿长控制和Roll补偿 */ l_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; 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; - r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + r_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) + gravity_ff + roll_comp; } static void WattLimitSet() /* 设定运动模态的输出 */ diff --git a/application/chassis/balance.h b/application/chassis/balance.h index da64ab5..c695ea0 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance.h @@ -57,7 +57,6 @@ typedef struct float T_wheel; float zw_ddot; // 驱动轮竖直方向加速度 float normal_force; // 支持力 - float gravity_ff; // 重力前馈 uint8_t fly_flag; // 离地标志位 // pod