#include <pid.h>
Public Member Functions | |
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. | |
bool | init (const ros::NodeHandle &n) |
Initialize PID with the parameters in a NodeHandle namespace. | |
bool | initParam (const std::string &prefix) |
Initialize PID with the parameters in a namespace. | |
void | initPid (double P, double I, double D, double I1, double I2) |
Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]. | |
bool | initXml (TiXmlElement *config) |
Pid & | operator= (const Pid &p) |
Pid (double P=0.0, double I=0.0, double D=0.0, double I1=0.0, double I2=-0.0) | |
Constructor, zeros out Pid values when created and initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2]. | |
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. | |
double | updatePid (double p_error, ros::Duration dt) |
Update the Pid loop with nonuniform time step size. | |
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 Attributes | |
double | cmd_ |
double | d_error_ |
double | d_gain_ |
double | i_error_ |
double | i_gain_ |
double | i_max_ |
double | i_min_ |
double | p_error_ |
double | p_error_last_ |
double | p_gain_ |
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; }
control_toolbox::Pid::Pid | ( | double | P = 0.0 , |
double | I = 0.0 , |
||
double | D = 0.0 , |
||
double | I1 = 0.0 , |
||
double | I2 = -0.0 |
||
) |
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 | ) |
bool control_toolbox::Pid::initParam | ( | const std::string & | prefix | ) |
void control_toolbox::Pid::initPid | ( | double | P, |
double | I, | ||
double | D, | ||
double | I1, | ||
double | I2 | ||
) |
bool control_toolbox::Pid::initXml | ( | TiXmlElement * | config | ) |
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 | ||
) |
double control_toolbox::Pid::updatePid | ( | double | p_error, |
ros::Duration | dt | ||
) |
double control_toolbox::Pid::updatePid | ( | double | error, |
double | error_dot, | ||
ros::Duration | dt | ||
) |
double control_toolbox::Pid::cmd_ [private] |
double control_toolbox::Pid::d_error_ [private] |
double control_toolbox::Pid::d_gain_ [private] |
double control_toolbox::Pid::i_error_ [private] |
double control_toolbox::Pid::i_gain_ [private] |
double control_toolbox::Pid::i_max_ [private] |
double control_toolbox::Pid::i_min_ [private] |
double control_toolbox::Pid::p_error_ [private] |
double control_toolbox::Pid::p_error_last_ [private] |
double control_toolbox::Pid::p_gain_ [private] |