From df8e90e18db622242f1bcb3edc9b5d1f5b0a9456 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 11 Nov 2024 16:38:12 +0100 Subject: [PATCH] mecanum: fix PID implementation --- .../RoverMecanumControl.cpp | 31 ++++++++++--------- .../RoverMecanumControl.hpp | 8 ++--- 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index 9f52ad175625..1c1be33cba34 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -52,29 +52,32 @@ void RoverMecanumControl::updateParams() { ModuleParams::updateParams(); _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + // Update PID + pid_set_parameters(&_pid_yaw_rate, _param_rm_yaw_rate_p.get(), // Proportional gain _param_rm_yaw_rate_i.get(), // Integral gain 0.f, // Derivative gain - 1.f, // Integral limit + _param_rm_yaw_rate_i.get() > FLT_EPSILON ? 1.f / _param_rm_yaw_rate_i.get() : 0.f, // Integral limit 1.f); // Output limit pid_set_parameters(&_pid_forward_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain + _param_rm_speed_p.get(), // Proportional gain + _param_rm_speed_i.get(), // Integral gain 0.f, // Derivative gain - 1.f, // Integral limit + _param_rm_speed_i.get() > FLT_EPSILON ? 1.f / _param_rm_speed_i.get() : 0.f, // Integral limit 1.f); // Output limit pid_set_parameters(&_pid_lateral_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain + _param_rm_speed_p.get(), // Proportional gain + _param_rm_speed_i.get(), // Integral gain 0.f, // Derivative gain - 1.f, // Integral limit + _param_rm_speed_i.get() > FLT_EPSILON ? 1.f / _param_rm_speed_i.get() : 0.f, // Integral limit 1.f); // Output limit pid_set_parameters(&_pid_yaw, - _param_rm_p_gain_yaw.get(), // Proportional gain - _param_rm_i_gain_yaw.get(), // Integral gain + _param_rm_yaw_p.get(), // Proportional gain + _param_rm_yaw_i.get(), // Integral gain 0.f, // Derivative gain - _max_yaw_rate, // Integral limit + _param_rm_yaw_i.get() > FLT_EPSILON ? _max_yaw_rate / _param_rm_yaw_i.get() : 0.f, // Integral limit _max_yaw_rate); // Output limit } @@ -156,10 +159,10 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; rover_mecanum_status.measured_yaw = vehicle_yaw; - rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - rover_mecanum_status.pid_yaw_integral = _pid_yaw.integral; - rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.integral; - rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.integral; + rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral * _param_rm_yaw_rate_i.get(); + rover_mecanum_status.pid_yaw_integral = _pid_yaw.integral * _param_rm_yaw_i.get(); + rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.integral * _param_rm_speed_i.get(); + rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.integral * _param_rm_speed_i.get(); _rover_mecanum_status_pub.publish(rover_mecanum_status); // Publish to motors diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp index 27a084934a0c..6d1a472eb503 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp @@ -125,10 +125,10 @@ class RoverMecanumControl : public ModuleParams (ParamFloat) _param_rm_max_yaw_rate, (ParamFloat) _param_rm_yaw_rate_p, (ParamFloat) _param_rm_yaw_rate_i, - (ParamFloat) _param_rm_p_gain_speed, - (ParamFloat) _param_rm_i_gain_speed, - (ParamFloat) _param_rm_p_gain_yaw, - (ParamFloat) _param_rm_i_gain_yaw, + (ParamFloat) _param_rm_speed_p, + (ParamFloat) _param_rm_speed_i, + (ParamFloat) _param_rm_yaw_p, + (ParamFloat) _param_rm_yaw_i, (ParamInt) _param_r_rev ) };