A basic pid class. More...
#include <pid.h>
Classes | |
struct | Gains |
Store gains in a struct to allow easier realtime buffer usage. More... | |
Public Member Functions | |
double | computeCommand (double error, ros::Duration dt) |
Set the PID error and compute the PID command with nonuniform time step size. The derivative error is computed from the change in the error and the timestep dt . | |
double | computeCommand (double error, double error_dot, ros::Duration dt) |
Set the PID error and compute the PID command with nonuniform time step size. This also allows the user to pass in a precomputed derivative error. | |
void | dynamicReconfigCallback (control_toolbox::ParametersConfig &config, uint32_t level) |
Update the PID parameters from dynamics reconfigure. | |
double | getCurrentCmd () |
Return current command for this PID controller. | |
void | getCurrentPIDErrors (double *pe, double *ie, double *de) |
Return PID error terms for the controller. | |
void | getGains (double &p, double &i, double &d, double &i_max, double &i_min) |
Get PID gains for the controller. | |
Gains | getGains () |
Get PID gains for the controller. | |
bool | init (const ros::NodeHandle &n) |
Initialize PID with the parameters in a NodeHandle namespace Initializes dynamic reconfigure for PID gains. | |
void | initDynamicReconfig (ros::NodeHandle &node) |
Start the dynamic reconfigure node and load the default values. | |
bool | initParam (const std::string &prefix) |
Initialize PID with the parameters in a namespace Initializes dynamic reconfigure for PID gains. | |
void | initPid (double p, double i, double d, double i_max, double i_min) |
Zeros out Pid values and initialize Pid-gains and integral term limits Does not initialize dynamic reconfigure for PID gains. | |
void | initPid (double p, double i, double d, double i_max, double i_min, const ros::NodeHandle &node) |
Zeros out Pid values and initialize Pid-gains and integral term limits Initializes dynamic reconfigure for PID gains. | |
bool | initXml (TiXmlElement *config) |
Initialize PID with the parameters in an XML element Initializes dynamic reconfigure for PID gains. | |
Pid & | operator= (const Pid &source) |
Custom assignment operator Does not initialize dynamic reconfigure for PID gains. | |
Pid (double p=0.0, double i=0.0, double d=0.0, double i_max=0.0, double i_min=-0.0) | |
Constructor, zeros out Pid values when created and initialize Pid-gains and integral term limits. Does not initialize dynamic reconfigure for PID gains. | |
Pid (const Pid &source) | |
Copy constructor required for preventing mutexes from being copied. | |
void | printValues () |
Print to console the current parameters. | |
void | reset () |
Reset the state of this PID controller. | |
void | setCurrentCmd (double cmd) |
Set current command for this PID controller. | |
void | setGains (double p, double i, double d, double i_max, double i_min) |
Set PID gains for the controller. | |
void | setGains (const Gains &gains) |
Set PID gains for the controller. | |
void | updateDynamicReconfig () |
Set Dynamic Reconfigure's gains to Pid's values. | |
void | updateDynamicReconfig (Gains gains_config) |
void | updateDynamicReconfig (control_toolbox::ParametersConfig config) |
ROS_DEPRECATED double | updatePid (double p_error, ros::Duration dt) |
Update the Pid loop with nonuniform time step size. | |
ROS_DEPRECATED double | updatePid (double error, double error_dot, ros::Duration dt) |
Update the Pid loop with nonuniform time step size. This update call allows the user to pass in a precomputed derivative error. | |
~Pid () | |
Destructor of Pid class. | |
Private Types | |
typedef dynamic_reconfigure::Server < control_toolbox::ParametersConfig > | DynamicReconfigServer |
Private Attributes | |
double | cmd_ |
double | d_error_ |
bool | dynamic_reconfig_initialized_ |
realtime_tools::RealtimeBuffer < Gains > | gains_buffer_ |
double | i_term_ |
double | p_error_ |
double | p_error_last_ |
DynamicReconfigServer::CallbackType | param_reconfig_callback_ |
boost::recursive_mutex | param_reconfig_mutex_ |
boost::shared_ptr < DynamicReconfigServer > | param_reconfig_server_ |
A basic pid class.
This class implements a generic structure that can be used to create a wide range of pid controllers. It can function independently or be subclassed to provide more specific controls based on a particular control loop.
In particular, this class implements the standard pid equation:
where:
given:
p | Proportional gain |
d | Derivative gain |
i | Integral gain |
i_clamp | Min/max bounds for the integral windup, the clamp is applied to the |
To use the Pid class, you should first call some version of init() (in non-realtime) and then call updatePid() at every update step. For example:
control_toolbox::Pid pid; pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3); double position_desi_ = 0.5; ... ros::Time last_time = ros::Time::now(); while (true) { ros::Time time = ros::Time::now(); double effort = pid.updatePid(currentPosition() - position_desi_, time - last_time); last_time = time; }
typedef dynamic_reconfigure::Server<control_toolbox::ParametersConfig> control_toolbox::Pid::DynamicReconfigServer [private] |
control_toolbox::Pid::Pid | ( | double | p = 0.0 , |
double | i = 0.0 , |
||
double | d = 0.0 , |
||
double | i_max = 0.0 , |
||
double | i_min = -0.0 |
||
) |
Constructor, zeros out Pid values when created and initialize Pid-gains and integral term limits. Does not initialize dynamic reconfigure for PID gains.
p | The proportional gain. |
i | The integral gain. |
d | The derivative gain. |
i_max | The max integral windup. |
i_min | The min integral windup. |
control_toolbox::Pid::Pid | ( | const Pid & | source | ) |
double control_toolbox::Pid::computeCommand | ( | double | error, |
ros::Duration | dt | ||
) |
Set the PID error and compute the PID command with nonuniform time step size. The derivative error is computed from the change in the error and the timestep dt
.
error | Error since last call (error = target - state) |
dt | Change in time since last call |
double control_toolbox::Pid::computeCommand | ( | double | error, |
double | error_dot, | ||
ros::Duration | dt | ||
) |
Set the PID error and compute the PID command with nonuniform time step size. This also allows the user to pass in a precomputed derivative error.
error | Error since last call (error = target - state) |
error_dot | d(Error)/dt since last call |
dt | Change in time since last call |
void control_toolbox::Pid::dynamicReconfigCallback | ( | control_toolbox::ParametersConfig & | config, |
uint32_t | level | ||
) |
double control_toolbox::Pid::getCurrentCmd | ( | ) |
void control_toolbox::Pid::getCurrentPIDErrors | ( | double * | pe, |
double * | ie, | ||
double * | de | ||
) |
void control_toolbox::Pid::getGains | ( | double & | p, |
double & | i, | ||
double & | d, | ||
double & | i_max, | ||
double & | i_min | ||
) |
bool control_toolbox::Pid::init | ( | const ros::NodeHandle & | n | ) |
void control_toolbox::Pid::initDynamicReconfig | ( | ros::NodeHandle & | node | ) |
bool control_toolbox::Pid::initParam | ( | const std::string & | prefix | ) |
void control_toolbox::Pid::initPid | ( | double | p, |
double | i, | ||
double | d, | ||
double | i_max, | ||
double | i_min | ||
) |
Zeros out Pid values and initialize Pid-gains and integral term limits Does not initialize dynamic reconfigure for PID gains.
p | The proportional gain. |
i | The integral gain. |
d | The derivative gain. |
i_max | The max integral windup. |
i_min | The min integral windup. |
void control_toolbox::Pid::initPid | ( | double | p, |
double | i, | ||
double | d, | ||
double | i_max, | ||
double | i_min, | ||
const ros::NodeHandle & | node | ||
) |
bool control_toolbox::Pid::initXml | ( | TiXmlElement * | config | ) |
void control_toolbox::Pid::printValues | ( | ) |
void control_toolbox::Pid::reset | ( | ) |
void control_toolbox::Pid::setCurrentCmd | ( | double | cmd | ) |
void control_toolbox::Pid::setGains | ( | double | p, |
double | i, | ||
double | d, | ||
double | i_max, | ||
double | i_min | ||
) |
void control_toolbox::Pid::setGains | ( | const Gains & | gains | ) |
void control_toolbox::Pid::updateDynamicReconfig | ( | Gains | gains_config | ) |
void control_toolbox::Pid::updateDynamicReconfig | ( | control_toolbox::ParametersConfig | config | ) |
double control_toolbox::Pid::updatePid | ( | double | p_error, |
ros::Duration | dt | ||
) |
Update the Pid loop with nonuniform time step size.
p_error = (state - target)
which is an unconventional definition of the error. Please use computeCommand instead, which assumes error = (target - state)
. Note that calls to computeCommand should not be mixed with calls to updatePid.p_error | Error since last call (p_state-p_target) |
dt | Change in time since last call |
double control_toolbox::Pid::updatePid | ( | double | error, |
double | error_dot, | ||
ros::Duration | dt | ||
) |
Update the Pid loop with nonuniform time step size. This update call allows the user to pass in a precomputed derivative error.
p_error = (state - target)
which is an unconventional definition of the error. Please use computeCommand instead, which assumes error = (target - state)
. Note that calls to computeCommand should not be mixed with calls to updatePid.error | Error since last call (p_state-p_target) |
error_dot | d(Error)/dt since last call |
dt | Change in time since last call |
double control_toolbox::Pid::cmd_ [private] |
double control_toolbox::Pid::d_error_ [private] |
bool control_toolbox::Pid::dynamic_reconfig_initialized_ [private] |
double control_toolbox::Pid::i_term_ [private] |
double control_toolbox::Pid::p_error_ [private] |
double control_toolbox::Pid::p_error_last_ [private] |
DynamicReconfigServer::CallbackType control_toolbox::Pid::param_reconfig_callback_ [private] |
boost::recursive_mutex control_toolbox::Pid::param_reconfig_mutex_ [private] |
boost::shared_ptr<DynamicReconfigServer> control_toolbox::Pid::param_reconfig_server_ [private] |