From 9192c5cfd780ffd83d2f8a7805d0ea0743f70193 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 11 Aug 2015 09:51:46 -0400 Subject: [PATCH] Update setGain/getGain, services, and dynamic reconfig --- cfg/Parameters.cfg | 1 + include/control_toolbox/pid.h | 24 +++++++++++++----------- src/pid.cpp | 26 +++++++++++++++----------- src/pid_gains_setter.cpp | 3 ++- srv/SetPidGains.srv | 1 + test/pid_tests.cpp | 16 +++++++++++----- 6 files changed, 43 insertions(+), 28 deletions(-) diff --git a/cfg/Parameters.cfg b/cfg/Parameters.cfg index 7f5e4247..4b25f4da 100755 --- a/cfg/Parameters.cfg +++ b/cfg/Parameters.cfg @@ -45,6 +45,7 @@ def generate(gen): gen.add( "d" , double_t, 1,"Derivative gain.", 1.0 , 0 , 1000) gen.add( "i_clamp_min" , double_t, 1,"Min bounds for the integral windup", -10.0 , -1000 , 0) gen.add( "i_clamp_max" , double_t, 1,"Max bounds for the integral windup", 10.0 , 0 , 1000) + gen.add( "antiwindup" , bool_t, 1,"Antiwindup.", True) # PkgName #NodeName #Prefix for generated .h include file, e.g. ParametersConfig.py exit(gen.generate(PACKAGE, "control_toolbox", "Parameters")) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index 5eeee9be..3ffa51ab 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -120,20 +120,22 @@ class Pid struct Gains { // Optional constructor for passing in values - Gains(double p, double i, double d, double i_max, double i_min) + Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), - i_min_(i_min) + i_min_(i_min), + antiwindup_(antiwindup) {} // Default constructor Gains() {} - double p_gain_; /**< Proportional gain. */ - double i_gain_; /**< Integral gain. */ - double d_gain_; /**< Derivative gain. */ - double i_max_; /**< Maximum allowable integral term. */ - double i_min_; /**< Minimum allowable integral term. */ + double p_gain_; /**< Proportional gain. */ + double i_gain_; /**< Integral gain. */ + double d_gain_; /**< Derivative gain. */ + double i_max_; /**< Maximum allowable integral term. */ + double i_min_; /**< Minimum allowable integral term. */ + bool antiwindup_; /**< Antiwindup. */ }; /*! @@ -170,7 +172,7 @@ class Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void initPid(double p, double i, double d, double i_max, double i_min); + void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup); /*! * \brief Zeros out Pid values and initialize Pid-gains and integral term limits @@ -182,7 +184,7 @@ class Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void initPid(double p, double i, double d, double i_max, double i_min, const ros::NodeHandle &node); + void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, const ros::NodeHandle &node); /*! * \brief Initialize PID with the parameters in a namespace @@ -229,7 +231,7 @@ class 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 getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup); /*! * \brief Get PID gains for the controller. @@ -245,7 +247,7 @@ class Pid * \param i_max The max integral windup. * \param i_min The min integral windup. */ - void setGains(double p, double i, double d, double i_max, double i_min); + void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup); /*! * \brief Set PID gains for the controller. diff --git a/src/pid.cpp b/src/pid.cpp index 8fb631e7..3fdf0524 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -48,7 +48,7 @@ static const std::string DEFAULT_NAMESPACE = "pid"; // \todo better default pref Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) : dynamic_reconfig_initialized_(false), antiwindup_(antiwindup) { - setGains(p,i,d,i_max,i_min); + setGains(p,i,d,i_max,i_min,antiwindup); reset(); } @@ -67,19 +67,19 @@ Pid::~Pid() { } -void Pid::initPid(double p, double i, double d, double i_max, double i_min, +void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup, const ros::NodeHandle &node) { - initPid(p, i, d, i_max, i_min); + initPid(p, i, d, i_max, i_min, antiwindup); // Create node handle for dynamic reconfigure ros::NodeHandle nh(DEFAULT_NAMESPACE); initDynamicReconfig(nh); } -void Pid::initPid(double p, double i, double d, double i_max, double i_min) +void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - setGains(p,i,d,i_max,i_min); + setGains(p,i,d,i_max,i_min, antiwindup); reset(); } @@ -147,7 +147,8 @@ bool Pid::initXml(TiXmlElement *config) config->Attribute("i") ? atof(config->Attribute("i")) : 0.0, config->Attribute("d") ? atof(config->Attribute("d")) : 0.0, std::abs(i_clamp), - -std::abs(i_clamp) + -std::abs(i_clamp), + config->Attribute("antiwindup") ? atof(config->Attribute("antiwindup")) : false ); reset(); @@ -182,7 +183,7 @@ void Pid::reset() cmd_ = 0.0; } -void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min) +void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup) { Gains gains = *gains_buffer_.readFromRT(); @@ -191,6 +192,7 @@ void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min d = gains.d_gain_; i_max = gains.i_max_; i_min = gains.i_min_; + antiwindup = gains.antiwindup_; } Pid::Gains Pid::getGains() @@ -198,9 +200,9 @@ Pid::Gains Pid::getGains() return *gains_buffer_.readFromRT(); } -void Pid::setGains(double p, double i, double d, double i_max, double i_min) +void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup) { - Gains gains(p,i,d,i_max,i_min); + Gains gains(p,i,d,i_max,i_min, antiwindup); setGains(gains); } @@ -223,7 +225,7 @@ void Pid::updateDynamicReconfig() control_toolbox::ParametersConfig config; // Get starting values - getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min); + getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup); updateDynamicReconfig(config); } @@ -242,6 +244,7 @@ void Pid::updateDynamicReconfig(Gains gains_config) config.d = gains_config.d_gain_; config.i_clamp_max = gains_config.i_max_; config.i_clamp_min = gains_config.i_min_; + config.antiwindup = gains_config.antiwindup_; updateDynamicReconfig(config); } @@ -263,7 +266,7 @@ void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uin ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved."); // Set the gains - setGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min); + setGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min, config.antiwindup); } double Pid::computeCommand(double error, ros::Duration dt) @@ -366,6 +369,7 @@ void Pid::printValues() << " D Gain: " << gains.d_gain_ << "\n" << " I_Max: " << gains.i_max_ << "\n" << " I_Min: " << gains.i_min_ << "\n" + << " Antiwindup: " << gains.antiwindup_ << "\n" << " P_Error_Last: " << p_error_last_ << "\n" << " P_Error: " << p_error_ << "\n" << " I_Error: " << i_error_ << "\n" diff --git a/src/pid_gains_setter.cpp b/src/pid_gains_setter.cpp index 80a5e746..58e68f9b 100644 --- a/src/pid_gains_setter.cpp +++ b/src/pid_gains_setter.cpp @@ -53,11 +53,12 @@ bool PidGainsSetter::setGains(control_toolbox::SetPidGains::Request &req, control_toolbox::SetPidGains::Response &resp) { for (size_t i = 0; i < pids_.size(); ++i) - pids_[i]->setGains(req.p, req.i, req.d, req.i_clamp, -req.i_clamp); + pids_[i]->setGains(req.p, req.i, req.d, req.i_clamp, -req.i_clamp, req.antiwindup); node_.setParam("p", req.p); node_.setParam("i", req.i); node_.setParam("d", req.d); node_.setParam("i_clamp", req.i_clamp); + node_.setParam("antiwindup", req.antiwindup); return true; } diff --git a/srv/SetPidGains.srv b/srv/SetPidGains.srv index e0dce4a8..97d089ea 100644 --- a/srv/SetPidGains.srv +++ b/srv/SetPidGains.srv @@ -2,4 +2,5 @@ float64 p float64 i float64 d float64 i_clamp +bool antiwindup --- diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index dd94ed34..cb84aed6 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -136,19 +136,22 @@ TEST(ParameterTest, gainSettingCopyPIDTest) double d_gain = rand() % 100; double i_max = rand() % 100; double i_min = -1 * rand() % 100; + bool antiwindup = false; // Initialize the default way - Pid pid1(p_gain, i_gain, d_gain, i_max, i_min); + Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); // Test return values ------------------------------------------------- double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return; - pid1.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + bool antiwindup_return; + pid1.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); + EXPECT_EQ(antiwindup, antiwindup_return); // Test return values using struct ------------------------------------------------- @@ -158,7 +161,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) d_gain = rand() % 100; i_max = rand() % 100; i_min = -1 * rand() % 100; - pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min); + pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup); Pid::Gains g1 = pid1.getGains(); EXPECT_EQ(p_gain, g1.p_gain_); @@ -166,6 +169,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest) EXPECT_EQ(d_gain, g1.d_gain_); EXPECT_EQ(i_max, g1.i_max_); EXPECT_EQ(i_min, g1.i_min_); + EXPECT_EQ(antiwindup, g1.antiwindup_); // \todo test initParam() ------------------------------------------------- @@ -180,13 +184,14 @@ TEST(ParameterTest, gainSettingCopyPIDTest) // Test copy constructor ------------------------------------------------- Pid pid2(pid1); - pid2.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + pid2.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); + EXPECT_EQ(antiwindup, antiwindup_return); // Test that errors are zero double pe, ie, de; @@ -199,13 +204,14 @@ TEST(ParameterTest, gainSettingCopyPIDTest) Pid pid3; pid3 = pid1; - pid3.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return); + pid3.getGains(p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return); EXPECT_EQ(p_gain, p_gain_return); EXPECT_EQ(i_gain, i_gain_return); EXPECT_EQ(d_gain, d_gain_return); EXPECT_EQ(i_max, i_max_return); EXPECT_EQ(i_min, i_min_return); + EXPECT_EQ(antiwindup, antiwindup_return); // Test that errors are zero double pe2, ie2, de2;