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 #ifndef PIDCONTROLLER_HPP 00035 #define PIDCONTROLLER_HPP 00036 00037 00038 #include <string> 00039 #include "generic/Time.hpp" 00040 00041 00042 namespace youbot { 00043 00044 /***************************************************/ 00045 /*! \class PidController 00046 \brief A basic pid class. 00047 00048 This class implements a generic structure that 00049 can be used to create a wide range of pid 00050 controllers. It can function independently or 00051 be subclassed to provide more specific controls 00052 based on a particular control loop. 00053 00054 In particular, this class implements the standard 00055 pid equation: 00056 00057 \f$command = -p_{term} - i_{term} - d_{term} \f$ 00058 00059 where: <br> 00060 <UL TYPE="none"> 00061 <LI> \f$ p_{term} = p_{gain} * p_{error} \f$ 00062 <LI> \f$ i_{term} = i_{gain} * i_{error} \f$ 00063 <LI> \f$ d_{term} = d_{gain} * d_{error} \f$ 00064 <LI> \f$ i_{error} = i_{error} + p_{error} * dt \f$ 00065 <LI> \f$ d_{error} = (p_{error} - p_{error last}) / dt \f$ 00066 </UL> 00067 00068 given:<br> 00069 <UL TYPE="none"> 00070 <LI> \f$ p_{error} = p_{state} - p_{target} \f$. 00071 </UL> 00072 00073 00074 @section Usage 00075 00076 To use the Pid class, you should first call some version of init() 00077 (in non-realtime) and then call updatePid() at every update step. 00078 For example: 00079 00080 \verbatim 00081 control_toolbox::Pid pid; 00082 pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3); 00083 double position_desi_ = 0.5; 00084 ... 00085 ros::Time last_time = ros::Time::now(); 00086 while (true) { 00087 ros::Time time = ros::Time::now(); 00088 double effort = pid.updatePid(currentPosition() - position_desi_, time - last_time); 00089 last_time = time; 00090 } 00091 \endverbatim 00092 00093 */ 00094 /***************************************************/ 00095 00096 class PidController 00097 { 00098 public: 00099 00100 /*! 00101 * \brief Constructor, zeros out Pid values when created and 00102 * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2]. 00103 * 00104 * \param P The proportional gain. 00105 * \param I The integral gain. 00106 * \param D The derivative gain. 00107 * \param I1 The integral upper limit. 00108 * \param I2 The integral lower limit. 00109 */ 00110 PidController(double P = 0.0, double I = 0.0, double D = 0.0, double I1 = 0.0, double I2 = -0.0); 00111 00112 /*! 00113 * \brief Destructor of Pid class. 00114 */ 00115 ~PidController(); 00116 00117 /*! 00118 * \brief Update the Pid loop with nonuniform time step size. 00119 * 00120 * \param p_error Error since last call (p_state-p_target) 00121 * \param dt Change in time since last call 00122 */ 00123 double updatePid(double p_error, boost::posix_time::time_duration dt); 00124 00125 /*! 00126 * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2] 00127 * 00128 * \param P The proportional gain. 00129 * \param I The integral gain. 00130 * \param D The derivative gain. 00131 * \param I1 The integral upper limit. 00132 * \param I2 The integral lower limit. 00133 */ 00134 void initPid(double P, double I, double D, double I1, double I2); 00135 00136 /*! 00137 * \brief Reset the state of this PID controller 00138 */ 00139 void reset(); 00140 00141 /*! 00142 * \brief Set current command for this PID controller 00143 */ 00144 void setCurrentCmd(double cmd); 00145 00146 /*! 00147 * \brief Return current command for this PID controller 00148 */ 00149 double getCurrentCmd(); 00150 00151 /*! 00152 * \brief Return PID error terms for the controller. 00153 * \param pe The proportional error. 00154 * \param ie The integral error. 00155 * \param de The derivative error. 00156 */ 00157 void getCurrentPIDErrors(double& pe, double& ie, double& de); 00158 00159 /*! 00160 * \brief Set PID gains for the controller. 00161 * \param P The proportional gain. 00162 * \param I The integral gain. 00163 * \param D The derivative gain. 00164 * \param i_max 00165 * \param i_min 00166 */ 00167 void setGains(double P, double I, double D, double i_max, double i_min); 00168 00169 /*! 00170 * \brief Get PID gains for the controller. 00171 * \param p The proportional gain. 00172 * \param i The integral gain. 00173 * \param d The derivative gain. 00174 * \param i_max The max integral windup. 00175 * \param i_mim The min integral windup. 00176 */ 00177 void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 00178 00179 /*! 00180 * \brief Update the Pid loop with nonuniform time step size. This update call 00181 * allows the user to pass in a precomputed derivative error. 00182 * 00183 * \param error Error since last call (p_state-p_target) 00184 * \param error_dot d(Error)/dt since last call 00185 * \param dt Change in time since last call 00186 */ 00187 double updatePid(double error, double error_dot, boost::posix_time::time_duration dt); 00188 00189 PidController &operator =(const PidController& p) 00190 { 00191 if (this == &p) 00192 return *this; 00193 00194 p_gain_ = p.p_gain_; 00195 i_gain_ = p.i_gain_; 00196 d_gain_ = p.d_gain_; 00197 i_max_ = p.i_max_; 00198 i_min_ = p.i_min_; 00199 00200 p_error_last_ = p_error_ = i_error_ = d_error_ = cmd_ = 0.0; 00201 return *this; 00202 } 00203 00204 private: 00205 double p_error_last_; /**< _Save position state for derivative state calculation. */ 00206 double p_error_; /**< Position error. */ 00207 double d_error_; /**< Derivative error. */ 00208 double i_error_; /**< Integator error. */ 00209 double p_gain_; /**< Proportional gain. */ 00210 double i_gain_; /**< Integral gain. */ 00211 double d_gain_; /**< Derivative gain. */ 00212 double i_max_; /**< Maximum allowable integral term. */ 00213 double i_min_; /**< Minimum allowable integral term. */ 00214 double cmd_; /**< Command to send. */ 00215 double last_i_error; 00216 }; 00217 00218 } 00219 00220 #endif