pid2.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*-
00002  *
00003  *  Description:  PID (Proportional, Integral, Derivative) control output.
00004  *
00005  *  Copyright (C) 2005 Austin Robot Technology, Jack O'Quin
00006  *  Copyright (C) 2008 Austin Robot Technology, Patrick Beeson
00007  *
00008  *  License: Modified BSD Software License Agreement
00009  */
00010 
00011 #ifndef _PID_H_
00012 #define _PID_H_
00013 
00019 #include <float.h>
00020 #include <ros/ros.h>
00021 
00023 class Pid 
00024 {
00025  public:
00026 
00030  Pid(const char *ctlname):
00031   kp(1.0), ki(0.0), kd(0.0), omax(FLT_MAX), omin(-FLT_MAX), C(0.0), starting(1)
00032     {
00033       this->name = ctlname;
00034       Clear();
00035     };
00036 
00037   Pid(const char *ctlname, float kp, float ki, float kd,
00038       float omax=FLT_MAX, float omin=-FLT_MAX, float C=0.0): starting(1)
00039     {
00040       this->name = ctlname;
00041       this->Configure(kp, ki, kd, omax, omin, C);
00042       Clear();
00043     };
00044   
00045   virtual ~Pid() {};
00046   
00048   virtual void Configure(float kp, float ki, float kd)
00049   {
00050       this->kp = kp;
00051       this->ki = ki;
00052       this->kd = kd;
00053   };
00054   
00056   virtual void Configure(float kp, float ki, float kd,
00057                          float omax, float omin, float C)
00058   {
00059       this->kp = kp;
00060       this->ki = ki;
00061       this->kd = kd;
00062       this->omax = omax;
00063       this->omin = omin;
00064       this->C = C;
00065   };
00066   
00068   virtual void Configure(const ros::NodeHandle &node)
00069   {
00070     // configure PID constants
00071     CfgParam(node, "kp", &this->kp);
00072     CfgParam(node, "ki", &this->ki);
00073     CfgParam(node, "kd", &this->kd);
00074     ROS_DEBUG("%s gains (%.3f, %.3f, %.3f)",
00075               this->name.c_str(), this->kp, this->ki, this->kd);
00076     CfgParam(node, "omax", &this->omax);
00077     CfgParam(node, "omin", &this->omin);
00078     CfgParam(node, "C", &this->C);
00079     ROS_DEBUG("%s output range [%.1f, %.1f]",
00080               this->name.c_str(), this->omin, this->omax);
00081   };
00082   
00088   float Update(float error, float output)
00089   {
00090     if (starting)
00091       {
00092         this->istate=0;
00093         this->dstate=output;
00094         starting=false;
00095       }
00096     
00097     // Proportional term
00098     float p = this->kp * error;
00099     
00100     // Derivative term
00101     float d = this->kd * (output - this->dstate);
00102     this->dstate = output;
00103 
00104     float i = this->ki * this->istate;
00105 
00106     float PID_control = (p + i - d);
00107 
00108     ROS_DEBUG("%s PID: %.3f = %.3f + %.3f - %.3f",
00109               this->name.c_str(), PID_control, p, i, d);
00110 
00111     float PID_out=PID_control;
00112 
00113     if (PID_control > omax)
00114       PID_out=omax;
00115     if (PID_control < omin)
00116       PID_out=omin;
00117 
00118     // Integral term -- In reading other code, I is calculated after
00119     // the output.
00120     // The C term reduces the integral when the controller is already
00121     // pushing as hard as it can.
00122     this->istate = this->istate + error;
00123     float tracking = C*(PID_out-PID_control);
00124     if((istate > 0 && -tracking > istate) || (istate < 0 && -tracking < istate))
00125       istate = 0;
00126     else
00127       this->istate = this->istate + tracking;
00128 
00129     if (isnan(istate) || isinf(istate))
00130       istate=0;
00131 
00132     ROS_DEBUG("%s istate = %.3f, PID_out: %.3f, C*(PID_out-PID_control):%.3f",
00133               this->name.c_str(), istate, PID_out, C*(PID_out-PID_control));
00134 
00135     return PID_out;
00136   }
00137 
00139   void Clear()
00140   {
00141     starting=true;
00142   }
00143 
00148   void CopyHistory(const Pid* pid)
00149   {
00150     // These values do not depend on the constants of the other PID,
00151     // so they're safe to copy
00152     this->dstate = pid->dstate;
00153     this->istate = pid->istate;
00154     // Check if we were called on an unused PID for whatever reason
00155     this->starting = pid->starting;
00156   }
00157   
00158  protected:
00159 
00160   std::string name;                     
00162   // PID control parameters
00163   float dstate;                         
00164   float istate;                         
00165   float kp;                             
00166   float ki;                             
00167   float kd;                             
00168   float omax;                           
00169   float omin;                           
00170   float C;                              
00172   bool starting;
00173 
00174 
00181   void CfgParam(const ros::NodeHandle &node,
00182                 const char *pname, float *fvalue)
00183   {
00184     double dvalue;                      // (ROS parameter type is double)
00185     std::string optname = this->name + '_' + pname;
00186     if (node.getParamCached(optname, dvalue))
00187       {
00188         float param_value = dvalue;     // convert double to float
00189         if (*fvalue != param_value)     // new value?
00190           {
00191             ROS_INFO("%s changed to %.3f", optname.c_str(), param_value);
00192             *fvalue = param_value;
00193           }
00194       }
00195   }
00196 
00197 
00198 };
00199 
00200 #endif // _PID_H_ //
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


art_common
Author(s): Austin Robot Technology
autogenerated on Tue Sep 24 2013 10:41:19