pid.h
Go to the documentation of this 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 #ifndef CONTROL_TOOLBOX__PID_H
00035 #define CONTROL_TOOLBOX__PID_H
00036 
00037 
00038 #include <string>
00039 #include <ros/ros.h>
00040 
00041 // Dynamic reconfigure
00042 #include <dynamic_reconfigure/server.h>
00043 #include <control_toolbox/ParametersConfig.h>
00044 #include <boost/thread/mutex.hpp>
00045 
00046 // Realtime buffer
00047 #include <realtime_tools/realtime_buffer.h>
00048 
00049 class TiXmlElement;
00050 
00051 namespace control_toolbox {
00052 
00053 /***************************************************/
00111 /***************************************************/
00112 
00113 class Pid
00114 {
00115 public:
00116 
00120   struct Gains
00121   {
00122     // Optional constructor for passing in values
00123     Gains(double p, double i, double d, double i_max, double i_min)
00124       : p_gain_(p),
00125         i_gain_(i),
00126         d_gain_(d),
00127         i_max_(i_max),
00128         i_min_(i_min)
00129     {}
00130     // Default constructor
00131     Gains() {}
00132     double p_gain_;  
00133     double i_gain_;  
00134     double d_gain_;  
00135     double i_max_;   
00136     double i_min_;   
00137   };
00138 
00150   Pid(double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0);
00151 
00156   Pid(const Pid &source);
00157 
00161   ~Pid();
00162 
00173   void initPid(double p, double i, double d, double i_max, double i_min);
00174 
00185   void initPid(double p, double i, double d, double i_max, double i_min, const ros::NodeHandle& /*node*/);
00186 
00194   bool initParam(const std::string& prefix, const bool quiet=false);
00195 
00203   bool init(const ros::NodeHandle &n, const bool quiet=false);
00204 
00211   bool initXml(TiXmlElement *config);
00212 
00217   void initDynamicReconfig(ros::NodeHandle &node);
00218 
00222   void reset();
00223 
00232   void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
00233 
00238   Gains getGains();
00239 
00248   void setGains(double p, double i, double d, double i_max, double i_min);
00249 
00254   void setGains(const Gains &gains);
00255 
00259   void updateDynamicReconfig();
00260   void updateDynamicReconfig(Gains gains_config);
00261   void updateDynamicReconfig(control_toolbox::ParametersConfig config);
00262 
00266   void dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t /*level*/);
00267 
00278   double computeCommand(double error, ros::Duration dt);
00279 
00291   double computeCommand(double error, double error_dot, ros::Duration dt);
00292 
00305   ROS_DEPRECATED double updatePid(double p_error, ros::Duration dt);
00306 
00321   ROS_DEPRECATED double updatePid(double error, double error_dot, ros::Duration dt);
00322 
00326   void setCurrentCmd(double cmd);
00327 
00331   double getCurrentCmd();
00332 
00339   void getCurrentPIDErrors(double *pe, double *ie, double *de);
00340 
00341 
00345   void printValues();
00346 
00351   Pid &operator =(const Pid& source)
00352   {
00353     if (this == &source)
00354       return *this;
00355 
00356     // Copy the realtime buffer to then new PID class
00357     gains_buffer_ = source.gains_buffer_;
00358 
00359     // Reset the state of this PID controller
00360     reset();
00361 
00362     return *this;
00363   }
00364 
00365 private:
00366 
00367   // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
00368   // blocking the realtime update loop
00369   realtime_tools::RealtimeBuffer<Gains> gains_buffer_;
00370 
00371   double p_error_last_; 
00372   double p_error_; 
00373   double i_error_; 
00374   double d_error_; 
00375   double cmd_;     
00377   // Dynamics reconfigure
00378   bool dynamic_reconfig_initialized_;
00379   typedef dynamic_reconfigure::Server<control_toolbox::ParametersConfig> DynamicReconfigServer;
00380   boost::shared_ptr<DynamicReconfigServer> param_reconfig_server_;
00381   DynamicReconfigServer::CallbackType param_reconfig_callback_;
00382 
00383   boost::recursive_mutex param_reconfig_mutex_;
00384 
00385 };
00386 
00387 }
00388 
00389 #endif


control_toolbox
Author(s): Melonee Wise, Sachin Chitta, John Hsu
autogenerated on Fri Apr 15 2016 15:39:29