diff --git a/Core/Src/ladrc.c b/Core/Src/ladrc.c index 8669eaf..2248d5e 100644 --- a/Core/Src/ladrc.c +++ b/Core/Src/ladrc.c @@ -183,12 +183,23 @@ void FourWheel_LADRC_ResetAll(void) /** * @brief 设置四个轮子的目标 RPM。 */ +#define LADRC_TARGET_SLEW_RPM_PER_S 80.0f // 目标斜率,先试 80 rpm/s + +static float Ramp_To_Target(float curr, float target, float step) +{ + if (target > curr + step) return curr + step; + if (target < curr - step) return curr - step; + return target; +} + void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm) { - ladrc_motors[MOTOR_FL].v = fl_rpm; - ladrc_motors[MOTOR_RL].v = rl_rpm; - ladrc_motors[MOTOR_FR].v = fr_rpm; - ladrc_motors[MOTOR_RR].v = rr_rpm; + const float step = LADRC_TARGET_SLEW_RPM_PER_S * LADRC_CONTROL_DT_S; + + ladrc_motors[MOTOR_FL].v = Ramp_To_Target(ladrc_motors[MOTOR_FL].v, fl_rpm, step); + ladrc_motors[MOTOR_RL].v = Ramp_To_Target(ladrc_motors[MOTOR_RL].v, rl_rpm, step); + ladrc_motors[MOTOR_FR].v = Ramp_To_Target(ladrc_motors[MOTOR_FR].v, fr_rpm, step); + ladrc_motors[MOTOR_RR].v = Ramp_To_Target(ladrc_motors[MOTOR_RR].v, rr_rpm, step); } /**