control_toolbox: control_toolbox: pid.cpp Source File

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 // Original version: Melonee Wise <mwise@willowgarage.com>
00036 
00037 #include "control_toolbox/pid.h"
00038 #include "tinyxml.h"
00039 
00040 namespace control_toolbox {
00041 
00042 Pid::Pid(double P, double I, double D, double I1, double I2) :
00043   p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
00044 {
00045   p_error_last_ = 0.0;
00046   p_error_ = 0.0;
00047   d_error_ = 0.0;
00048   i_error_ = 0.0;
00049   cmd_ = 0.0;
00050 }
00051 
00052 Pid::~Pid()
00053 {
00054 }
00055 
00056 void Pid::initPid(double P, double I, double D, double I1, double I2)
00057 {
00058   p_gain_ = P;
00059   i_gain_ = I;
00060   d_gain_ = D;
00061   i_max_ = I1;
00062   i_min_ = I2;
00063 
00064   reset();
00065 }
00066 
00067 void Pid::reset()
00068 {
00069   p_error_last_ = 0.0;
00070   p_error_ = 0.0;
00071   d_error_ = 0.0;
00072   i_error_ = 0.0;
00073   cmd_ = 0.0;
00074 }
00075 
00076 void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00077 {
00078   p = p_gain_;
00079   i = i_gain_;
00080   d = d_gain_;
00081   i_max = i_max_;
00082   i_min = i_min_;
00083 }
00084 
00085 void Pid::setGains(double P, double I, double D, double I1, double I2)
00086 {
00087   p_gain_ = P;
00088   i_gain_ = I;
00089   d_gain_ = D;
00090   i_max_ = I1;
00091   i_min_ = I2;
00092 }
00093 
00094 bool Pid::initParam(const std::string& prefix)
00095 {
00096   ros::NodeHandle node(prefix);
00097 
00098   if (!node.getParam("p", p_gain_)) {
00099     ROS_ERROR("No p gain specified for pid.  Prefix: %s", prefix.c_str());
00100     return false;
00101   }
00102   node.param("i", i_gain_, 0.0) ;
00103   node.param("d", d_gain_, 0.0) ;
00104   node.param("i_clamp", i_max_, 0.0) ;
00105   i_min_ = -i_max_;
00106 
00107   reset();
00108   return true;
00109 }
00110 
00111 bool Pid::initXml(TiXmlElement *config)
00112 {
00113   p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0;
00114   i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0;
00115   d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0;
00116   i_max_ = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
00117   i_min_ = -i_max_;
00118 
00119   reset();
00120   return true;
00121 }
00122 
00123 bool Pid::init(const ros::NodeHandle &node)
00124 {
00125   ros::NodeHandle n(node);
00126   if (!n.getParam("p", p_gain_)) {
00127     ROS_ERROR("No p gain specified for pid.  Namespace: %s", n.getNamespace().c_str());
00128     return false;
00129   }
00130   n.param("i", i_gain_, 0.0);
00131   n.param("d", d_gain_, 0.0);
00132   n.param("i_clamp", i_max_, 0.0);
00133   i_min_ = -i_max_;
00134 
00135   reset();
00136   return true;
00137 }
00138 
00139 
00140 double Pid::updatePid(double error, ros::Duration dt)
00141 {
00142   double p_term, d_term, i_term;
00143   p_error_ = error; //this is pError = pState-pTarget
00144 
00145   if (dt == ros::Duration(0.0) || isnan(error) || isinf(error))
00146     return 0.0;
00147 
00148   // Calculate proportional contribution to command
00149   p_term = p_gain_ * p_error_;
00150 
00151   // Calculate the integral error
00152   i_error_ = i_error_ + dt.toSec() * p_error_;
00153 
00154   //Calculate integral contribution to command
00155   i_term = i_gain_ * i_error_;
00156 
00157   // Limit i_term so that the limit is meaningful in the output
00158   if (i_term > i_max_)
00159   {
00160     i_term = i_max_;
00161     i_error_=i_term/i_gain_;
00162   }
00163   else if (i_term < i_min_)
00164   {
00165     i_term = i_min_;
00166     i_error_=i_term/i_gain_;
00167   }
00168 
00169   // Calculate the derivative error
00170   if (dt.toSec() != 0)
00171   {
00172     d_error_ = (p_error_ - p_error_last_) / dt.toSec();
00173     p_error_last_ = p_error_;
00174   }
00175   // Calculate derivative contribution to command
00176   d_term = d_gain_ * d_error_;
00177   cmd_ = -p_term - i_term - d_term;
00178 
00179   return cmd_;
00180 }
00181 
00182 
00183 double Pid::updatePid(double error, double error_dot, ros::Duration dt)
00184 {
00185   double p_term, d_term, i_term;
00186   p_error_ = error; //this is pError = pState-pTarget
00187   d_error_ = error_dot;
00188 
00189   if (dt == ros::Duration(0.0) || isnan(error) || isinf(error) || isnan(error_dot) || isinf(error_dot))
00190     return 0.0;
00191 
00192 
00193   // Calculate proportional contribution to command
00194   p_term = p_gain_ * p_error_;
00195 
00196   // Calculate the integral error
00197   i_error_ = i_error_ + dt.toSec() * p_error_;
00198 
00199   //Calculate integral contribution to command
00200   i_term = i_gain_ * i_error_;
00201 
00202   // Limit i_term so that the limit is meaningful in the output
00203   if (i_term > i_max_)
00204   {
00205     i_term = i_max_;
00206     i_error_=i_term/i_gain_;
00207   }
00208   else if (i_term < i_min_)
00209   {
00210     i_term = i_min_;
00211     i_error_=i_term/i_gain_;
00212   }
00213 
00214   // Calculate derivative contribution to command
00215   d_term = d_gain_ * d_error_;
00216   cmd_ = -p_term - i_term - d_term;
00217 
00218   return cmd_;
00219 }
00220 
00221 
00222 
00223 void Pid::setCurrentCmd(double cmd)
00224 {
00225   cmd_ = cmd;
00226 }
00227 
00228 double Pid::getCurrentCmd()
00229 {
00230   return cmd_;
00231 }
00232 
00233 void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
00234 {
00235   *pe = p_error_;
00236   *ie = i_error_;
00237   *de = d_error_;
00238 }
00239 
00240 }