diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp index 251767f..43cdb82 100644 --- a/include/control_toolbox/pid.hpp +++ b/include/control_toolbox/pid.hpp @@ -89,13 +89,13 @@ namespace control_toolbox \verbatim control_toolbox::Pid pid; - pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3); + pid.init_pid(6.0, 1.0, 2.0, 0.3, -0.3); double position_desi_ = 0.5; ... rclcpp::Time last_time = get_clock()->now(); while (true) { rclcpp::Time time = get_clock()->now(); - double effort = pid.computeCommand(position_desi_ - currentPosition(), (time - last_time).nanoseconds()); + double effort = pid.computeCommand(position_desi_ - currentPosition(), (time - last_time)); last_time = time; } \endverbatim @@ -197,7 +197,25 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min_ > i_max_ */ - void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + void init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + + /*! + * \brief Zeros out Pid values and initialize Pid-gains and integral term limits + * Does not initialize the node's parameter interface for PID gains + * + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced + * + * \note New gains are not applied if i_min_ > i_max_ + */ + [[deprecated("Use init_pid() instead")]] void initPid( + double p, double i, double d, double i_max, double i_min, bool antiwindup = false) { + init_pid(p, i, d, i_max, i_min, antiwindup); + } /*! * \brief Reset the state of this PID controller @@ -212,7 +230,21 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void getGains(double & p, double & i, double & d, double & i_max, double & i_min); + void get_gains(double & p, double & i, double & d, double & i_max, double & i_min); + + /*! + * \brief Get PID gains for the controller. + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + */ + [[deprecated("Use get_gains() instead")]] void getGains( + double & p, double & i, double & d, double & i_max, double & i_min) { + get_gains(p, i, d, i_max, i_min); + } + /*! * \brief Get PID gains for the controller. * \param p The proportional gain. @@ -222,14 +254,49 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \param i_min The min integral windup. * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced */ - void getGains( + void get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup); + /*! + * \brief Get PID gains for the controller. + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced + */ + [[deprecated("Use get_gains() instead")]] void getGains( + double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) { + get_gains(p, i, d, i_max, i_min, antiwindup); + } + + /*! + * \brief Get PID gains for the controller. + * \return gains A struct of the PID gain values + */ + Gains get_gains(); + /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values */ - Gains getGains(); + [[deprecated("Use get_gains() instead")]] Gains getGains() { + return get_gains(); + } + + /*! + * \brief Set PID gains for the controller. + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced + * + * \note New gains are not applied if i_min > i_max + */ + void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller. @@ -242,7 +309,18 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if i_min > i_max */ - void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + [[deprecated("Use set_gains() instead")]] void setGains( + double p, double i, double d, double i_max, double i_min, bool antiwindup = false) { + set_gains(p, i, d, i_max, i_min, antiwindup); + } + + /*! + * \brief Set PID gains for the controller. + * \param gains A struct of the PID gain values + * + * \note New gains are not applied if gains.i_min_ > gains.i_max_ + */ + void set_gains(const Gains & gains); /*! * \brief Set PID gains for the controller. @@ -250,7 +328,9 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \note New gains are not applied if gains.i_min_ > gains.i_max_ */ - void setGains(const Gains & gains); + [[deprecated("Use set_gains() instead")]] void setGains(const Gains & gains) { + set_gains(gains); + } /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -262,7 +342,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double dt_s); + [[nodiscard]] double compute_command(double error, double dt_s); /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -274,7 +354,24 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, uint64_t dt_ns); + [[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand( + double error, uint64_t dt_ns) { + return compute_command(error, static_cast(dt_ns) / 1.e9); + } + + /*! + * \brief Set the PID error and compute the PID command with nonuniform time + * step size. The derivative error is computed from the change in the error + * and the timestep \c dt. + * + * \param error Error since last call (error = target - state) + * \param dt Change in time since last call, measured in nanoseconds. + * + * \returns PID command + */ + [[nodiscard]] double compute_command(double error, rcl_duration_value_t dt_ns) { + return compute_command(error, static_cast(dt_ns)/1.e9); + } /*! * \brief Set the PID error and compute the PID command with nonuniform time @@ -286,8 +383,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, rcl_duration_value_t dt) { - return computeCommand(error, static_cast(dt)); + [[nodiscard]] double compute_command(double error, rclcpp::Duration dt) { + return compute_command(error, dt.seconds()); } /*! @@ -301,7 +398,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \returns PID command */ [[nodiscard]] double computeCommand(double error, std::chrono::nanoseconds dt_ns) { - return computeCommand(error, static_cast(dt_ns.count())); + return compute_command(error, static_cast(dt_ns.count())/1.e9); } /*! @@ -315,7 +412,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double error_dot, double dt_s); + [[nodiscard]] double compute_command(double error, double error_dot, double dt_s); /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -328,7 +425,10 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt_ns); + [[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand( + double error, double error_dot, uint64_t dt_ns) { + return compute_command(error, error_dot, static_cast(dt_ns) / 1.e9); + } /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -337,12 +437,12 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \param error Error since last call (error = target - state) * \param error_dot d(Error)/dt since last call - * \param dt Change in time since last call + * \param dt Change in time since last call, measured in nanoseconds. * * \returns PID command */ - [[nodiscard]] double computeCommand(double error, double error_dot, rcl_duration_value_t dt) { - return computeCommand(error, error_dot, static_cast(dt)); + [[nodiscard]] double compute_command(double error, double error_dot, rcl_duration_value_t dt_ns) { + return compute_command(error, error_dot, static_cast(dt_ns)/1.e9); } /*! @@ -356,25 +456,68 @@ class CONTROL_TOOLBOX_PUBLIC Pid * * \returns PID command */ - [[nodiscard]] double computeCommand( + [[nodiscard]] double compute_command(double error, double error_dot, rclcpp::Duration dt) { + return compute_command(error, error_dot, dt.seconds()); + } + + /*! + * \brief Set the PID error and compute the PID command with nonuniform + * time step size. This also allows the user to pass in a precomputed + * derivative error. + * + * \param error Error since last call (error = target - state) + * \param error_dot d(Error)/dt since last call + * \param dt Change in time since last call, measured in nanoseconds. + * + * \returns PID command + */ + [[nodiscard]] double compute_command( double error, double error_dot, std::chrono::nanoseconds dt_ns) { - return computeCommand(error, error_dot, static_cast(dt_ns.count())); + return compute_command(error, error_dot, static_cast(dt_ns.count())/1.e9); } /*! * \brief Set current command for this PID controller */ - void setCurrentCmd(double cmd); + void set_current_cmd(double cmd); + + /*! + * \brief Set current command for this PID controller + */ + [[deprecated("Use set_current_cmd() instead")]] void setCurrentCmd(double cmd) { + set_current_cmd(cmd); + } + + /*! + * \brief Return current command for this PID controller + */ + double get_current_cmd(); /*! * \brief Return current command for this PID controller */ - double getCurrentCmd(); + [[deprecated("Use get_current_cmd() instead")]] double getCurrentCmd() { + return get_current_cmd(); + } /*! * \brief Return derivative error */ - double getDerivativeError(); + double get_derivative_error(); + + /*! + * \brief Return derivative error + */ + [[deprecated("Use get_derivative_error() instead")]] + double getDerivativeError() { return get_derivative_error(); } + + /*! + * \brief Return PID error terms for the controller. + * \param pe The proportional error. + * \param ie The integral error. + * \param de The derivative error. + */ + void get_current_pid_errors(double & pe, double & ie, double & de); /*! * \brief Return PID error terms for the controller. @@ -382,7 +525,10 @@ class CONTROL_TOOLBOX_PUBLIC Pid * \param ie The integral error. * \param de The derivative error. */ - void getCurrentPIDErrors(double & pe, double & ie, double & de); + [[deprecated("Use get_current_pid_errors() instead")]] void getCurrentPIDErrors( + double & pe, double & ie, double & de) { + get_current_pid_errors(pe, ie, de); + } /*! * @brief Custom assignment operator diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp index cf55eb3..e58ad47 100644 --- a/include/control_toolbox/pid_ros.hpp +++ b/include/control_toolbox/pid_ros.hpp @@ -101,13 +101,37 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \note New gains are not applied if i_min_ > i_max_ */ - void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup); + void init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup); + + /*! + * \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 i_max The max integral windup. + * \param i_min The min integral windup. + * \param antiwindup antiwindup. + * + * \note New gains are not applied if i_min_ > i_max_ + */ + [[deprecated("Use init_pid() instead")]] void initPid( + double p, double i, double d, double i_max, double i_min, bool antiwindup) { + init_pid(p, i, d, i_max, i_min, antiwindup); + } + + /*! + * \brief Initialize the PID controller based on already set parameters + * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise + */ + bool init_pid(); /*! * \brief Initialize the PID controller based on already set parameters * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise */ - bool initPid(); + [[deprecated("Use init_pid() instead")]] bool initPid() { + return init_pid(); + } /*! * \brief Reset the state of this PID controller @@ -124,7 +148,22 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \returns PID command */ - double computeCommand(double error, rclcpp::Duration dt); + double compute_command(double error, rclcpp::Duration dt); + + /*! + * \brief Set the PID error and compute the PID command with nonuniform time + * step size. The derivative error is computed from the change in the error + * and the timestep \c dt. + * + * \param error Error since last call (error = target - state) + * \param dt Change in time since last call in seconds + * + * \returns PID command + */ + [[deprecated("Use compute_command() instead")]] double computeCommand( + double error, rclcpp::Duration dt) { + return compute_command(error, dt); + } /*! * \brief Set the PID error and compute the PID command with nonuniform @@ -137,13 +176,48 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \returns PID command */ - double computeCommand(double error, double error_dot, rclcpp::Duration dt); + double compute_command(double error, double error_dot, rclcpp::Duration dt); + + /*! + * \brief Set the PID error and compute the PID command with nonuniform + * time step size. This also allows the user to pass in a precomputed + * derivative error. + * + * \param error Error since last call (error = target - state) + * \param error_dot d(Error)/dt since last call + * \param dt Change in time since last call in seconds + * + * \returns PID command + */ + [[deprecated("Use compute_command() instead")]] double computeCommand( + double error, double error_dot, rclcpp::Duration dt) { + return compute_command(error, error_dot, dt); + } + + /*! + * \brief Get PID gains for the controller. + * \return gains A struct of the PID gain values + */ + Pid::Gains get_gains(); /*! * \brief Get PID gains for the controller. * \return gains A struct of the PID gain values */ - Pid::Gains getGains(); + [[deprecated("Use get_gains() instead")]] Pid::Gains getGains() { return get_gains(); } + + /*! + * \brief Set PID gains for the controller. + * \param p The proportional gain. + * \param i The integral gain. + * \param d The derivative gain. + * \param i_max The max integral windup. + * \param i_min The min integral windup. + * \param antiwindup antiwindup. + * + * \note New gains are not applied if i_min > i_max + */ + void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); /*! * \brief Set PID gains for the controller. @@ -156,7 +230,18 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \note New gains are not applied if i_min > i_max */ - void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); + [[deprecated("Use set_gains() instead")]] void setGains( + double p, double i, double d, double i_max, double i_min, bool antiwindup = false) { + set_gains(p, i, d, i_max, i_min, antiwindup); + } + + /*! + * \brief Set PID gains for the controller. + * \param gains A struct of the PID gain values + * + * \note New gains are not applied if gains.i_min_ > gains.i_max_ + */ + void set_gains(const Pid::Gains & gains); /*! * \brief Set PID gains for the controller. @@ -164,25 +249,60 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * * \note New gains are not applied if gains.i_min_ > gains.i_max_ */ - void setGains(const Pid::Gains & gains); + [[deprecated("Use set_gains() instead")]] void setGains(const Pid::Gains & gains) { + set_gains(gains); + } /*! * \brief Set current command for this PID controller * \param cmd command to set */ - void setCurrentCmd(double cmd); + void set_current_cmd(double cmd); + + /*! + * \brief Set current command for this PID controller + * \param cmd command to set + */ + [[deprecated("Use set_current_cmd() instead")]] void setCurrentCmd(double cmd) { + set_current_cmd(cmd); + } + + /*! + * \brief Return current command for this PID controller + * \return current cmd + */ + double get_current_cmd(); /*! * \brief Return current command for this PID controller * \return current cmd */ - double getCurrentCmd(); + [[deprecated("Use get_current_cmd() instead")]] double getCurrentCmd() { + return get_current_cmd(); + } /*! * \brief Return PID state publisher * \return shared_ptr to the PID state publisher */ - std::shared_ptr> getPidStatePublisher(); + std::shared_ptr> get_pid_state_publisher(); + + /*! + * \brief Return PID state publisher + * \return shared_ptr to the PID state publisher + */ + [[deprecated("Use get_pid_state_publisher() instead")]] + std::shared_ptr> getPidStatePublisher() { + return get_pid_state_publisher(); + } + + /*! + * \brief Return PID error terms for the controller. + * \param pe[out] The proportional error. + * \param ie[out] The integral error. + * \param de[out] The derivative error. + */ + void get_current_pid_errors(double & pe, double & ie, double & de); /*! * \brief Return PID error terms for the controller. @@ -190,37 +310,56 @@ class CONTROL_TOOLBOX_PUBLIC PidROS * \param ie[out] The integral error. * \param de[out] The derivative error. */ - void getCurrentPIDErrors(double & pe, double & ie, double & de); + [[deprecated("Use get_current_pid_errors() instead")]] void getCurrentPIDErrors( + double & pe, double & ie, double & de) { + get_current_pid_errors(pe, ie, de); + } /*! * \brief Print to console the current parameters */ - void printValues(); + void print_values(); + + /*! + * \brief Print to console the current parameters + */ + [[deprecated("Use print_values() instead")]] void printValues() { print_values(); } /*! * \brief Return PID parameters callback handle * \return shared_ptr to the PID parameters callback handle */ inline rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr - getParametersCallbackHandle() + get_parameters_callback_handle() { return parameter_callback_; } + /*! + * \brief Return PID parameters callback handle + * \return shared_ptr to the PID parameters callback handle + */ + [[deprecated("Use get_parameters_callback_handle() instead")]] + inline rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + getParametersCallbackHandle() + { + return get_parameters_callback_handle(); + } + protected: std::string topic_prefix_; std::string param_prefix_; private: - void setParameterEventCallback(); + void set_parameter_event_callback(); - void publishPIDState(double cmd, double error, rclcpp::Duration dt); + void publish_pid_state(double cmd, double error, rclcpp::Duration dt); - void declareParam(const std::string & param_name, rclcpp::ParameterValue param_value); + void declare_param(const std::string & param_name, rclcpp::ParameterValue param_value); - bool getDoubleParam(const std::string & param_name, double & value); + bool get_double_param(const std::string & param_name, double & value); - bool getBooleanParam(const std::string & param_name, bool & value); + bool get_boolean_param(const std::string & param_name, bool & value); /*! * \param topic_prefix prefix to add to the pid parameters. diff --git a/src/pid.cpp b/src/pid.cpp index f6b7824..fb1ee9b 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -52,7 +52,7 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind if (i_min > i_max) { throw std::invalid_argument("received i_min > i_max"); } - setGains(p, i, d, i_max, i_min, antiwindup); + set_gains(p, i, d, i_max, i_min, antiwindup); reset(); } @@ -68,9 +68,9 @@ Pid::Pid(const Pid & source) Pid::~Pid() {} -void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void Pid::init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - setGains(p, i, d, i_max, i_min, antiwindup); + set_gains(p, i, d, i_max, i_min, antiwindup); reset(); } @@ -84,13 +84,13 @@ void Pid::reset() cmd_ = 0.0; } -void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min) +void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min) { bool antiwindup; - getGains(p, i, d, i_max, i_min, antiwindup); + get_gains(p, i, d, i_max, i_min, antiwindup); } -void Pid::getGains( +void Pid::get_gains( double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) { Gains gains = *gains_buffer_.readFromRT(); @@ -103,16 +103,16 @@ void Pid::getGains( antiwindup = gains.antiwindup_; } -Pid::Gains Pid::getGains() { return *gains_buffer_.readFromRT(); } +Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } -void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { Gains gains(p, i, d, i_max, i_min, antiwindup); - setGains(gains); + set_gains(gains); } -void Pid::setGains(const Gains & gains) +void Pid::set_gains(const Gains & gains) { if (gains.i_min_ > gains.i_max_) { std::cout << "received i_min > i_max, skip new gains\n"; @@ -121,23 +121,7 @@ void Pid::setGains(const Gains & gains) } } -double Pid::computeCommand(double error, uint64_t dt_ns) -{ - if (dt_ns == 0 || std::isnan(error) || std::isinf(error)) { - return 0.0; - } - - error_dot_ = d_error_; - - // Calculate the derivative error - double dt_s = (static_cast(dt_ns) / 1e9); - error_dot_ = (error - p_error_last_) / dt_s; - p_error_last_ = error; - - return computeCommand(error, error_dot_, dt_s); -} - -double Pid::computeCommand(double error, double dt_s) +double Pid::compute_command(double error, double dt_s) { if (dt_s <= 0.0 || std::isnan(error) || std::isinf(error)) { return 0.0; @@ -149,15 +133,10 @@ double Pid::computeCommand(double error, double dt_s) error_dot_ = (error - p_error_last_) / dt_s; p_error_last_ = error; - return computeCommand(error, error_dot_, dt_s); -} - -double Pid::computeCommand(double error, double error_dot, uint64_t dt_ns) -{ - return computeCommand(error, error_dot, static_cast(dt_ns) / 1.e9); + return compute_command(error, error_dot_, dt_s); } -double Pid::computeCommand(double error, double error_dot, double dt_s) +double Pid::compute_command(double error, double error_dot, double dt_s) { // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); @@ -202,13 +181,13 @@ double Pid::computeCommand(double error, double error_dot, double dt_s) return cmd_; } -void Pid::setCurrentCmd(double cmd) { cmd_ = cmd; } +void Pid::set_current_cmd(double cmd) { cmd_ = cmd; } -double Pid::getDerivativeError() { return error_dot_; } +double Pid::get_derivative_error() { return error_dot_; } -double Pid::getCurrentCmd() { return cmd_; } +double Pid::get_current_cmd() { return cmd_; } -void Pid::getCurrentPIDErrors(double & pe, double & ie, double & de) +void Pid::get_current_pid_errors(double & pe, double & ie, double & de) { pe = p_error_; ie = i_error_; diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp index 36e18d2..20488ba 100644 --- a/src/pid_ros.cpp +++ b/src/pid_ros.cpp @@ -119,9 +119,9 @@ void PidROS::initialize(std::string topic_prefix) } } -bool PidROS::getBooleanParam(const std::string & param_name, bool & value) +bool PidROS::get_boolean_param(const std::string & param_name, bool & value) { - declareParam(param_name, rclcpp::ParameterValue(value)); + declare_param(param_name, rclcpp::ParameterValue(value)); rclcpp::Parameter param; if (node_params_->has_parameter(param_name)) { node_params_->get_parameter(param_name, param); @@ -138,9 +138,9 @@ bool PidROS::getBooleanParam(const std::string & param_name, bool & value) } // TODO(anyone): to-be-removed once this functionality becomes supported by the param API directly -bool PidROS::getDoubleParam(const std::string & param_name, double & value) +bool PidROS::get_double_param(const std::string & param_name, double & value) { - declareParam(param_name, rclcpp::ParameterValue(value)); + declare_param(param_name, rclcpp::ParameterValue(value)); rclcpp::Parameter param; if (node_params_->has_parameter(param_name)) { node_params_->get_parameter(param_name, param); @@ -164,80 +164,80 @@ bool PidROS::getDoubleParam(const std::string & param_name, double & value) } } -bool PidROS::initPid() +bool PidROS::init_pid() { double p, i, d, i_min, i_max; p = i = d = i_min = i_max = std::numeric_limits::quiet_NaN(); bool antiwindup = false; bool all_params_available = true; - all_params_available &= getDoubleParam(param_prefix_ + "p", p); - all_params_available &= getDoubleParam(param_prefix_ + "i", i); - all_params_available &= getDoubleParam(param_prefix_ + "d", d); - all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_max", i_max); - all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_min", i_min); + 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_ + "i_clamp_max", i_max); + all_params_available &= get_double_param(param_prefix_ + "i_clamp_min", i_min); - getBooleanParam(param_prefix_ + "antiwindup", antiwindup); + get_boolean_param(param_prefix_ + "antiwindup", antiwindup); if (all_params_available) { - setParameterEventCallback(); + set_parameter_event_callback(); } - pid_.initPid(p, i, d, i_max, i_min, antiwindup); + pid_.init_pid(p, i, d, i_max, i_min, antiwindup); return all_params_available; } -void PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue param_value) +void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValue param_value) { if (!node_params_->has_parameter(param_name)) { node_params_->declare_parameter(param_name, param_value); } } -void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void PidROS::init_pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.initPid(p, i, d, i_max, i_min, antiwindup); + pid_.init_pid(p, i, d, i_max, i_min, antiwindup); - declareParam(param_prefix_ + "p", rclcpp::ParameterValue(p)); - declareParam(param_prefix_ + "i", rclcpp::ParameterValue(i)); - declareParam(param_prefix_ + "d", rclcpp::ParameterValue(d)); - declareParam(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max)); - declareParam(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min)); - declareParam(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); + declare_param(param_prefix_ + "p", rclcpp::ParameterValue(p)); + declare_param(param_prefix_ + "i", rclcpp::ParameterValue(i)); + declare_param(param_prefix_ + "d", rclcpp::ParameterValue(d)); + declare_param(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max)); + declare_param(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min)); + declare_param(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup)); - setParameterEventCallback(); + set_parameter_event_callback(); } } void PidROS::reset() { pid_.reset(); } -std::shared_ptr> PidROS::getPidStatePublisher() +std::shared_ptr> PidROS::get_pid_state_publisher() { return state_pub_; } -double PidROS::computeCommand(double error, rclcpp::Duration dt) +double PidROS::compute_command(double error, rclcpp::Duration dt) { - double cmd_ = pid_.computeCommand(error, static_cast(dt.nanoseconds())); - publishPIDState(cmd_, error, dt); + double cmd_ = pid_.compute_command(error, dt); + publish_pid_state(cmd_, error, dt); return cmd_; } -double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration dt) +double PidROS::compute_command(double error, double error_dot, rclcpp::Duration dt) { - double cmd_ = pid_.computeCommand(error, error_dot, static_cast(dt.nanoseconds())); - publishPIDState(cmd_, error, dt); + double cmd_ = pid_.compute_command(error, error_dot, dt); + publish_pid_state(cmd_, error, dt); return cmd_; } -Pid::Gains PidROS::getGains() { return pid_.getGains(); } +Pid::Gains PidROS::get_gains() { return pid_.get_gains(); } -void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) +void PidROS::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { if (i_min > i_max) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); @@ -249,16 +249,16 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min, rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min), rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)}); - pid_.setGains(p, i, d, i_max, i_min, antiwindup); + pid_.set_gains(p, i, d, i_max, i_min, antiwindup); } } -void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) +void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt) { - Pid::Gains gains = pid_.getGains(); + Pid::Gains gains = pid_.get_gains(); double p_error_, i_error_, d_error_; - getCurrentPIDErrors(p_error_, i_error_, d_error_); + get_current_pid_errors(p_error_, i_error_, d_error_); // Publish controller state if configured if (rt_state_pub_) { @@ -266,7 +266,7 @@ void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) rt_state_pub_->msg_.header.stamp = rclcpp::Clock().now(); rt_state_pub_->msg_.timestep = dt; rt_state_pub_->msg_.error = error; - rt_state_pub_->msg_.error_dot = pid_.getDerivativeError(); + rt_state_pub_->msg_.error_dot = pid_.get_derivative_error(); rt_state_pub_->msg_.p_error = p_error_; rt_state_pub_->msg_.i_error = i_error_; rt_state_pub_->msg_.d_error = d_error_; @@ -281,25 +281,25 @@ void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt) } } -void PidROS::setCurrentCmd(double cmd) { pid_.setCurrentCmd(cmd); } +void PidROS::set_current_cmd(double cmd) { pid_.set_current_cmd(cmd); } -double PidROS::getCurrentCmd() { return pid_.getCurrentCmd(); } +double PidROS::get_current_cmd() { return pid_.get_current_cmd(); } -void PidROS::getCurrentPIDErrors(double & pe, double & ie, double & de) +void PidROS::get_current_pid_errors(double & pe, double & ie, double & de) { double _pe, _ie, _de; - pid_.getCurrentPIDErrors(_pe, _ie, _de); + pid_.get_current_pid_errors(_pe, _ie, _de); pe = _pe; ie = _ie; de = _de; } -void PidROS::printValues() +void PidROS::print_values() { - Pid::Gains gains = pid_.getGains(); + Pid::Gains gains = pid_.get_gains(); double p_error_, i_error_, d_error_; - getCurrentPIDErrors(p_error_, i_error_, d_error_); + get_current_pid_errors(p_error_, i_error_, d_error_); RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Current Values of PID template:\n" << " P Gain: " << gains.p_gain_ << "\n" @@ -312,26 +312,26 @@ void PidROS::printValues() << " P_Error: " << p_error_ << "\n" << " I_Error: " << i_error_ << "\n" << " D_Error: " << d_error_ << "\n" - << " Command: " << getCurrentCmd();); + << " Command: " << get_current_cmd();); } -void PidROS::setGains(const Pid::Gains & gains) +void PidROS::set_gains(const Pid::Gains & gains) { if (gains.i_min_ > gains.i_max_) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.setGains(gains); + pid_.set_gains(gains); } } -void PidROS::setParameterEventCallback() +void PidROS::set_parameter_event_callback() { auto on_parameter_event_callback = [this](const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; /// @note don't use getGains, it's rt - Pid::Gains gains = pid_.getGains(); + Pid::Gains gains = pid_.get_gains(); bool changed = false; for (auto & parameter : parameters) { @@ -362,11 +362,11 @@ void PidROS::setParameterEventCallback() } if (changed) { - /// @note don't call setGains() from inside a callback + /// @note don't call set_gains() from inside a callback if (gains.i_min_ > gains.i_max_) { RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains"); } else { - pid_.setGains(gains); + pid_.set_gains(gains); } } diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp index 9a26d65..d54dfce 100644 --- a/test/pid_parameters_tests.cpp +++ b/test/pid_parameters_tests.cpp @@ -58,7 +58,7 @@ void check_set_parameters( const double I_MIN = -10.0; const bool ANTIWINDUP = true; - ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); + ASSERT_NO_THROW(pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP)); rclcpp::Parameter param; @@ -82,7 +82,7 @@ void check_set_parameters( ASSERT_EQ(param.get_value(), ANTIWINDUP); // check gains were set - control_toolbox::Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -115,7 +115,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) const double I_MAX_BAD = -10.0; const double I_MIN_BAD = 10.0; - ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); + ASSERT_NO_THROW(pid.init_pid(P, I, D, I_MAX_BAD, I_MIN_BAD, false)); rclcpp::Parameter param; @@ -128,7 +128,7 @@ TEST(PidParametersTest, InitPidTestBadParameter) ASSERT_FALSE(node->get_parameter("antiwindup", param)); // check gains were NOT set - control_toolbox::Pid::Gains gains = pid.getGains(); + 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); @@ -216,7 +216,7 @@ TEST(PidParametersTest, SetParametersTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rcl_interfaces::msg::SetParametersResult set_result; @@ -242,7 +242,7 @@ TEST(PidParametersTest, SetParametersTest) rclcpp::spin_some(node->get_node_base_interface()); // check gains were set using the parameters - control_toolbox::Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -266,7 +266,7 @@ TEST(PidParametersTest, SetBadParametersTest) const double I_MIN_BAD = 20.0; const bool ANTIWINDUP = true; - pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.init_pid(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rcl_interfaces::msg::SetParametersResult set_result; @@ -292,7 +292,7 @@ TEST(PidParametersTest, SetBadParametersTest) rclcpp::spin_some(node->get_node_base_interface()); // check gains were NOT set using the parameters - control_toolbox::Pid::Gains gains = pid.getGains(); + control_toolbox::Pid::Gains gains = pid.get_gains(); ASSERT_EQ(gains.p_gain_, P); ASSERT_EQ(gains.i_gain_, I); ASSERT_EQ(gains.d_gain_, D); @@ -314,8 +314,8 @@ TEST(PidParametersTest, GetParametersTest) const double I_MIN = -10.0; const bool ANTIWINDUP = true; - pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false); - pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); + pid.init_pid(0.0, 0.0, 0.0, 0.0, 0.0, false); + pid.set_gains(P, I, D, I_MAX, I_MIN, ANTIWINDUP); rclcpp::Parameter param; @@ -344,7 +344,7 @@ TEST(PidParametersTest, GetParametersFromParams) TestablePidROS pid(node); - ASSERT_TRUE(pid.initPid()); + ASSERT_TRUE(pid.init_pid()); rclcpp::Parameter param_p; ASSERT_TRUE(node->get_parameter("p", param_p)); @@ -380,8 +380,8 @@ TEST(PidParametersTest, MultiplePidInstances) const double I_MAX = 10.0; const double I_MIN = -10.0; - ASSERT_NO_THROW(pid_1.initPid(P, I, D, I_MAX, I_MIN, false)); - ASSERT_NO_THROW(pid_2.initPid(P, I, D, I_MAX, I_MIN, true)); + ASSERT_NO_THROW(pid_1.init_pid(P, I, D, I_MAX, I_MIN, false)); + ASSERT_NO_THROW(pid_2.init_pid(P, I, D, I_MAX, I_MIN, true)); rclcpp::Parameter param_1, param_2; ASSERT_TRUE(node->get_parameter("PID_1.p", param_1)); diff --git a/test/pid_publisher_tests.cpp b/test/pid_publisher_tests.cpp index 1c3de5f..f215b6a 100644 --- a/test/pid_publisher_tests.cpp +++ b/test/pid_publisher_tests.cpp @@ -39,7 +39,7 @@ TEST(PidPublisherTest, PublishTest) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); - pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false); + pid_ros.init_pid(1.0, 1.0, 1.0, 5.0, -5.0, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -50,12 +50,12 @@ TEST(PidPublisherTest, PublishTest) auto state_sub = node->create_subscription( "/pid_state", rclcpp::SensorDataQoS(), state_callback); - double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); EXPECT_EQ(-1.5, command); // wait for callback for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { - pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); rclcpp::spin_some(node); std::this_thread::sleep_for(DELAY); } @@ -74,10 +74,10 @@ TEST(PidPublisherTest, PublishTestLifecycle) auto state_pub_lifecycle_ = std::dynamic_pointer_cast>( - pid_ros.getPidStatePublisher()); + pid_ros.get_pid_state_publisher()); // state_pub_lifecycle_->on_activate(); - pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false); + pid_ros.init_pid(1.0, 1.0, 1.0, 5.0, -5.0, false); bool callback_called = false; control_msgs::msg::PidState::SharedPtr last_state_msg; @@ -88,12 +88,12 @@ TEST(PidPublisherTest, PublishTestLifecycle) auto state_sub = node->create_subscription( "/pid_state", rclcpp::SensorDataQoS(), state_callback); - double command = pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); EXPECT_EQ(-1.5, command); // wait for callback for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { - pid_ros.computeCommand(-0.5, rclcpp::Duration(1, 0)); + pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); rclcpp::spin_some(node->get_node_base_interface()); std::this_thread::sleep_for(DELAY); } diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index f8b7146..15e6646 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -58,11 +58,11 @@ TEST(ParameterTest, ITermBadIBoundsTest) "i_min > i_max)."); Pid pid(1.0, 1.0, 1.0, 1.0, -1.0); - auto gains = pid.getGains(); + auto gains = pid.get_gains(); EXPECT_DOUBLE_EQ(gains.i_min_, -1.0); EXPECT_DOUBLE_EQ(gains.i_max_, 1.0); // Try to set bad i-bounds, i.e. i_min > i_max - EXPECT_NO_THROW(pid.setGains(1.0, 1.0, 1.0, -2.0, 2.0)); + EXPECT_NO_THROW(pid.set_gains(1.0, 1.0, 1.0, -2.0, 2.0)); // Check if gains were not updated because i-bounds are bad, i.e. i_min > i_max EXPECT_DOUBLE_EQ(gains.i_min_, -1.0); EXPECT_DOUBLE_EQ(gains.i_max_, 1.0); @@ -80,11 +80,11 @@ TEST(ParameterTest, integrationClampTest) double cmd = 0.0; // Test lower limit - cmd = pid.computeCommand(-10.03, 1.0); + cmd = pid.compute_command(-10.03, 1.0); EXPECT_EQ(-1.0, cmd); // Test upper limit - cmd = pid.computeCommand(30.0, 1.0); + cmd = pid.compute_command(30.0, 1.0); EXPECT_EQ(1.0, cmd); } @@ -104,13 +104,13 @@ TEST(ParameterTest, integrationClampZeroGainTest) double cmd = 0.0; double pe, ie, de; - cmd = pid.computeCommand(-1.0, 1.0); - pid.getCurrentPIDErrors(pe, ie, de); + cmd = pid.compute_command(-1.0, 1.0); + pid.get_current_pid_errors(pe, ie, de); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); EXPECT_LE(i_min, cmd); EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); @@ -129,16 +129,16 @@ TEST(ParameterTest, integrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); - cmd = pid.computeCommand(0.5, 1.0); + cmd = pid.compute_command(0.5, 1.0); EXPECT_EQ(0.0, cmd); - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); EXPECT_EQ(-1.0, cmd); } @@ -155,16 +155,16 @@ TEST(ParameterTest, negativeIntegrationAntiwindupTest) double cmd = 0.0; - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.compute_command(0.1, 1.0); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.compute_command(0.1, 1.0); EXPECT_EQ(-0.2, cmd); - cmd = pid.computeCommand(-0.05, 1.0); + cmd = pid.compute_command(-0.05, 1.0); EXPECT_EQ(-0.075, cmd); - cmd = pid.computeCommand(0.1, 1.0); + cmd = pid.compute_command(0.1, 1.0); EXPECT_EQ(-0.2, cmd); } @@ -189,7 +189,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; bool antiwindup_return; - pid1.getGains( + pid1.get_gains( p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); @@ -207,9 +207,9 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = std::rand() % 100; i_max = std::rand() % 100; i_min = -1 * std::rand() % 100; - pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); + pid1.set_gains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); - Pid::Gains g1 = pid1.getGains(); + 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_); @@ -222,13 +222,13 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // \todo test bool init(const ros::NodeHandle &n); ----------------------------------- // Send update command to populate errors ------------------------------------------------- - pid1.setCurrentCmd(10); - (void) pid1.computeCommand(20, 1.0); + pid1.set_current_cmd(10); + (void) pid1.compute_command(20, 1.0); // Test copy constructor ------------------------------------------------- Pid pid2(pid1); - pid2.getGains( + pid2.get_gains( p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); @@ -240,7 +240,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test that errors are zero double pe2, ie2, de2; - pid2.getCurrentPIDErrors(pe2, ie2, de2); + pid2.get_current_pid_errors(pe2, ie2, de2); EXPECT_EQ(0.0, pe2); EXPECT_EQ(0.0, ie2); EXPECT_EQ(0.0, de2); @@ -249,7 +249,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid3; pid3 = pid1; - pid3.getGains( + pid3.get_gains( p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); @@ -261,7 +261,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test that errors are zero double pe3, ie3, de3; - pid3.getCurrentPIDErrors(pe3, ie3, de3); + pid3.get_current_pid_errors(pe3, ie3, de3); EXPECT_EQ(0.0, pe3); EXPECT_EQ(0.0, ie3); EXPECT_EQ(0.0, de3); @@ -270,12 +270,12 @@ TEST(ParameterTest, gainSettingCopyPIDTest) pid1.reset(); double pe1, ie1, de1; - pid1.getCurrentPIDErrors(pe1, ie1, de1); + pid1.get_current_pid_errors(pe1, ie1, de1); EXPECT_EQ(0.0, pe1); EXPECT_EQ(0.0, ie1); EXPECT_EQ(0.0, de1); - double cmd1 = pid1.getCurrentCmd(); + double cmd1 = pid1.get_current_cmd(); EXPECT_EQ(0.0, cmd1); } @@ -291,22 +291,22 @@ TEST(CommandTest, proportionalOnlyTest) double cmd = 0.0; // If initial error = 0, p-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect the same as before EXPECT_EQ(-0.5, cmd); // If call again doubling the error - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); // Then expect the command doubled EXPECT_EQ(-1.0, cmd); // If call with positive error - cmd = pid.computeCommand(0.5, 1.0); + cmd = pid.compute_command(0.5, 1.0); // Then expect always command = error EXPECT_EQ(0.5, cmd); } @@ -323,26 +323,26 @@ TEST(CommandTest, integralOnlyTest) double cmd = 0.0; // If initial error = 0, i-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again with same arguments - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect the integral part to double the command EXPECT_EQ(-1.0, cmd); // Call again with no error - cmd = pid.computeCommand(0.0, 1.0); + cmd = pid.compute_command(0.0, 1.0); // Expect the integral part to keep the previous command because it ensures error = 0 EXPECT_EQ(-1.0, cmd); // Double check that the integral contribution keep the previous command - cmd = pid.computeCommand(0.0, 1.0); + 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.computeCommand(1.0, 1.0); + cmd = pid.compute_command(1.0, 1.0); // Expect that the command is cleared since error = -1 * previous command, i-gain = 1, dt = 1 EXPECT_EQ(0.0, cmd); } @@ -359,27 +359,27 @@ TEST(CommandTest, derivativeOnlyTest) double cmd = 0.0; // If initial error = 0, d-gain = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = error EXPECT_EQ(-0.5, cmd); // If call again with same error - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = 0 due to no variation on error EXPECT_EQ(0.0, cmd); // If call again with same error and smaller control period - cmd = pid.computeCommand(-0.5, 0.1); + cmd = pid.compute_command(-0.5, 0.1); // Then expect command = 0 again EXPECT_EQ(0.0, cmd); // If the error increases, with dt = 1 - cmd = pid.computeCommand(-1.0, 1.0); + 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 - cmd = pid.computeCommand(-0.5, 1.0); + 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); } @@ -396,17 +396,17 @@ TEST(CommandTest, completePIDTest) // All contributions are tested, here few tests check that they sum up correctly // If initial error = 0, all gains = 1, dt = 1 - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = 3x error EXPECT_EQ(-1.5, cmd); // If call again with same arguments, no error change, but integration do its part - cmd = pid.computeCommand(-0.5, 1.0); + cmd = pid.compute_command(-0.5, 1.0); // Then expect command = 3x error again EXPECT_EQ(-1.5, cmd); // If call again increasing the error - cmd = pid.computeCommand(-1.0, 1.0); + cmd = pid.compute_command(-1.0, 1.0); // Then expect command equals to p = -1, i = -2.0 (i.e. - 0.5 - 0.5 - 1.0), d = -0.5 EXPECT_EQ(-3.5, cmd); }