diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index 8d05af1e..55f54fd8 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -33,8 +33,10 @@ #ifndef CONTROL_TOOLBOX__PID_HPP_ #define CONTROL_TOOLBOX__PID_HPP_ +#include #include #include +#include #include #include #include @@ -118,14 +120,14 @@ struct AntiWindupStrategy (tracking_time_constant < 0.0 || !std::isfinite(tracking_time_constant))) { throw std::invalid_argument( - "AntiWindupStrategy 'back_calculation' requires a valid positive tracking time constant " + "AntiWindupStrategy 'back_calculation' requires a non-negative tracking time constant " "(tracking_time_constant)"); } - if (i_min >= i_max) + if (i_min > i_max) { throw std::invalid_argument( fmt::format( - "PID requires i_min < i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); + "PID requires i_min <= i_max if limits are finite (i_min: {}, i_max: {})", i_min, i_max)); } if ( type != NONE && type != UNDEFINED && type != BACK_CALCULATION && @@ -168,6 +170,87 @@ struct AntiWindupStrategy std::numeric_limits::epsilon(); /**< Error deadband to avoid integration. */ }; +struct DiscretizationMethod +{ +public: + enum Value : int8_t + { + UNDEFINED = -1, + FORWARD_EULER, + BACKWARD_EULER, + TRAPEZOIDAL + }; + + constexpr DiscretizationMethod() : type(FORWARD_EULER) {} + explicit constexpr DiscretizationMethod(Value v) : type(v) {} + explicit DiscretizationMethod(const std::string & s) { set_type(s); } + + void set_type(const std::string & s) + { + if (s == "forward_euler") + { + type = FORWARD_EULER; + } + else if (s == "backward_euler") + { + type = BACKWARD_EULER; + } + else if (s == "trapezoidal") + { + type = TRAPEZOIDAL; + } + else + { + type = UNDEFINED; + throw std::invalid_argument( + "DiscretizationMethod: Unknown discretization method : '" + s + + "'. Valid methods are: 'forward_euler', 'backward_euler', 'trapezoidal'."); + } + } + + void validate() const + { + if (type == UNDEFINED) + { + throw std::invalid_argument("DiscretizationMethod is UNDEFINED. Please set a valid type"); + } + if (type != FORWARD_EULER && type != BACKWARD_EULER && type != TRAPEZOIDAL && type != UNDEFINED) + { + throw std::invalid_argument("DiscretizationMethod has an invalid type"); + } + } + + operator std::string() const { return to_string(); } + + constexpr bool operator==(Value other) const { return type == other; } + constexpr bool operator!=(Value other) const { return type != other; } + constexpr bool operator==(const DiscretizationMethod & other) const { return type == other.type; } + constexpr bool operator!=(const DiscretizationMethod & other) const { return type != other.type; } + constexpr DiscretizationMethod & operator=(Value v) + { + type = v; + return *this; + } + + std::string to_string() const + { + switch (type) + { + case FORWARD_EULER: + return "forward_euler"; + case BACKWARD_EULER: + return "backward_euler"; + case TRAPEZOIDAL: + return "trapezoidal"; + case UNDEFINED: + default: + return "UNDEFINED"; + } + } + + Value type = UNDEFINED; +}; + template inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) { @@ -204,6 +287,7 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) \param p Proportional gain. Reacts to current error. \param i Integral gain. Accumulates past error to eliminate steady-state error. \param d Derivative gain. Predicts future error to reduce overshoot and settling time. + \param tf Derivative filter time constant. Used to filter high-frequency noise in the derivative term. \param u\_min Minimum bound for the controller output. \param u\_max Maximum bound for the controller output. \param tracking\_time\_constant Tracking time constant for BACK_CALCULATION anti-windup. @@ -216,6 +300,14 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) and unsaturated outputs using \c tracking\_time\_constant. - CONDITIONAL_INTEGRATION: only integrates when output is not saturated or error drives it away from saturation. + \param integration\_method Method to compute the integral contribution. Options: + - "forward_euler" + - "backward_euler" + - "trapezoidal" + \param derivative\_method Method to compute the derivative contribution. Options: + - "forward_euler" + - "backward_euler" + - "trapezoidal" \section antiwindup Anti-Windup Strategies Without anti-windup, clamping causes integral windup, leading to overshoot and sluggish @@ -275,14 +367,45 @@ class Pid Gains( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) + : Gains( + p, i, d, + /*tf*/ 0.0, u_max, u_min, antiwindup_strat, + /*i_method*/ DiscretizationMethod(), + /*d_method*/ DiscretizationMethod()) + { + } + + /*! + * \brief Constructor for passing in values. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * \param i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * + */ + Gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) : p_gain_(p), i_gain_(i), d_gain_(d), + tf_(tf), i_max_(antiwindup_strat.i_max), i_min_(antiwindup_strat.i_min), u_max_(u_max), u_min_(u_min), - antiwindup_strat_(antiwindup_strat) + antiwindup_strat_(antiwindup_strat), + i_method_(i_method), + d_method_(d_method) { if (std::isnan(u_min) || std::isnan(u_max)) { @@ -301,12 +424,14 @@ class Pid { if (i_min_ > i_max_) { - error_msg = fmt::format("Gains: i_min ({}) must be less than i_max ({})", i_min_, i_max_); + error_msg = + fmt::format("Gains: i_min ({}) must be less than or equal to i_max ({})", i_min_, i_max_); return false; } - else if (u_min_ >= u_max_) + else if (u_min_ > u_max_) { - error_msg = fmt::format("Gains: u_min ({}) must be less than u_max ({})", u_min_, u_max_); + error_msg = + fmt::format("Gains: u_min ({}) must be less than or equal to u_max ({})", u_min_, u_max_); return false; } else if (std::isnan(u_min_) || std::isnan(u_max_)) @@ -314,9 +439,16 @@ class Pid error_msg = "Gains: u_min and u_max must not be NaN"; return false; } + else if (tf_ < 0.0) + { + error_msg = "Gains: tf (derivative filter time constant) must be non-negative"; + return false; + } try { antiwindup_strat_.validate(); + i_method_.validate(); + d_method_.validate(); } catch (const std::exception & e) { @@ -329,14 +461,17 @@ class Pid void print() const { std::cout << "Gains: p: " << p_gain_ << ", i: " << i_gain_ << ", d: " << d_gain_ - << ", i_max: " << i_max_ << ", i_min: " << i_min_ << ", u_max: " << u_max_ - << ", u_min: " << u_min_ << ", antiwindup_strat: " << antiwindup_strat_.to_string() - << std::endl; + << ", tf: " << tf_ << ", i_max: " << i_max_ << ", i_min: " << i_min_ + << ", u_max: " << u_max_ << ", u_min: " << u_min_ + << ", antiwindup_strat: " << antiwindup_strat_.to_string() + << ", i_method: " << i_method_.to_string() + << ", d_method: " << d_method_.to_string() << std::endl; } double p_gain_ = 0.0; /**< Proportional gain. */ double i_gain_ = 0.0; /**< Integral gain. */ double d_gain_ = 0.0; /**< Derivative gain. */ + double tf_ = 0.0; /**< Derivative filter time constant. */ double i_max_ = std::numeric_limits::infinity(); /**< Maximum allowable integral term. */ double i_min_ = @@ -344,6 +479,8 @@ class Pid double u_max_ = std::numeric_limits::infinity(); /**< Maximum allowable output. */ double u_min_ = -std::numeric_limits::infinity(); /**< Minimum allowable output. */ AntiWindupStrategy antiwindup_strat_; /**< Anti-windup strategy. */ + DiscretizationMethod i_method_; /**< Integration method. */ + DiscretizationMethod d_method_; /**< Derivative method. */ }; /*! @@ -366,6 +503,28 @@ class Pid double u_min = -std::numeric_limits::infinity(), const AntiWindupStrategy & antiwindup_strat = AntiWindupStrategy()); + /*! + * \brief Constructor, initialize Pid-gains and term limits. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * \param i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * + * \throws An std::invalid_argument exception is thrown if u_min > u_max. + */ + Pid( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method); + /*! * \brief Copy constructor required for preventing mutexes from being copied * \param source - Pid to copy @@ -396,6 +555,29 @@ class Pid double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat); + /*! + * \brief Initialize Pid-gains and term limits. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * \param i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * \return True if all parameters are successfully set, False otherwise. + * + * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + */ + bool initialize( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method); + /*! * \brief Reset the state of this PID controller * @note The integral term is not retained and it is reset to zero @@ -431,6 +613,27 @@ class Pid double & p, double & i, double & d, double & u_max, double & u_min, AntiWindupStrategy & antiwindup_strat); + /*! + * \brief Get PID gains for the controller (preferred). + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * \param i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * + * \note This method is not RT safe + */ + void get_gains( + double & p, double & i, double & d, double & tf, double & u_max, double & u_min, + AntiWindupStrategy & antiwindup_strat, DiscretizationMethod & i_method, + DiscretizationMethod & d_method); + /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values @@ -467,6 +670,30 @@ class Pid double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat); + /*! + * \brief Set PID gains for the controller. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * \param i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * \return True if all parameters are successfully set, False otherwise. + * + * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + * \note This method is not RT safe + */ + bool set_gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values. @@ -623,19 +850,29 @@ class Pid 0.0, 0.0, 0.0, + 0.0, std::numeric_limits::infinity(), -std::numeric_limits::infinity(), - AntiWindupStrategy()}; + AntiWindupStrategy(), + DiscretizationMethod(), + DiscretizationMethod()}; // Store the PID gains in a realtime box to allow dynamic reconfigure to update it without // blocking the realtime update loop realtime_tools::RealtimeThreadSafeBox gains_box_{gains_}; double p_error_last_ = 0; /** Save state for derivative state calculation. */ double p_error_ = 0; /** Error. */ + + double d_error_last_ = 0; /** Save state for derivative state calculation. */ + double d_term_last_ = 0; /** Save state for derivative filter. */ double d_error_ = 0; /** Derivative of error. */ + double i_term_ = 0; /** Integrator state. */ - double cmd_ = 0; /** Command to send. */ - double cmd_unsat_ = 0; /** command without saturation. */ + double i_term_last_ = 0; /** Last integrator state. */ + double aw_term_last_ = 0; /** Last anti-windup term. */ + + double cmd_ = 0; /** Command to send. */ + double cmd_unsat_ = 0; /** command without saturation. */ }; } // namespace control_toolbox diff --git a/control_toolbox/include/control_toolbox/pid_ros.hpp b/control_toolbox/include/control_toolbox/pid_ros.hpp index a973aa39..6acaa520 100644 --- a/control_toolbox/include/control_toolbox/pid_ros.hpp +++ b/control_toolbox/include/control_toolbox/pid_ros.hpp @@ -143,6 +143,30 @@ class PidROS double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat, bool save_i_term); + /*! + * \brief Initialize the PID controller and set the parameters. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * \param i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * \param save_i_term save integrator output between resets. + * \return True if all parameters are successfully set, False otherwise. + * + * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + */ + bool initialize_from_args( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method, bool save_i_term); + /*! * \brief Initialize the PID controller based on already set parameters * \return True if all parameters are set (p, i, d, i_max, i_min, u_max, u_min), False otherwise @@ -217,6 +241,30 @@ class PidROS double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat); + /*! + * \brief Set PID gains for the controller. + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param tf The derivative filter time constant. + * \param u_max Upper output clamp. + * \param u_min Lower output clamp. + * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior. + * \param i_method Method to compute the integral contribution. + * \param d_method Method to compute the derivative contribution. + * \return True if all parameters are successfully set, False otherwise. + * + * \note New gains are not applied if antiwindup_strat.i_min > antiwindup_strat.i_max or u_min > u_max. + * \note This method is not RT safe + */ + bool set_gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method); + /*! * \brief Set PID gains for the controller. * \param gains A struct of the PID gain values diff --git a/control_toolbox/src/pid.cpp b/control_toolbox/src/pid.cpp index 8a2495d1..538b0eed 100644 --- a/control_toolbox/src/pid.cpp +++ b/control_toolbox/src/pid.cpp @@ -49,12 +49,20 @@ namespace control_toolbox Pid::Pid( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) +: Pid(p, i, d, 0.0, u_max, u_min, antiwindup_strat, DiscretizationMethod(), DiscretizationMethod()) +{ +} + +Pid::Pid( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) { if (u_min > u_max) { throw std::invalid_argument("received u_min > u_max"); } - set_gains(p, i, d, u_max, u_min, antiwindup_strat); + set_gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); // Initialize saved i-term values clear_saved_iterm(); @@ -80,7 +88,16 @@ bool Pid::initialize( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - if (set_gains(p, i, d, u_max, u_min, antiwindup_strat)) + return initialize( + p, i, d, 0.0, u_max, u_min, antiwindup_strat, DiscretizationMethod(), DiscretizationMethod()); +} + +bool Pid::initialize( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) +{ + if (set_gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method)) { reset(); return true; @@ -95,6 +112,9 @@ void Pid::reset(bool save_i_term) p_error_last_ = 0.0; p_error_ = 0.0; d_error_ = 0.0; + d_error_last_ = 0; + d_term_last_ = 0; + aw_term_last_ = 0; cmd_ = 0.0; // Check to see if we should reset integral error here @@ -107,7 +127,11 @@ void Pid::reset(bool save_i_term) gains_ = gains_box_.get(); } -void Pid::clear_saved_iterm() { i_term_ = 0.0; } +void Pid::clear_saved_iterm() +{ + i_term_ = 0.0; + i_term_last_ = 0.0; +} void Pid::get_gains( double & p, double & i, double & d, double & u_max, double & u_min, @@ -122,6 +146,23 @@ void Pid::get_gains( antiwindup_strat = gains.antiwindup_strat_; } +void Pid::get_gains( + double & p, double & i, double & d, double & tf, double & u_max, double & u_min, + AntiWindupStrategy & antiwindup_strat, DiscretizationMethod & i_method, + DiscretizationMethod & d_method) +{ + Gains gains = get_gains(); + p = gains.p_gain_; + i = gains.i_gain_; + d = gains.d_gain_; + tf = gains.tf_; + u_max = gains.u_max_; + u_min = gains.u_min_; + antiwindup_strat = gains.antiwindup_strat_; + i_method = gains.i_method_; + d_method = gains.d_method_; +} + Pid::Gains Pid::get_gains() { // blocking, as get_gains() is called from non-RT thread @@ -131,10 +172,19 @@ Pid::Gains Pid::get_gains() bool Pid::set_gains( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) +{ + return set_gains( + p, i, d, 0.0, u_max, u_min, antiwindup_strat, DiscretizationMethod(), DiscretizationMethod()); +} + +bool Pid::set_gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) { try { - Gains gains(p, i, d, u_max, u_min, antiwindup_strat); + Gains gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); if (set_gains(gains)) { return true; @@ -198,9 +248,23 @@ double Pid::compute_command(double error, const double & dt_s) return cmd_ = std::numeric_limits::quiet_NaN(); } - // Calculate the derivative error - d_error_ = (error - p_error_last_) / dt_s; - p_error_last_ = error; + // Calculate the derivative error based on the selected method + if ( + gains_.d_method_ == DiscretizationMethod::FORWARD_EULER || + gains_.d_method_ == DiscretizationMethod::BACKWARD_EULER) + { + // Since \dot{e}[k-1] and \dot{e}[k] are calculated the same way for forward and backward euler, + // we can combine them here + d_error_ = (error - p_error_last_) / dt_s; + } + else if (gains_.d_method_ == DiscretizationMethod::TRAPEZOIDAL) + { + d_error_ = 2 * (error - p_error_last_) / dt_s - d_error_last_; + } + else + { + throw std::invalid_argument("Unknown derivative method: " + gains_.d_method_.to_string()); + } return compute_command(error, d_error_, dt_s); } @@ -246,6 +310,7 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) { throw std::invalid_argument("Pid is called with negative dt"); } + // Get the gain parameters from the realtime box auto gains_opt = gains_box_.try_get(); if (gains_opt.has_value()) @@ -268,7 +333,31 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) p_term = gains_.p_gain_ * p_error_; // Calculate derivative contribution to command - d_term = gains_.d_gain_ * d_error_; + if (gains_.tf_ > 0.0 && gains_.d_method_ == DiscretizationMethod::FORWARD_EULER) + { + // Derivative filter is on + d_term = + ((gains_.tf_ - dt_s) * d_term_last_ + (gains_.d_gain_ * dt_s * d_error_)) / (gains_.tf_); + } + else if (gains_.tf_ > 0.0 && gains_.d_method_ == DiscretizationMethod::BACKWARD_EULER) + { + // Derivative filter is on + d_term = + ((gains_.tf_) * d_term_last_ + (gains_.d_gain_ * dt_s * d_error_)) / (gains_.tf_ + dt_s); + } + else if (gains_.tf_ > 0.0 && gains_.d_method_ == DiscretizationMethod::TRAPEZOIDAL) + { + // Derivative filter is on + d_term = ((2 * gains_.tf_ - dt_s) * d_term_last_ + + (gains_.d_gain_ * dt_s * (d_error_ + d_error_last_))) / + (2 * gains_.tf_ + dt_s); + } + else if (gains_.tf_ == 0.0) + { + // Derivative filter is off. Since forward doesn't exist for tf=0, use backward. + // To avoid artificial states and amplify noise, trapezoidal also falls back to backward. + d_term = gains_.d_gain_ * d_error_; + } if (gains_.antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED) { @@ -279,6 +368,47 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) const bool is_error_in_deadband_zone = control_toolbox::is_zero(error, gains_.antiwindup_strat_.error_deadband); + // Calculate integral contribution to command + if (gains_.i_method_ == DiscretizationMethod::FORWARD_EULER) + { + i_term_ = i_term_last_ + gains_.i_gain_ * dt_s * p_error_last_; + } + else if (gains_.i_method_ == DiscretizationMethod::BACKWARD_EULER) + { + i_term_ = i_term_last_ + gains_.i_gain_ * dt_s * error; + } + else if (gains_.i_method_ == DiscretizationMethod::TRAPEZOIDAL) + { + i_term_ = i_term_last_ + gains_.i_gain_ * (dt_s * 0.5) * (error + p_error_last_); + } + else + { + throw std::invalid_argument("Unknown integration method: " + gains_.i_method_.to_string()); + } + + // Anti-windup via conditional integration + if (gains_.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) + { + if (!is_zero(cmd_ - cmd_unsat_)) + { + // If we are in saturation, don't integrate if it would drive the controller further into + // saturation. + + double dI = i_term_ - i_term_last_; // The variation of the integral term + bool sat_high = (cmd_ == gains_.u_max_); // Check if saturation at upper bound + bool sat_low = (cmd_ == gains_.u_min_); // Check if saturation at lower bound + + // If we are saturated at the upper limit and the variation of the integral term is decreasing + // or if we are saturated at the lower limit and the variation of the integral term is + // increasing, add the variation to the last integral term. + bool move_saturation = (sat_high && dI < 0) || (sat_low && dI > 0); + i_term_ = move_saturation ? i_term_ : i_term_last_; + } + } + + // Clamp the i_term_. Notice that this clamp occurs before the back-calculation technique. + i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); + // Compute the command cmd_unsat_ = p_term + i_term_ + d_term; @@ -298,30 +428,43 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s) { cmd_ = cmd_unsat_; } + if (!is_error_in_deadband_zone) { if ( gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && - !is_zero(gains_.i_gain_)) + !is_zero(gains_.i_gain_) && gains_.i_method_ == DiscretizationMethod::FORWARD_EULER) { - i_term_ += dt_s * (gains_.i_gain_ * error + - 1 / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_)); + i_term_ += dt_s * 1 / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_); } - else if (gains_.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION) + else if ( + gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && + !is_zero(gains_.i_gain_) && gains_.i_method_ == DiscretizationMethod::BACKWARD_EULER) { - if (!(!is_zero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0)) - { - i_term_ += dt_s * gains_.i_gain_ * error; - } + i_term_ = i_term_ / (1 + dt_s / gains_.antiwindup_strat_.tracking_time_constant) + + dt_s / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - p_term - d_term) / + (1 + dt_s / gains_.antiwindup_strat_.tracking_time_constant); } - else if (gains_.antiwindup_strat_.type == AntiWindupStrategy::NONE) + else if ( + gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION && + !is_zero(gains_.i_gain_) && gains_.i_method_ == DiscretizationMethod::TRAPEZOIDAL) { - // No anti-windup strategy, so just integrate the error - i_term_ += dt_s * gains_.i_gain_ * error; + const double alpha = dt_s / (2.0 * gains_.antiwindup_strat_.tracking_time_constant); + const double num_i_last = (1.0 - alpha) * i_term_last_; + const double trap_inc = gains_.i_gain_ * (dt_s * 0.5) * (p_error_ + p_error_last_); + const double aw_sum = (cmd_ - (p_term + d_term)) + aw_term_last_; + const double denom = (1.0 + alpha); + + i_term_ = (num_i_last + trap_inc + alpha * aw_sum) / denom; } } - i_term_ = std::clamp(i_term_, gains_.i_min_, gains_.i_max_); + // Update last values for next iteration + d_term_last_ = d_term; + d_error_last_ = d_error_; + i_term_last_ = i_term_; + aw_term_last_ = cmd_ - p_term - d_term; + p_error_last_ = error; return cmd_; } diff --git a/control_toolbox/src/pid_ros.cpp b/control_toolbox/src/pid_ros.cpp index 710c217d..d854b083 100644 --- a/control_toolbox/src/pid_ros.cpp +++ b/control_toolbox/src/pid_ros.cpp @@ -30,12 +30,16 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include +#include #include #include #include #include #include +#include "rcl_interfaces/msg/set_parameters_result.hpp" + #include "control_toolbox/pid_ros.hpp" namespace control_toolbox @@ -160,19 +164,22 @@ bool PidROS::get_string_param(const std::string & param_name, std::string & valu bool PidROS::initialize_from_ros_parameters() { - double p, i, d, i_max, i_min, u_max, u_min, tracking_time_constant, error_deadband; - p = i = d = tracking_time_constant = std::numeric_limits::quiet_NaN(); + double p, i, d, tf, i_max, i_min, u_max, u_min, tracking_time_constant, error_deadband; + p = i = d = tf = tracking_time_constant = std::numeric_limits::quiet_NaN(); error_deadband = std::numeric_limits::epsilon(); i_max = MAX_INFINITY; i_min = -MAX_INFINITY; u_max = MAX_INFINITY; u_min = -MAX_INFINITY; std::string antiwindup_strat_str = "none"; + std::string i_method_str = "forward_euler"; + std::string d_method_str = "forward_euler"; bool all_params_available = true; all_params_available &= get_double_param(param_prefix_ + "p", p); all_params_available &= get_double_param(param_prefix_ + "i", i); all_params_available &= get_double_param(param_prefix_ + "d", d); + all_params_available &= get_double_param(param_prefix_ + "derivative_filter_time", tf); all_params_available &= get_double_param(param_prefix_ + "i_clamp_max", i_max); all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min); all_params_available &= get_double_param(param_prefix_ + "u_clamp_max", u_max); @@ -189,6 +196,9 @@ bool PidROS::initialize_from_ros_parameters() u_min = -MAX_INFINITY; } get_string_param(param_prefix_ + "antiwindup_strategy", antiwindup_strat_str); + get_string_param(param_prefix_ + "integration_method", i_method_str); + get_string_param(param_prefix_ + "derivative_method", d_method_str); + declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(false)); declare_param( param_prefix_ + "activate_state_publisher", rclcpp::ParameterValue(rt_state_pub_ != nullptr)); @@ -205,6 +215,9 @@ bool PidROS::initialize_from_ros_parameters() antiwindup_strat.tracking_time_constant = tracking_time_constant; antiwindup_strat.error_deadband = error_deadband; + DiscretizationMethod i_method{i_method_str}; + DiscretizationMethod d_method{d_method_str}; + try { antiwindup_strat.validate(); @@ -214,8 +227,18 @@ bool PidROS::initialize_from_ros_parameters() RCLCPP_ERROR(node_logging_->get_logger(), "Invalid antiwindup strategy: %s", e.what()); return false; } + try + { + i_method.validate(); + d_method.validate(); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(node_logging_->get_logger(), "Invalid discretization method: %s", e.what()); + return false; + } - if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) + if (pid_.initialize(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method)) { return all_params_available; } @@ -235,7 +258,19 @@ bool PidROS::initialize_from_args( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat, bool save_i_term) { - Pid::Gains verify_gains(p, i, d, u_max, u_min, antiwindup_strat); + DiscretizationMethod i_method; + DiscretizationMethod d_method; + + return initialize_from_args( + p, i, d, 0.0, u_max, u_min, antiwindup_strat, i_method, d_method, save_i_term); +} + +bool PidROS::initialize_from_args( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method, bool save_i_term) +{ + Pid::Gains verify_gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); std::string error_msg = ""; if (!verify_gains.validate(error_msg)) { @@ -246,12 +281,13 @@ bool PidROS::initialize_from_args( } else { - if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat)) + if (pid_.initialize(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method)) { const Pid::Gains gains = pid_.get_gains(); declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_)); declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_)); declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_)); + declare_param(param_prefix_ + "derivative_filter_time", rclcpp::ParameterValue(gains.tf_)); declare_param( param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(gains.antiwindup_strat_.i_max)); declare_param( @@ -269,6 +305,10 @@ bool PidROS::initialize_from_args( declare_param( param_prefix_ + "antiwindup_strategy", rclcpp::ParameterValue(gains.antiwindup_strat_.to_string())); + declare_param( + param_prefix_ + "integration_method", rclcpp::ParameterValue(gains.i_method_.to_string())); + declare_param( + param_prefix_ + "derivative_method", rclcpp::ParameterValue(gains.d_method_.to_string())); declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term)); declare_param( param_prefix_ + "activate_state_publisher", @@ -320,7 +360,18 @@ bool PidROS::set_gains( double p, double i, double d, double u_max, double u_min, const AntiWindupStrategy & antiwindup_strat) { - Pid::Gains gains(p, i, d, u_max, u_min, antiwindup_strat); + DiscretizationMethod i_method; + DiscretizationMethod d_method; + + return set_gains(p, i, d, 0.0, u_max, u_min, antiwindup_strat, i_method, d_method); +} + +bool PidROS::set_gains( + double p, double i, double d, double tf, double u_max, double u_min, + const AntiWindupStrategy & antiwindup_strat, DiscretizationMethod i_method, + DiscretizationMethod d_method) +{ + Pid::Gains gains(p, i, d, tf, u_max, u_min, antiwindup_strat, i_method, d_method); return set_gains(gains); } @@ -343,6 +394,7 @@ bool PidROS::set_gains(const Pid::Gains & gains) {rclcpp::Parameter(param_prefix_ + "p", gains.p_gain_), rclcpp::Parameter(param_prefix_ + "i", gains.i_gain_), rclcpp::Parameter(param_prefix_ + "d", gains.d_gain_), + rclcpp::Parameter(param_prefix_ + "derivative_filter_time", gains.tf_), rclcpp::Parameter(param_prefix_ + "i_clamp_max", gains.antiwindup_strat_.i_max), rclcpp::Parameter(param_prefix_ + "i_clamp_min", gains.antiwindup_strat_.i_min), rclcpp::Parameter(param_prefix_ + "u_clamp_max", gains.u_max_), @@ -354,7 +406,9 @@ bool PidROS::set_gains(const Pid::Gains & gains) param_prefix_ + "error_deadband", gains.antiwindup_strat_.error_deadband), rclcpp::Parameter(param_prefix_ + "saturation", true), rclcpp::Parameter( - param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string())}); + param_prefix_ + "antiwindup_strategy", gains.antiwindup_strat_.to_string()), + rclcpp::Parameter(param_prefix_ + "integration_method", gains.i_method_.to_string()), + rclcpp::Parameter(param_prefix_ + "derivative_method", gains.d_method_.to_string())}); return true; } } @@ -410,17 +464,19 @@ void PidROS::print_values() << " P Gain: " << gains.p_gain_ << "\n" << " I Gain: " << gains.i_gain_ << "\n" << " D Gain: " << gains.d_gain_ << "\n" + << " D Filter Time: " << gains.tf_ << "\n" << " I Max: " << gains.i_max_ << "\n" << " I Min: " << gains.i_min_ << "\n" << " U_Max: " << gains.u_max_ << "\n" << " U_Min: " << gains.u_min_ << "\n" << " Tracking_Time_Constant: " << gains.antiwindup_strat_.tracking_time_constant << "\n" << " Antiwindup_Strategy: " << gains.antiwindup_strat_.to_string() << "\n" - << "\n" + << " integration_method: " << gains.i_method_.to_string() << "\n" + << " derivative_method: " << gains.d_method_.to_string() << "\n" << " P Error: " << p_error << "\n" << " I Term: " << i_term << "\n" << " D Error: " << d_error << "\n" - << " Command: " << get_current_cmd();); + << " Command: " << get_current_cmd()); } void PidROS::set_parameter_event_callback() @@ -500,6 +556,11 @@ void PidROS::set_parameter_event_callback() gains.d_gain_ = parameter.get_value(); changed = true; } + else if (param_name == param_prefix_ + "derivative_filter_time") + { + gains.tf_ = parameter.get_value(); + changed = true; + } else if (param_name == param_prefix_ + "i_clamp_max") { gains.i_max_ = parameter.get_value(); diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 16d97456..cb40ef42 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -22,6 +22,7 @@ #include "rclcpp/utilities.hpp" using control_toolbox::AntiWindupStrategy; +using control_toolbox::DiscretizationMethod; using rclcpp::executors::MultiThreadedExecutor; class TestablePidROS : public control_toolbox::PidROS @@ -49,6 +50,7 @@ void check_set_parameters( const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 0.5; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -60,9 +62,12 @@ void check_set_parameters( ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; const bool SAVE_I_TERM = true; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, SAVE_I_TERM)); + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, SAVE_I_TERM)); rclcpp::Parameter param; @@ -76,6 +81,9 @@ void check_set_parameters( ASSERT_TRUE(node->get_parameter(prefix + "d", param)); ASSERT_EQ(param.get_value(), D); + ASSERT_TRUE(node->get_parameter(prefix + "derivative_filter_time", param)); + ASSERT_EQ(param.get_value(), TF); + ASSERT_TRUE(node->get_parameter(prefix + "i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); @@ -97,6 +105,12 @@ void check_set_parameters( ASSERT_TRUE(node->get_parameter(prefix + "antiwindup_strategy", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter(prefix + "integration_method", param)); + ASSERT_EQ(param.get_value(), I_METHOD.to_string()); + + ASSERT_TRUE(node->get_parameter(prefix + "derivative_method", param)); + ASSERT_EQ(param.get_value(), D_METHOD.to_string()); + ASSERT_TRUE(node->get_parameter(prefix + "save_i_term", param)); ASSERT_EQ(param.get_value(), SAVE_I_TERM); @@ -105,12 +119,15 @@ void check_set_parameters( ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); + ASSERT_EQ(gains.tf_, TF); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, I_METHOD); + ASSERT_EQ(gains.d_method_, D_METHOD); } TEST(PidParametersTest, InitPid_no_prefix) @@ -134,6 +151,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX_BAD = -10.0; const double I_MIN_BAD = 10.0; const double U_MAX_BAD = -10.0; @@ -146,7 +164,11 @@ TEST(PidParametersTest, InitPidTestBadParameter) ANTIWINDUP_STRAT.i_min = I_MIN_BAD; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_NO_THROW(pid.initialize_from_args(P, I, D, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, false)); + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; + + ASSERT_NO_THROW(pid.initialize_from_args( + P, I, D, TF, U_MAX_BAD, U_MIN_BAD, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); rclcpp::Parameter param; @@ -154,6 +176,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("p", param)); ASSERT_FALSE(node->get_parameter("i", param)); ASSERT_FALSE(node->get_parameter("d", param)); + ASSERT_FALSE(node->get_parameter("derivative_filter_time", param)); ASSERT_FALSE(node->get_parameter("i_clamp_max", param)); ASSERT_FALSE(node->get_parameter("i_clamp_min", param)); ASSERT_FALSE(node->get_parameter("u_clamp_max", param)); @@ -161,18 +184,23 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("tracking_time_constant", param)); ASSERT_FALSE(node->get_parameter("saturation", param)); ASSERT_FALSE(node->get_parameter("antiwindup_strategy", param)); + ASSERT_FALSE(node->get_parameter("integration_method", param)); + ASSERT_FALSE(node->get_parameter("derivative_method", param)); // check gains were NOT set control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, 0.0); ASSERT_EQ(gains.i_gain_, 0.0); ASSERT_EQ(gains.d_gain_, 0.0); + ASSERT_EQ(gains.tf_, 0.0); ASSERT_EQ(gains.i_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.i_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, 0.0); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, DiscretizationMethod::FORWARD_EULER); + ASSERT_EQ(gains.d_method_, DiscretizationMethod::FORWARD_EULER); } TEST(PidParametersTest, InitPid_param_prefix_only) @@ -229,6 +257,7 @@ TEST(PidParametersTest, SetParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -240,9 +269,12 @@ TEST(PidParametersTest, SetParametersTest) ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; const bool SAVE_I_TERM = false; - pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, SAVE_I_TERM); + pid.initialize_from_args( + P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, SAVE_I_TERM); rcl_interfaces::msg::SetParametersResult set_result; @@ -257,6 +289,9 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("d", D))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("derivative_filter_time", TF))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_max", I_MAX))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN))); @@ -273,6 +308,12 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_NO_THROW( set_result = node->set_parameter(rclcpp::Parameter("antiwindup_strategy", ANTIWINDUP_STRAT))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("integration_method", I_METHOD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("derivative_method", D_METHOD))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_i_term", SAVE_I_TERM))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW( @@ -287,12 +328,15 @@ TEST(PidParametersTest, SetParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); + ASSERT_EQ(gains.tf_, TF); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, U_MAX); ASSERT_EQ(gains.u_min_, U_MIN); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, I_METHOD); + ASSERT_EQ(gains.d_method_, D_METHOD); } TEST(PidParametersTest, SetBadParametersTest) @@ -304,6 +348,7 @@ TEST(PidParametersTest, SetBadParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double I_MAX_BAD = -20.0; @@ -321,7 +366,10 @@ TEST(PidParametersTest, SetBadParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false); + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; + + pid.initialize_from_args(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false); rcl_interfaces::msg::SetParametersResult set_result; @@ -336,6 +384,9 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("d", D))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("derivative_filter_time", TF))); + ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_max", I_MAX_BAD))); ASSERT_TRUE(set_result.successful); ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("i_clamp_min", I_MIN_BAD))); @@ -352,6 +403,12 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_NO_THROW( set_result = node->set_parameter(rclcpp::Parameter("antiwindup_strategy", ANTIWINDUP_STRAT))); ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("integration_method", I_METHOD))); + ASSERT_TRUE(set_result.successful); + ASSERT_NO_THROW( + set_result = node->set_parameter(rclcpp::Parameter("derivative_method", D_METHOD))); + ASSERT_TRUE(set_result.successful); // process callbacks rclcpp::spin_some(node->get_node_base_interface()); @@ -362,12 +419,15 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); + ASSERT_EQ(gains.tf_, TF); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, I_METHOD); + ASSERT_EQ(gains.d_method_, D_METHOD); // Set the good gains @@ -384,12 +444,15 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); + ASSERT_EQ(gains.tf_, TF); ASSERT_EQ(gains.i_max_, I_MAX); ASSERT_EQ(gains.i_min_, I_MIN); ASSERT_EQ(gains.u_max_, std::numeric_limits::infinity()); ASSERT_EQ(gains.u_min_, -std::numeric_limits::infinity()); ASSERT_EQ(gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(gains.i_method_, I_METHOD); + ASSERT_EQ(gains.d_method_, D_METHOD); // Now re-enabling it should have the old gains back ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("saturation", true))); @@ -403,12 +466,15 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_EQ(updated_gains.p_gain_, P); ASSERT_EQ(updated_gains.i_gain_, I); ASSERT_EQ(updated_gains.d_gain_, D); + ASSERT_EQ(updated_gains.tf_, TF); ASSERT_EQ(updated_gains.i_max_, I_MAX); ASSERT_EQ(updated_gains.i_min_, I_MIN); ASSERT_EQ(updated_gains.u_max_, U_MAX); ASSERT_EQ(updated_gains.u_min_, U_MIN); ASSERT_EQ(updated_gains.antiwindup_strat_.tracking_time_constant, TRK_TC); ASSERT_EQ(updated_gains.antiwindup_strat_, AntiWindupStrategy::NONE); + ASSERT_EQ(updated_gains.i_method_, I_METHOD); + ASSERT_EQ(updated_gains.d_method_, D_METHOD); } TEST(PidParametersTest, GetParametersTest) @@ -421,6 +487,7 @@ TEST(PidParametersTest, GetParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -434,11 +501,15 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_FALSE(pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)) - << "Zero u_min and u_max are not valid so initialization should fail"; - ASSERT_TRUE(pid.initialize_from_args(0, 0, 0, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; + + ASSERT_TRUE(pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); + ASSERT_TRUE(pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); std::cout << "Setting gains with set_gains()" << std::endl; - pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); + pid.set_gains(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD); rclcpp::Parameter param; @@ -451,6 +522,9 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("d", param)); ASSERT_EQ(param.get_value(), D); + ASSERT_TRUE(node->get_parameter("derivative_filter_time", param)); + ASSERT_EQ(param.get_value(), TF); + ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); @@ -472,6 +546,12 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter("integration_method", param)); + ASSERT_EQ(param.get_value(), I_METHOD.to_string()); + + ASSERT_TRUE(node->get_parameter("derivative_method", param)); + ASSERT_EQ(param.get_value(), D_METHOD.to_string()); + ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); @@ -486,6 +566,7 @@ TEST(PidParametersTest, GetParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -498,7 +579,11 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_TRUE(pid.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; + + ASSERT_TRUE(pid.initialize_from_args( + P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); rclcpp::Parameter param; ASSERT_TRUE(node->get_parameter("activate_state_publisher", param)); @@ -512,6 +597,7 @@ TEST(PidParametersTest, GetParametersTest) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = std::numeric_limits::infinity(); @@ -524,10 +610,14 @@ TEST(PidParametersTest, GetParametersTest) ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; - ASSERT_FALSE(pid.initialize_from_args(0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, false)) - << "Zero u_min and u_max are not valid so initialization should fail"; - ASSERT_TRUE(pid.initialize_from_args(0, 0, 0, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); - pid.set_gains(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT); + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; + + ASSERT_TRUE(pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); + ASSERT_TRUE(pid.initialize_from_args( + 0.0, 0.0, 0.0, 0.0, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); + pid.set_gains(P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD); rclcpp::Parameter param; @@ -540,6 +630,9 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("d", param)); ASSERT_EQ(param.get_value(), D); + ASSERT_TRUE(node->get_parameter("derivative_filter_time", param)); + ASSERT_EQ(param.get_value(), TF); + ASSERT_TRUE(node->get_parameter("i_clamp_max", param)); ASSERT_EQ(param.get_value(), I_MAX); @@ -561,6 +654,12 @@ TEST(PidParametersTest, GetParametersTest) ASSERT_TRUE(node->get_parameter("antiwindup_strategy", param)); ASSERT_EQ(param.get_value(), ANTIWINDUP_STRAT.to_string()); + ASSERT_TRUE(node->get_parameter("integration_method", param)); + ASSERT_EQ(param.get_value(), I_METHOD.to_string()); + + ASSERT_TRUE(node->get_parameter("derivative_method", param)); + ASSERT_EQ(param.get_value(), D_METHOD.to_string()); + ASSERT_TRUE(node->get_parameter("save_i_term", param)); ASSERT_EQ(param.get_value(), false); } @@ -586,6 +685,10 @@ TEST(PidParametersTest, GetParametersFromParams) ASSERT_TRUE(node->get_parameter("d", param_d)); EXPECT_TRUE(std::isnan(param_d.get_value())); + rclcpp::Parameter param_tf; + ASSERT_TRUE(node->get_parameter("derivative_filter_time", param_tf)); + EXPECT_TRUE(std::isnan(param_tf.get_value())); + rclcpp::Parameter param_i_clamp_max; ASSERT_TRUE(node->get_parameter("i_clamp_max", param_i_clamp_max)); EXPECT_TRUE(std::isinf(param_i_clamp_max.get_value())); @@ -875,6 +978,7 @@ TEST(PidParametersTest, MultiplePidInstances) const double P = 1.0; const double I = 2.0; const double D = 3.0; + const double TF = 4.0; const double I_MAX = 10.0; const double I_MIN = -10.0; const double U_MAX = 10.0; @@ -885,9 +989,13 @@ TEST(PidParametersTest, MultiplePidInstances) ANTIWINDUP_STRAT.i_max = I_MAX; ANTIWINDUP_STRAT.i_min = I_MIN; ANTIWINDUP_STRAT.tracking_time_constant = TRK_TC; + DiscretizationMethod I_METHOD{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod D_METHOD{DiscretizationMethod::FORWARD_EULER}; - ASSERT_NO_THROW(pid_1.initialize_from_args(P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); - ASSERT_NO_THROW(pid_2.initialize_from_args(2 * P, I, D, U_MAX, U_MIN, ANTIWINDUP_STRAT, false)); + ASSERT_NO_THROW(pid_1.initialize_from_args( + P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); + ASSERT_NO_THROW(pid_2.initialize_from_args( + 2 * P, I, D, TF, U_MAX, U_MIN, ANTIWINDUP_STRAT, I_METHOD, D_METHOD, false)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 7ae6d146..a3ecd308 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -28,6 +28,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" using control_toolbox::AntiWindupStrategy; +using control_toolbox::DiscretizationMethod; using PidStateMsg = control_msgs::msg::PidState; using rclcpp::executors::MultiThreadedExecutor; @@ -45,7 +46,12 @@ TEST(PidPublisherTest, PublishTest) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -89,7 +95,12 @@ TEST(PidPublisherTest, PublishTest_start_deactivated) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -165,7 +176,12 @@ TEST(PidPublisherTest, PublishTest_prefix) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -209,7 +225,12 @@ TEST(PidPublisherTest, PublishTest_local_prefix) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -313,7 +334,12 @@ TEST(PidPublisherTest, PublishTestLifecycle) antiwindup_strat.i_max = 5.0; antiwindup_strat.i_min = -5.0; antiwindup_strat.tracking_time_constant = 1.0; - pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, false); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + pid_ros.initialize_from_args( + 1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; diff --git a/control_toolbox/test/pid_tests.cpp b/control_toolbox/test/pid_tests.cpp index 5629cd5f..ef9b850f 100644 --- a/control_toolbox/test/pid_tests.cpp +++ b/control_toolbox/test/pid_tests.cpp @@ -39,6 +39,7 @@ #include "gmock/gmock.h" using control_toolbox::AntiWindupStrategy; +using control_toolbox::DiscretizationMethod; using control_toolbox::Pid; using namespace std::chrono_literals; @@ -48,13 +49,17 @@ TEST(ParameterTest, UTermBadIBoundsTestConstructor) "description", "This test checks if an error is thrown for bad u_bounds specification (i.e. u_min > u_max)."); - // Pid(double p, double i, double d, double u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; - EXPECT_THROW(Pid pid(1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat), std::invalid_argument); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + EXPECT_THROW( + Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat, i_method, d_method), + std::invalid_argument); } TEST(ParameterTest, UTermBadIBoundsTest) @@ -63,18 +68,21 @@ TEST(ParameterTest, UTermBadIBoundsTest) "description", "This test checks if gains remain for bad u_bounds specification (i.e. u_min > u_max)."); - // Pid(double p, double i, double d, double u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, i_method, d_method); auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); // Try to set bad u-bounds, i.e. u_min > u_max - EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat)); + EXPECT_NO_THROW( + pid.set_gains(1.0, 1.0, 1.0, 1.0, -1.0, 1.0, antiwindup_strat, i_method, d_method)); // Check if gains were not updated because u-bounds are bad, i.e. u_min > u_max EXPECT_DOUBLE_EQ(gains.u_max_, 1.0); EXPECT_DOUBLE_EQ(gains.u_min_, -1.0); @@ -85,13 +93,15 @@ TEST(ParameterTest, outputClampTest) RecordProperty( "description", "This test succeeds if the output is clamped when the saturation is active."); - // Pid(double p, double i, double d, double u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Setting u_max = 1.0 and u_min = -1.0 to test clamping - Pid pid(1.0, 0.0, 0.0, 1.0, -1.0, antiwindup_strat); + Pid pid(1.0, 0.0, 0.0, 1.0, 1.0, -1.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; @@ -141,15 +151,17 @@ TEST(ParameterTest, noOutputClampTest) RecordProperty( "description", "This test succeeds if the output isn't clamped when the saturation is false."); - // Pid(double p, double i, double d, double u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Setting u_max = INF and u_min = -INF to disable clamping Pid pid( - 1.0, 0.0, 0.0, std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), antiwindup_strat); + 1.0, 0.0, 0.0, 1.0, std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), antiwindup_strat, i_method, d_method); double cmd = 0.0; @@ -201,14 +213,16 @@ TEST(ParameterTest, integrationBackCalculationZeroGainTest) "This test succeeds if the integral contribution is clamped when the integral gain is zero for " "the back calculation technique."); - // Pid(double p, double i, double d, double u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; antiwindup_strat.tracking_time_constant = 0.0; // Set to 0.0 to use the default value - Pid pid(0.0, 0.0, 0.0, 20.0, -20.0, antiwindup_strat); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 0.0, 0.0, 1.0, 20.0, -20.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -253,13 +267,15 @@ TEST(ParameterTest, integrationConditionalIntegrationZeroGainTest) "This test succeeds if the integral contribution is clamped when the integral gain is zero for " "the conditional integration technique."); - // Pid(double p, double i, double d, double u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; - Pid pid(0.0, 0.0, 0.0, 20.0, -20.0, antiwindup_strat); + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 0.0, 0.0, 1.0, 20.0, -20.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; double pe, ie, de; @@ -308,14 +324,18 @@ TEST(ParameterTest, ITermBadIBoundsTest) antiwindup_strat.i_max = 1.0; antiwindup_strat.i_min = -1.0; - Pid pid(1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat); + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, i_method, d_method); auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); // Try to set bad i-bounds, i.e. i_min > i_max antiwindup_strat.i_max = -1.0; antiwindup_strat.i_min = 1.0; - EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat)); + EXPECT_NO_THROW( + pid.set_gains(1.0, 1.0, 1.0, 1.0, 1.0, -1.0, antiwindup_strat, i_method, d_method)); // Check if gains were not updated because i-bounds are bad, i.e. i_min > i_max EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_max, 1.0); EXPECT_DOUBLE_EQ(gains.antiwindup_strat_.i_min, -1.0); @@ -338,7 +358,10 @@ TEST(ParameterTest, integrationAntiwindupTest) const double u_max = std::numeric_limits::infinity(); const double u_min = -std::numeric_limits::infinity(); - Pid pid(0.0, i_gain, 0.0, u_max, u_min, antiwindup_strat); + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, i_gain, 0.0, 1.0, u_max, u_min, antiwindup_strat, i_method, d_method); double cmd = 0.0; @@ -366,6 +389,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double p_gain = std::rand() % 100; double i_gain = std::rand() % 100; double d_gain = std::rand() % 100; + double tf = std::rand() % 100; double i_max = std::rand() % 100; double i_min = -1 * std::rand() % 100; double u_max = std::numeric_limits::infinity(); @@ -376,27 +400,33 @@ TEST(ParameterTest, gainSettingCopyPIDTest) antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.tracking_time_constant = tracking_time_constant; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; // Initialize the default way - Pid pid1(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); + Pid pid1(p_gain, i_gain, d_gain, tf, u_max, u_min, antiwindup_strat, i_method, d_method); // Test return values ------------------------------------------------- - double p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return; + double p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return; AntiWindupStrategy antiwindup_strat_return; + DiscretizationMethod i_method_return, d_method_return; pid1.get_gains( - p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, - antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return, + antiwindup_strat_return, i_method_return, d_method_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); + EXPECT_EQ(tf, tf_return); EXPECT_EQ(u_max, u_max_return); EXPECT_EQ(u_min, u_min_return); EXPECT_EQ(tracking_time_constant, antiwindup_strat_return.tracking_time_constant); EXPECT_EQ(i_min, antiwindup_strat_return.i_min); EXPECT_EQ(i_max, antiwindup_strat_return.i_max); EXPECT_EQ(antiwindup_strat.to_string(), antiwindup_strat_return.to_string()); + EXPECT_EQ(i_method, i_method_return); + EXPECT_EQ(d_method, d_method_return); // Test return values using struct ------------------------------------------------- @@ -404,6 +434,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) p_gain = std::rand() % 100; i_gain = std::rand() % 100; d_gain = std::rand() % 100; + tf = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; u_max = std::numeric_limits::infinity(); @@ -413,13 +444,16 @@ TEST(ParameterTest, gainSettingCopyPIDTest) antiwindup_strat.i_max = i_max; antiwindup_strat.i_min = i_min; antiwindup_strat.tracking_time_constant = tracking_time_constant; + i_method = DiscretizationMethod::BACKWARD_EULER; + d_method = DiscretizationMethod::BACKWARD_EULER; - pid1.set_gains(p_gain, i_gain, d_gain, u_max, u_min, antiwindup_strat); + pid1.set_gains(p_gain, i_gain, d_gain, tf, u_max, u_min, antiwindup_strat, i_method, d_method); Pid::Gains g1 = pid1.get_gains(); EXPECT_EQ(p_gain, g1.p_gain_); EXPECT_EQ(i_gain, g1.i_gain_); EXPECT_EQ(d_gain, g1.d_gain_); + EXPECT_EQ(tf, g1.tf_); EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); @@ -428,6 +462,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(i_max, g1.antiwindup_strat_.i_max); EXPECT_EQ(i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); + EXPECT_EQ(i_method, g1.i_method_); + EXPECT_EQ(d_method, g1.d_method_); // Send update command to populate errors ------------------------------------------------- pid1.set_current_cmd(10); @@ -437,12 +473,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid2(pid1); pid2.get_gains( - p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, - antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return, + antiwindup_strat_return, i_method_return, d_method_return); EXPECT_EQ(p_gain_return, g1.p_gain_); EXPECT_EQ(i_gain_return, g1.i_gain_); EXPECT_EQ(d_gain_return, g1.d_gain_); + EXPECT_EQ(tf_return, g1.tf_); EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_); EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); @@ -451,6 +488,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max); EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); + EXPECT_EQ(i_method_return, g1.i_method_); + EXPECT_EQ(d_method_return, g1.d_method_); // Test that errors are zero double pe2, ie2, de2; @@ -464,12 +503,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid3 = pid1; pid3.get_gains( - p_gain_return, i_gain_return, d_gain_return, u_max_return, u_min_return, - antiwindup_strat_return); + p_gain_return, i_gain_return, d_gain_return, tf_return, u_max_return, u_min_return, + antiwindup_strat_return, i_method_return, d_method_return); EXPECT_EQ(p_gain_return, g1.p_gain_); EXPECT_EQ(i_gain_return, g1.i_gain_); EXPECT_EQ(d_gain_return, g1.d_gain_); + EXPECT_EQ(tf_return, g1.tf_); EXPECT_EQ(antiwindup_strat_return.i_max, g1.i_max_); EXPECT_EQ(antiwindup_strat_return.i_min, g1.i_min_); EXPECT_EQ(u_max, g1.u_max_); @@ -478,6 +518,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(antiwindup_strat_return.i_max, g1.antiwindup_strat_.i_max); EXPECT_EQ(antiwindup_strat_return.i_min, g1.antiwindup_strat_.i_min); EXPECT_EQ(antiwindup_strat.to_string(), g1.antiwindup_strat_.to_string()); + EXPECT_EQ(i_method_return, g1.i_method_); + EXPECT_EQ(d_method_return, g1.d_method_); // Test that errors are zero double pe3, ie3, de3; @@ -509,8 +551,11 @@ TEST(CommandTest, proportionalOnlyTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only proportional gain - Pid pid(1.0, 0.0, 0.0, 10.0, -10.0, antiwindup_strat); + Pid pid(1.0, 0.0, 0.0, 1.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 @@ -544,8 +589,11 @@ TEST(CommandTest, integralOnlyTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only integral gains with enough limits to test behavior - Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); + Pid pid(0.0, 1.0, 0.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 @@ -604,7 +652,147 @@ TEST(CommandTest, integralOnlyTest) EXPECT_EQ(-0.5, cmd); } -TEST(CommandTest, derivativeOnlyTest) +TEST(CommandTest, integralOnlyBackwardTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using the integral contribution only " + "with backward euler discretization (ATTENTION: this test depends on the integration scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + + DiscretizationMethod i_method{DiscretizationMethod::BACKWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + // Set only integral gains with enough limits to test behavior + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + double cmd = 0.0; + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); + + // If call again with same arguments + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Call again with no error + cmd = pid.compute_command(0.0, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Check that the integral contribution keep the previous command + cmd = pid.compute_command(0.0, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Finally call again with positive error to see if the command changes in the opposite direction + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(0.0, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(1.0, cmd); + + // after reset without argument (save_i_term=false) + // we expect the command to be 0 if update is called error = 0 + pid.reset(); + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.5, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = 0.5 + EXPECT_EQ(0.5, cmd); + + // after reset with argument (save_i_term=false) + pid.reset(false); + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.5, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); + // after reset with save_i_term=true + // we expect still the same command if update is called error = 0 + pid.reset(true); + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); +} + +TEST(CommandTest, integralOnlyTrapezoidalTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using the integral contribution only " + "with trapezoidal discretization (ATTENTION: this test depends on the integration scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + + DiscretizationMethod i_method{DiscretizationMethod::TRAPEZOIDAL}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + // Set only integral gains with enough limits to test behavior + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + double cmd = 0.0; + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Since trapezoidal integration is used, we expect the mean value of the error over + // this and the last period, which is -0.25 + EXPECT_EQ(-0.25, cmd); + + // If call again with same arguments + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.75, cmd); + + // Call again with no error + cmd = pid.compute_command(0.0, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Check that the integral contribution keep the previous command + cmd = pid.compute_command(0.0, 1.0); + EXPECT_EQ(-1.0, cmd); + + // Finally call again with positive error to see if the command changes in the opposite direction + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(-0.5, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(1.0, 1.0); + EXPECT_EQ(0.5, cmd); + + // after reset without argument (save_i_term=false) + pid.reset(); + cmd = pid.compute_command(0.5, 1.0); + EXPECT_EQ(0.25, cmd); + + // If initial error = 0, i-gain = 1, dt = 1 + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = 0.5 + EXPECT_EQ(0.5, cmd); + + // after reset with argument (save_i_term=false) + pid.reset(false); + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_EQ(-0.25, cmd); + + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); + + // after reset with save_i_term=true + // we expect still the same command if update is called error = 0 + pid.reset(true); + cmd = pid.compute_command(0.0, 1.0); + // Then expect command = -0.5 + EXPECT_EQ(-0.5, cmd); +} + +TEST(CommandTest, derivativeOnlyForwardTest) { RecordProperty( "description", @@ -614,8 +802,11 @@ TEST(CommandTest, derivativeOnlyTest) AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + // Set only derivative gain - Pid pid(0.0, 0.0, 1.0, 10.0, -10.0, antiwindup_strat); + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 @@ -633,148 +824,534 @@ TEST(CommandTest, derivativeOnlyTest) // Then expect command = 0 again EXPECT_EQ(0.0, cmd); - // If the error increases, with dt = 1 + // If the error increases in module, with dt = 1 cmd = pid.compute_command(-1.0, 1.0); // Then expect the command = change in dt EXPECT_EQ(-0.5, cmd); - // If error decreases, with dt = 1 + // If error decreases in module, with dt = 1 cmd = pid.compute_command(-0.5, 1.0); // Then expect always the command = change in dt (note the sign flip) EXPECT_EQ(0.5, cmd); } -TEST(CommandTest, completePIDTest) +TEST(CommandTest, derivativeOnlyBackwardTest) { RecordProperty( "description", - "This test checks that a command is computed correctly using a complete PID controller " - "(ATTENTION: this test depends on the integral and differentiation schemes)."); + "This test checks that a command is computed correctly using the derivative contribution only " + "with own differentiation (ATTENTION: this test depends on the differentiation scheme)."); AntiWindupStrategy antiwindup_strat; antiwindup_strat.type = AntiWindupStrategy::NONE; - antiwindup_strat.i_max = 10.0; - antiwindup_strat.i_min = -10.0; - Pid pid(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::BACKWARD_EULER}; + + // Set only derivative gain + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; - // All contributions are tested, here few tests check that they sum up correctly - // If initial error = 0, all gains = 1, dt = 1 + // If initial error = 0, d-gain = 1, dt = 1 cmd = pid.compute_command(-0.5, 1.0); - // Then expect command = -1.0 - EXPECT_EQ(-1.0, cmd); + // Then expect command = error + EXPECT_EQ(-0.5, cmd); - // If call again with same arguments, no error change, but integration do its part + // If call again with same error cmd = pid.compute_command(-0.5, 1.0); - // Then expect command = -1.0 - EXPECT_EQ(-1.0, cmd); + // Then expect command = 0 due to no variation on error + EXPECT_EQ(0.0, cmd); - // If call again increasing the error - cmd = pid.compute_command(0.0, 1.0); - // Then expect command equals to p = 0, i = -1.0 (i.e. - 0.5 - 0.5), d = 0.5, cmd = -0.5 + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + // Then expect command = 0 again + EXPECT_EQ(0.0, cmd); + + // If the error increases in module, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + // Then expect the command = change in dt EXPECT_EQ(-0.5, cmd); + + // If error decreases in module, with dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect always the command = change in dt (note the sign flip) + EXPECT_EQ(0.5, cmd); } -TEST(CommandTest, backCalculationPIDTest) +TEST(CommandTest, derivativeOnlyTrapezoidalTest) { RecordProperty( "description", - "This test checks that a command is computed correctly using a complete PID controller with " - "back calculation technique."); + "This test checks that a command is computed correctly using the derivative contribution only " + "with own differentiation (ATTENTION: this test depends on the differentiation scheme)."); - // Pid(double p, double i, double d, double u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); - // Setting u_max = 5.0 and u_min = -5.0 to test clamping AntiWindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; - antiwindup_strat.i_max = 10.0; - antiwindup_strat.i_min = -10.0; - antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value - Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); + antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::TRAPEZOIDAL}; + + // Set only derivative gain + Pid pid(0.0, 0.0, 1.0, 0.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; - double pe, ie, de; - // Small error to not have saturation - cmd = pid.compute_command(1.0, 1.0); - pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(1.0, ie); - EXPECT_EQ(0.0, cmd); + // If initial error = 0, d-gain = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = error + EXPECT_EQ(-1.0, cmd); - // Small error to not have saturation - cmd = pid.compute_command(2.0, 1.0); - pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(3.0, ie); + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = 0 due to no variation on error EXPECT_EQ(1.0, cmd); - // Error to cause saturation - cmd = pid.compute_command(3.0, 1.0); - pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(6.0, ie); - EXPECT_EQ(3.0, cmd); - - // Saturation applied, back calculation now reduces the integral term - cmd = pid.compute_command(1.0, 1.0); - pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(6.0, ie); - EXPECT_EQ(5.0, cmd); - - // Saturation applied, back calculation now reduces the integral term - cmd = pid.compute_command(2.0, 1.0); - pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(7.0, ie); - EXPECT_EQ(5.0, cmd); + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + // Then expect command = 0 again + EXPECT_EQ(-1.0, cmd); - // Saturation applied, back calculation now reduces the integral term + // If the error increases in module, with dt = 1 cmd = pid.compute_command(-1.0, 1.0); - pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(4.0, ie); - EXPECT_EQ(5.0, cmd); + // Then expect the command = change in dt + EXPECT_EQ(0.0, cmd); - // PID recover from the windup/saturation - cmd = pid.compute_command(1.0, 1.0); - pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(5.0, ie); - EXPECT_EQ(4.0, cmd); + // If error decreases in module, with dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect always the command = change in dt (note the sign flip) + EXPECT_EQ(1.0, cmd); } -TEST(CommandTest, conditionalIntegrationPIDTest) +TEST(CommandTest, derivativeFilteredForwardTest) { RecordProperty( "description", - "This test checks that a command is computed correctly using a complete PID controller with " - "conditional integration technique."); + "This test verifies that a command is computed correctly using only the filtered derivative " + "contribution with its own differentiation (NOTE: this test depends on the differentiation " + "scheme)."); - // Pid(double p, double i, double d, double u_max, double u_min, - // AntiWindupStrategy antiwindup_strat); - // Setting u_max = 5.0 and u_min = -5.0 to test clamping AntiWindupStrategy antiwindup_strat; - antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; - antiwindup_strat.i_max = 10.0; - antiwindup_strat.i_min = -10.0; - antiwindup_strat.tracking_time_constant = 1.0; - Pid pid(0.0, 1.0, 0.0, 5.0, -5.0, antiwindup_strat); + antiwindup_strat.type = AntiWindupStrategy::NONE; + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + // Set only derivative gain and derivative filter time (tf) + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); double cmd = 0.0; - double pe, ie, de; - // Small error to not have saturation - cmd = pid.compute_command(1.0, 1.0); - pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(1.0, ie); - EXPECT_EQ(0.0, cmd); + // If initial error = 0, d-gain = 1, tf = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-0.25, cmd, EPS); - // Small error to not have saturation - cmd = pid.compute_command(2.0, 1.0); + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-0.125, cmd, EPS); + + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + EXPECT_NEAR(-0.11875, cmd, EPS); + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + EXPECT_NEAR(-0.309375, cmd, EPS); + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-2.090625, 1.0); + EXPECT_NEAR(-0.7, cmd, EPS); + + // If error increases, with dt = 1 + cmd = pid.compute_command(0.0, 1.0); + EXPECT_NEAR(0.6953125, cmd, EPS); + + // If error increases, with dt = 1 + cmd = pid.compute_command(3.0, 1.0); + EXPECT_NEAR(1.84765625, cmd, EPS); + + // If error increases, with dt = 1 + cmd = pid.compute_command(5.15234375, 1.0); + EXPECT_NEAR(2, cmd, EPS); +} + +TEST(CommandTest, derivativeFilteredBackwardTest) +{ + RecordProperty( + "description", + "This test verifies that a command is computed correctly using only the filtered derivative " + "contribution with its own differentiation (NOTE: this test depends on the differentiation " + "scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::BACKWARD_EULER}; + + // Set only derivative gain and derivative filter time (tf) + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); + double cmd = 0.0; + + // If initial error = 0, d-gain = 1, tf = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-1.0 / 6.0, cmd, EPS); // -0.1666666666 + + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-1.0 / 9.0, cmd, EPS); // -0.1111111111 + + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + EXPECT_NEAR(-20.0 / 189.0, cmd, EPS); // -0.1058201058 + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + EXPECT_NEAR(-269.0 / 1134.0, cmd, EPS); // -0.2372134038 + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-2.0, 1.0); + EXPECT_NEAR(-836.0 / 1701.0, cmd, EPS); // -0.4914756025 + + // If error increases, with dt = 1 + cmd = pid.compute_command(0.0, 1.0); + EXPECT_NEAR(1730.0 / 5103.0, cmd, EPS); // 0.3390162649 + + // If error increases, with dt = 1 + cmd = pid.compute_command(3.0, 1.0); + EXPECT_NEAR(18769.0 / 15309.0, cmd, EPS); // 1.2260108432 + + // If error increases, with dt = 1 + cmd = pid.compute_command(100243.0 / 15309.0, 1.0); // 6.5479783134 + EXPECT_NEAR(2, cmd, EPS); +} + +TEST(CommandTest, derivativeFilteredTrapezoidalTest) +{ + RecordProperty( + "description", + "This test verifies that a command is computed correctly using only the filtered derivative " + "contribution with its own differentiation (NOTE: this test depends on the differentiation " + "scheme)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::TRAPEZOIDAL}; + + // Set only derivative gain and derivative filter time (tf) + Pid pid(0.0, 0.0, 1.0, 2.0, 10.0, -10.0, antiwindup_strat, i_method, d_method); + double cmd = 0.0; + + // If initial error = 0, d-gain = 1, tf = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-1.0 / 5.0, cmd, EPS); // -0.2 + + // If call again with same error + cmd = pid.compute_command(-0.5, 1.0); + EXPECT_NEAR(-3.0 / 25.0, cmd, EPS); // -0.12 + + // If call again with same error and smaller control period + cmd = pid.compute_command(-0.5, 0.1); + EXPECT_NEAR(-117.0 / 1025.0, cmd, EPS); // -0.114146341 + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-1.0, 1.0); + EXPECT_NEAR(-1376.0 / 5125.0, cmd, EPS); // -0.268487805 + + // If the error becomes more negative, with dt = 1 + cmd = pid.compute_command(-2.0, 1.0); + EXPECT_NEAR(-14378.0 / 25625.0, cmd, EPS); // -0.561092683 + + // If error increases, with dt = 1 + cmd = pid.compute_command(0.0, 1.0); + EXPECT_NEAR(59366.0 / 128125.0, cmd, EPS); // 0.46334439 + + // If error increases, with dt = 1 + cmd = pid.compute_command(3.0, 1.0); + EXPECT_NEAR(946848.0 / 640625.0, cmd, EPS); // 1.478006634 + + // If error increases, with dt = 1 + cmd = pid.compute_command(4206331.0 / 1281250.0, 1.0); // 3.282990049 + EXPECT_NEAR(1, cmd, EPS); +} + +TEST(CommandTest, completePIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller " + "(ATTENTION: this test depends on the integral and differentiation schemes)."); + + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::NONE; + antiwindup_strat.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + double cmd = 0.0; + + // All contributions are tested, here few tests check that they sum up correctly + // If initial error = 0, all gains = 1, dt = 1 + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = -1.0 + EXPECT_EQ(-1.0, cmd); + + // If call again with same arguments, no error change, but integration do its part + cmd = pid.compute_command(-0.5, 1.0); + // Then expect command = -1.0 + EXPECT_EQ(-1.0, cmd); + + // If call again increasing the error + cmd = pid.compute_command(0.0, 1.0); + // Then expect command equals to p = 0, i = -1.0 (i.e. - 0.5 - 0.5), d = 0.5, cmd = -0.5 + EXPECT_EQ(-0.5, cmd); +} + +TEST(CommandTest, backCalculationForwardPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a PID controller with " + "back calculation technique and forward discretization."); + + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; + antiwindup_strat.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + // Since default discretization is forward Euler, the integral term is not updated at + // the first call (last error = 0) + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); pid.get_current_pid_errors(pe, ie, de); EXPECT_EQ(3.0, ie); + EXPECT_EQ(3.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.0, ie); + EXPECT_EQ(4.0, cmd); +} + +TEST(CommandTest, backCalculationBackwardPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a PID controller with " + "back calculation technique and backward discretization."); + + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; + antiwindup_strat.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value + + DiscretizationMethod i_method{DiscretizationMethod::BACKWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); EXPECT_EQ(1.0, cmd); + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(3.0, cmd); + // Error to cause saturation cmd = pid.compute_command(3.0, 1.0); pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.5, ie); // Reduced from 6.0 (1.0 + 2.0 + 3.0) to 5.5 due to back-calculation + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.75, ie); // Reduced from 6.5 (5.5 + 1.0) to 5.75 due to back-calculation + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.375, ie); // Reduced from 7.75 (5.75 + 2.0) to 6.375 due to back-calculation + EXPECT_EQ(5.0, cmd); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(5.1875, ie); // Reduced from 5.375 (6.375 - 1.0) to 5.1875 due to back-calculation + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.1875, ie); + EXPECT_EQ(4.1875, cmd); +} + +TEST(CommandTest, backCalculationTrapezoidalPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a PID controller with " + "back calculation technique and trapezoidal discretization."); + + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::BACK_CALCULATION; + antiwindup_strat.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; // Set to 0.0 to use the default value + + DiscretizationMethod i_method{DiscretizationMethod::TRAPEZOIDAL}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(0.5, ie, EPS); + EXPECT_NEAR(0.5, cmd, EPS); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(2.0, ie, EPS); + EXPECT_NEAR(2.0, cmd, EPS); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(4.5, ie, EPS); + EXPECT_NEAR(4.5, cmd, EPS); + + // Error to cause saturation + cmd = pid.compute_command(5.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(22.0 / 3.0, ie, EPS); // 7.33... + EXPECT_NEAR(5.0, cmd, EPS); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(70.0 / 9.0, ie, EPS); // 7.77... + EXPECT_NEAR(5.0, cmd, EPS); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(187.0 / 27.0, ie, EPS); // 6.92592592 + EXPECT_NEAR(5.0, cmd, EPS); + + // Saturation applied, back calculation now reduces the integral term + cmd = pid.compute_command(-2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(457.0 / 81.0, ie, EPS); // 5.12962963 + EXPECT_NEAR(5.0, cmd, EPS); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_NEAR(1909.0 / 486.0, ie, EPS); // 3.92798353 + EXPECT_NEAR(671.0 / 162.0, cmd, EPS); // 4.14197531 +} + +TEST(CommandTest, conditionalIntegrationForwardPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditional integration technique and forward euler discretization."); + + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; + antiwindup_strat.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; + + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + // Since default discretization is forward Euler, the integral term is not updated + // at the first call (last error = 0) + EXPECT_EQ(0.0, ie); + EXPECT_EQ(0.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(1.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); EXPECT_EQ(3.0, cmd); // Saturation applied, conditional integration now holds the integral term @@ -792,7 +1369,7 @@ TEST(CommandTest, conditionalIntegrationPIDTest) // PID recover from the windup/saturation cmd = pid.compute_command(-1.0, 1.0); pid.get_current_pid_errors(pe, ie, de); - EXPECT_EQ(5.0, ie); + EXPECT_EQ(6.0, ie); EXPECT_EQ(5.0, cmd); // PID recover from the windup/saturation @@ -802,6 +1379,148 @@ TEST(CommandTest, conditionalIntegrationPIDTest) EXPECT_EQ(5.0, cmd); } +TEST(CommandTest, conditionalIntegrationBackwardPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditional integration technique and backward euler discretization."); + + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; + antiwindup_strat.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; + + DiscretizationMethod i_method{DiscretizationMethod::BACKWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(1.0, ie); + EXPECT_EQ(1.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.0, ie); + EXPECT_EQ(3.0, cmd); + + // Error to cause saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.0, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-1.5, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.5, ie); + EXPECT_EQ(4.5, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(0.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.5, ie); + EXPECT_EQ(4.5, cmd); +} + +TEST(CommandTest, conditionalIntegrationTrapezoidalPIDTest) +{ + RecordProperty( + "description", + "This test checks that a command is computed correctly using a complete PID controller with " + "conditional integration technique and trapezoidal discretization."); + + // Setting u_max = 5.0 and u_min = -5.0 to test clamping + AntiWindupStrategy antiwindup_strat; + antiwindup_strat.type = AntiWindupStrategy::CONDITIONAL_INTEGRATION; + antiwindup_strat.i_max = 10.0; + antiwindup_strat.i_min = -10.0; + antiwindup_strat.tracking_time_constant = 1.0; + + DiscretizationMethod i_method{DiscretizationMethod::TRAPEZOIDAL}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid(0.0, 1.0, 0.0, 0.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + + double cmd = 0.0; + double pe, ie, de; + + // Small error to not have saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(0.5, ie); + EXPECT_EQ(0.5, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(2.0, ie); + EXPECT_EQ(2.0, cmd); + + // Small error to not have saturation + cmd = pid.compute_command(3.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.5, ie); + EXPECT_EQ(4.5, cmd); + + // Error to cause saturation + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.5, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.5, ie); + EXPECT_EQ(5.0, cmd); + + // Saturation applied, conditional integration now holds the integral term + cmd = pid.compute_command(2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.5, ie); + EXPECT_EQ(5.0, cmd); + + // PID recovering from the windup/saturation + cmd = pid.compute_command(-2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(6.5, ie); + EXPECT_EQ(5.0, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(-2.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(4.5, ie); + EXPECT_EQ(4.5, cmd); + + // PID recover from the windup/saturation + cmd = pid.compute_command(0.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); + EXPECT_EQ(3.5, ie); + EXPECT_EQ(3.5, cmd); +} + TEST(CommandTest, timeArgumentTest) { RecordProperty("description", "Tests different dt argument type methods."); @@ -812,10 +1531,13 @@ TEST(CommandTest, timeArgumentTest) antiwindup_strat.i_min = -10.0; antiwindup_strat.tracking_time_constant = 1.0; - Pid pid1(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); - Pid pid2(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); - Pid pid3(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); - Pid pid4(1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat); + DiscretizationMethod i_method{DiscretizationMethod::FORWARD_EULER}; + DiscretizationMethod d_method{DiscretizationMethod::FORWARD_EULER}; + + Pid pid1(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + Pid pid2(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + Pid pid3(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); + Pid pid4(1.0, 1.0, 1.0, 1.0, 5.0, -5.0, antiwindup_strat, i_method, d_method); // call without error_dt, dt is used to calculate error_dt auto cmd1 = pid1.compute_command(-0.5, 1.0);