#include <pid.h>
Public Member Functions | |
double | clamp (const double &value, const double &lower_limit, const double &upper_limit) |
Clam given value to upper and lower limits. More... | |
void | dynamicReconfigCallback (diffbot_base::ParametersConfig &config, uint32_t) |
Update the PID parameters from dynamics reconfigure. More... | |
double | getError () const |
Get the current error. More... | |
void | getParameters (double &f, double &p, double &i, double &d, double &i_max, double &i_min) |
Get the FPID parameters. More... | |
void | getParameters (double &f, double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup) |
Get the FPID parameters. More... | |
void | init (ros::NodeHandle &nh, double f, double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min) |
Initialize the. More... | |
void | initDynamicReconfig (ros::NodeHandle &node) |
Start the dynamic reconfigure node and load the default values. More... | |
double | operator() (const double &measured_value, const double &setpoint, const ros::Duration &dt) |
Compute PID output value from error using process value, set point and time period. More... | |
PID (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, double out_max=0.0, double out_min=0.0) | |
Construct a new PID object. More... | |
void | setOutputLimits (double out_min, double out_max) |
Set the Output Limits of the PID controller. More... | |
void | setParameters (double f, double p, double i, double d, double i_max, double i_min, bool antiwindup=false) |
Set the Parameters using the Gains object of control_toolbox::Pid. More... | |
void | updateDynamicReconfig () |
Set Dynamic Reconfigure's gains to PID's values. More... | |
void | updateDynamicReconfig (diffbot_base::ParametersConfig config) |
void | updateDynamicReconfig (Gains gains_config) |
![]() | |
double | computeCommand (double error, double error_dot, ros::Duration dt) |
double | computeCommand (double error, ros::Duration dt) |
void | dynamicReconfigCallback (control_toolbox::ParametersConfig &config, uint32_t) |
double | getCurrentCmd () |
void | getCurrentPIDErrors (double *pe, double *ie, double *de) |
Gains | getGains () |
void | getGains (double &p, double &i, double &d, double &i_max, double &i_min) |
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) |
void | initDynamicReconfig (ros::NodeHandle &node) |
bool | initParam (const std::string &prefix, const bool quiet=false) |
void | initPid (double p, double i, double d, double i_max, double i_min, bool antiwindup, const ros::NodeHandle &) |
void | initPid (double p, double i, double d, double i_max, double i_min, bool antiwindup=false) |
void | initPid (double p, double i, double d, double i_max, double i_min, const ros::NodeHandle &) |
bool | initXml (TiXmlElement *config) |
Pid & | operator= (const Pid &source) |
Pid (const Pid &source) | |
Pid (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) | |
void | printValues () |
void | reset () |
void | reset (double d_error, double i_error) |
void | setCurrentCmd (double cmd) |
void | setGains (const Gains &gains) |
void | setGains (double p, double i, double d, double i_max, double i_min, bool antiwindup=false) |
void | updateDynamicReconfig () |
void | updateDynamicReconfig (control_toolbox::ParametersConfig config) |
void | updateDynamicReconfig (Gains gains_config) |
ROS_DEPRECATED double | updatePid (double error, double error_dot, ros::Duration dt) |
ROS_DEPRECATED double | updatePid (double p_error, ros::Duration dt) |
~Pid () | |
Private Types | |
typedef dynamic_reconfigure::Server< diffbot_base::ParametersConfig > | DynamicReconfigServer |
Private Attributes | |
bool | dynamic_reconfig_initialized_ |
double | error_ |
double | f_ |
double | out_max_ |
double | out_min_ |
DynamicReconfigServer::CallbackType | param_reconfig_callback_ |
boost::recursive_mutex | param_reconfig_mutex_ |
boost::shared_ptr< DynamicReconfigServer > | param_reconfig_server_ |
Definition at line 11 of file include/diffbot_base/pid.h.
|
private |
Definition at line 142 of file include/diffbot_base/pid.h.
diffbot_base::PID::PID | ( | 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 , |
||
double | out_max = 0.0 , |
||
double | out_min = 0.0 |
||
) |
Construct a new PID object.
p | The proportional gain. |
i | The integral gain. |
d | The derivative gain. |
i_max | The max integral windup. |
i_min | The min integral windup. |
antiwindup | Enable or disable antiwindup of the integrator. |
out_min | The min computed output. |
out_max | The max computed output. |
Definition at line 6 of file src/pid.cpp.
double diffbot_base::PID::clamp | ( | const double & | value, |
const double & | lower_limit, | ||
const double & | upper_limit | ||
) |
Clam given value to upper and lower limits.
value | Input value that's possibly clamped. |
lower_limit | Lower limit which the value must not exceed. |
upper_limit | Upper limit which the value must not exceed. |
Definition at line 95 of file src/pid.cpp.
void diffbot_base::PID::dynamicReconfigCallback | ( | diffbot_base::ParametersConfig & | config, |
uint32_t | |||
) |
Update the PID parameters from dynamics reconfigure.
Definition at line 157 of file src/pid.cpp.
|
inline |
Get the current error.
Definition at line 114 of file include/diffbot_base/pid.h.
void diffbot_base::PID::getParameters | ( | double & | f, |
double & | p, | ||
double & | i, | ||
double & | d, | ||
double & | i_max, | ||
double & | i_min | ||
) |
Get the FPID parameters.
f | The feed forward gain. |
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 65 of file src/pid.cpp.
void diffbot_base::PID::getParameters | ( | double & | f, |
double & | p, | ||
double & | i, | ||
double & | d, | ||
double & | i_max, | ||
double & | i_min, | ||
bool & | antiwindup | ||
) |
Get the FPID parameters.
f | The feed forward gain. |
p | The proportional gain. |
i | The integral gain. |
d | The derivative gain. |
i_max | The max integral windup. |
i_min | The min integral windup. |
antiwindup | Enable or disable antiwindup check. |
Definition at line 71 of file src/pid.cpp.
void diffbot_base::PID::init | ( | ros::NodeHandle & | nh, |
double | f, | ||
double | p, | ||
double | i, | ||
double | d, | ||
double | i_max, | ||
double | i_min, | ||
bool | antiwindup, | ||
double | out_max, | ||
double | out_min | ||
) |
Initialize the.
nh | ROS node handle with possible namespace describing the purpose of the PID controller. |
f | The feed forward gain. |
p | The proportional gain. |
i | The integral gain. |
d | The derivative gain. |
i_max | The max integral windup. |
i_min | The min integral windup. |
antiwindup | |
out_min | The min computed output. |
out_max | The max computed output. |
Definition at line 18 of file src/pid.cpp.
void diffbot_base::PID::initDynamicReconfig | ( | ros::NodeHandle & | node | ) |
Start the dynamic reconfigure node and load the default values.
node | - a node handle where dynamic reconfigure services will be published |
Definition at line 111 of file src/pid.cpp.
double diffbot_base::PID::operator() | ( | const double & | measured_value, |
const double & | setpoint, | ||
const ros::Duration & | dt | ||
) |
Compute PID output value from error using process value, set point and time period.
measured_value | The process value measured by sensors. |
setpoint | The desired target value. |
dt | The delta time or period since the last call. |
Definition at line 35 of file src/pid.cpp.
void diffbot_base::PID::setOutputLimits | ( | double | out_min, |
double | out_max | ||
) |
Set the Output Limits of the PID controller.
out_min | |
out_max |
Definition at line 87 of file src/pid.cpp.
void diffbot_base::PID::setParameters | ( | double | f, |
double | p, | ||
double | i, | ||
double | d, | ||
double | i_max, | ||
double | i_min, | ||
bool | antiwindup = false |
||
) |
Set the Parameters using the Gains object of control_toolbox::Pid.
f | The feed forward gain. |
p | The proportional gain. |
i | The integral gain. |
d | The derivative gain. |
i_max | The max integral windup. |
i_min | The min integral windup. |
antiwindup | Enable or disable antiwindup check. |
Definition at line 78 of file src/pid.cpp.
void diffbot_base::PID::updateDynamicReconfig | ( | ) |
Set Dynamic Reconfigure's gains to PID's values.
Definition at line 129 of file src/pid.cpp.
void diffbot_base::PID::updateDynamicReconfig | ( | diffbot_base::ParametersConfig | config | ) |
Definition at line 145 of file src/pid.cpp.
void diffbot_base::PID::updateDynamicReconfig | ( | Gains | gains_config | ) |
|
private |
Definition at line 141 of file include/diffbot_base/pid.h.
|
private |
Definition at line 136 of file include/diffbot_base/pid.h.
|
private |
Definition at line 135 of file include/diffbot_base/pid.h.
|
private |
Definition at line 138 of file include/diffbot_base/pid.h.
|
private |
Definition at line 137 of file include/diffbot_base/pid.h.
|
private |
Definition at line 144 of file include/diffbot_base/pid.h.
|
private |
Definition at line 146 of file include/diffbot_base/pid.h.
|
private |
Definition at line 143 of file include/diffbot_base/pid.h.