From e4c7de9b193f73572e8eac15d63ca8c10d4a29f4 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Mon, 18 Sep 2023 16:51:43 +0300 Subject: [PATCH] change pid wrt design --- .../include/pid_longitudinal_controller/pid.hpp | 3 ++- control/pid_longitudinal_controller/src/pid.cpp | 6 +++--- .../src/pid_longitudinal_controller.cpp | 8 ++++---- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp index 8b981c0485ed9..2e989321e0c70 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp @@ -29,6 +29,7 @@ class PIDController /** * @brief calculate the output of this PID * @param [in] error previous error + * @param [in] error_filtered filtered error * @param [in] dt time step [s] * @param [in] is_integrated if true, will use the integral component for calculation * @param [out] pid_contributions values of the proportional, integral, and derivative components @@ -36,7 +37,7 @@ class PIDController * @throw std::runtime_error if gains or limits have not been set */ double calculate( - const double error, const double dt, const bool is_integrated, + const double error, const double error_filtered, const double dt, const bool is_integrated, std::vector & pid_contributions); /** * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms diff --git a/control/pid_longitudinal_controller/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp index 530b5cd15e754..4ca8ebbd35ef2 100644 --- a/control/pid_longitudinal_controller/src/pid.cpp +++ b/control/pid_longitudinal_controller/src/pid.cpp @@ -32,7 +32,7 @@ PIDController::PIDController() } double PIDController::calculate( - const double error, const double dt, const bool enable_integration, + const double error, const double filtered_error, const double dt, const bool enable_integration, std::vector & pid_contributions) { if (!m_is_gains_set || !m_is_limits_set) { @@ -55,12 +55,12 @@ double PIDController::calculate( error_differential = 0; m_is_first_time = false; } else { - error_differential = (error - m_prev_error) / dt; + error_differential = (filtered_error - m_prev_error) / dt; } double ret_d = p.kd * error_differential; ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); - m_prev_error = error; + m_prev_error = filtered_error; pid_contributions.resize(3); pid_contributions.at(0) = ret_p; diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index ac968b4e9164c..94ad3c648b571 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -977,11 +977,11 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; const bool enable_integration = (vel_after_delay_abs > m_current_vel_threshold_pid_integrate) && is_under_control; - const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - vel_after_delay_abs); - + const double error_vel = abs(target_vel_abs - vel_after_delay_abs); + const double error_vel_filtered = m_lpf_vel_error->filter(error_vel); std::vector pid_contributions(3); - const double pid_acc = - m_pid_vel.calculate(error_vel_filtered, control_data.dt, enable_integration, pid_contributions); + const double pid_acc = m_pid_vel.calculate( + error_vel, error_vel_filtered, control_data.dt, enable_integration, pid_contributions); // Feedforward scaling: // This is for the coordinate conversion where feedforward is applied, from Time to Arclength.