#include <sr_plain_pid.hpp>
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 PID gains for the controller. More... | |
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. More... | |
double | getCurrentCmd () |
Return current command for this PID controller. More... | |
void | getCurrentPIDErrors (double *pe, double *ie, double *de) |
Return PID error terms for the controller. More... | |
void | getGains (double &p, double &i, double &d, double &i_max, double &i_min) |
Get PID gains for the controller. More... | |
void | getGains (double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup) |
bool | init (const ros::NodeHandle &n, const bool quiet=false) |
Initialize PID with the parameters in a NodeHandle namespace Initializes dynamic reconfigure for PID gains. More... | |
bool | initParam (const std::string &prefix, const bool quiet=false) |
Initialize PID with the parameters in a namespace Initializes dynamic reconfigure for PID gains. More... | |
void | initPid (double p, double i, double d, double i_max, double i_min, bool antiwindup=false) |
Zeros out Pid values and initialize Pid-gains and integral term limits Does not initialize dynamic reconfigure for PID gains. More... | |
bool | initXml (TiXmlElement *config) |
Initialize PID with the parameters in an XML element Initializes dynamic reconfigure for PID gains. More... | |
PlainPid (double p=0.0, double i=0.0, double d=0.0, double i_max=0.0, double i_min=-0.0, bool antiwindup=false) | |
Constructor, zeros out Pid values when created and initialize Pid-gains and integral term limits. Does not initialize dynamic reconfigure for PID gains. More... | |
void | printValues () |
Print to console the current parameters. More... | |
void | reset () |
Reset the state of this PID controller. More... | |
void | setCurrentCmd (double cmd) |
Set current command for this PID controller. More... | |
void | setGains (double p, double i, double d, double i_max, double i_min, bool antiwindup=false) |
Set PID gains for the controller. More... | |
ROS_DEPRECATED double | updatePid (double p_error, ros::Duration dt) |
Update the Pid loop with nonuniform time step size. More... | |
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. More... | |
~PlainPid () | |
Destructor of Pid class. More... | |
Public Attributes | |
Gains | pid_gains |
Get PID gains for the controller. More... | |
Private Attributes | |
double | cmd_ |
double | d_error_ |
double | i_error_ |
double | p_error_ |
double | p_error_last_ |
Definition at line 110 of file sr_plain_pid.hpp.
controller::PlainPid::PlainPid | ( | double | p = 0.0 , |
double | i = 0.0 , |
||
double | d = 0.0 , |
||
double | i_max = 0.0 , |
||
double | i_min = -0.0 , |
||
bool | antiwindup = false |
||
) |
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. |
Definition at line 52 of file sr_plain_pid.cpp.
controller::PlainPid::~PlainPid | ( | ) |
Destructor of Pid class.
Definition at line 59 of file sr_plain_pid.cpp.
double controller::PlainPid::computeCommand | ( | double | error, |
ros::Duration | dt | ||
) |
Set PID gains for the controller.
gains | A struct of the PID gain values |
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 |
Definition at line 172 of file sr_plain_pid.cpp.
double controller::PlainPid::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 |
Definition at line 194 of file sr_plain_pid.cpp.
double controller::PlainPid::getCurrentCmd | ( | ) |
Return current command for this PID controller.
Definition at line 249 of file sr_plain_pid.cpp.
void controller::PlainPid::getCurrentPIDErrors | ( | double * | pe, |
double * | ie, | ||
double * | de | ||
) |
Return PID error terms for the controller.
pe | The proportional error. |
ie | The integral error. |
de | The derivative error. |
Definition at line 254 of file sr_plain_pid.cpp.
void controller::PlainPid::getGains | ( | double & | p, |
double & | i, | ||
double & | d, | ||
double & | i_max, | ||
double & | i_min | ||
) |
Get PID gains for the controller.
p | The proportional gain. |
i | The integral gain. |
d | The derivative gain. |
i_max | The max integral windup. |
i_min | The min integral windup. |
Definition at line 146 of file sr_plain_pid.cpp.
void controller::PlainPid::getGains | ( | double & | p, |
double & | i, | ||
double & | d, | ||
double & | i_max, | ||
double & | i_min, | ||
bool & | antiwindup | ||
) |
Definition at line 152 of file sr_plain_pid.cpp.
bool controller::PlainPid::init | ( | const ros::NodeHandle & | n, |
const bool | quiet = false |
||
) |
Initialize PID with the parameters in a NodeHandle namespace Initializes dynamic reconfigure for PID gains.
n | The NodeHandle which should be used to query parameters. |
quiet | If true, no error messages will be emitted on failure. |
Definition at line 76 of file sr_plain_pid.cpp.
bool controller::PlainPid::initParam | ( | const std::string & | prefix, |
const bool | quiet = false |
||
) |
Initialize PID with the parameters in a namespace Initializes dynamic reconfigure for PID gains.
prefix | The namespace prefix. |
quiet | If true, no error messages will be emitted on failure. |
Definition at line 70 of file sr_plain_pid.cpp.
void controller::PlainPid::initPid | ( | double | p, |
double | i, | ||
double | d, | ||
double | i_max, | ||
double | i_min, | ||
bool | antiwindup = false |
||
) |
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. |
Definition at line 63 of file sr_plain_pid.cpp.
bool controller::PlainPid::initXml | ( | TiXmlElement * | config | ) |
Initialize PID with the parameters in an XML element Initializes dynamic reconfigure for PID gains.
config | the XML element |
Definition at line 119 of file sr_plain_pid.cpp.
void controller::PlainPid::printValues | ( | ) |
Print to console the current parameters.
Definition at line 261 of file sr_plain_pid.cpp.
void controller::PlainPid::reset | ( | ) |
Reset the state of this PID controller.
Definition at line 137 of file sr_plain_pid.cpp.
void controller::PlainPid::setCurrentCmd | ( | double | cmd | ) |
Set current command for this PID controller.
Definition at line 244 of file sr_plain_pid.cpp.
void controller::PlainPid::setGains | ( | double | p, |
double | i, | ||
double | d, | ||
double | i_max, | ||
double | i_min, | ||
bool | antiwindup = false |
||
) |
Set PID gains for the controller.
p | The proportional gain. |
i | The integral gain. |
d | The derivative gain. |
i_max | The max integral windup. |
i_min | The min integral windup. |
Definition at line 162 of file sr_plain_pid.cpp.
double controller::PlainPid::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 |
Definition at line 189 of file sr_plain_pid.cpp.
double controller::PlainPid::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 |
Definition at line 239 of file sr_plain_pid.cpp.
|
private |
Command to send.
Definition at line 336 of file sr_plain_pid.hpp.
|
private |
Derivative of position error.
Definition at line 335 of file sr_plain_pid.hpp.
|
private |
Integral of position error.
Definition at line 334 of file sr_plain_pid.hpp.
|
private |
Position error.
Definition at line 333 of file sr_plain_pid.hpp.
|
private |
_Save position state for derivative state calculation.
Definition at line 332 of file sr_plain_pid.hpp.
Gains controller::PlainPid::pid_gains |
Get PID gains for the controller.
Definition at line 234 of file sr_plain_pid.hpp.