17 #ifndef IGNITION_MATH_PID_HH_
18 #define IGNITION_MATH_PID_HH_
22 #include <ignition/math/config.hh>
29 inline namespace IGNITION_MATH_VERSION_NAMESPACE {
38 class IGNITION_MATH_VISIBLE
PID
57 public:
PID(
const double _p = 0.0,
58 const double _i = 0.0,
59 const double _d = 0.0,
60 const double _imax = -1.0,
61 const double _imin = 0.0,
62 const double _cmdMax = -1.0,
63 const double _cmdMin = 0.0,
64 const double _cmdOffset = 0.0);
86 public:
void Init(
const double _p = 0.0,
87 const double _i = 0.0,
88 const double _d = 0.0,
89 const double _imax = -1.0,
90 const double _imin = 0.0,
91 const double _cmdMax = -1.0,
92 const double _cmdMin = 0.0,
93 const double _cmdOffset = 0.0);
167 public:
double Update(
const double _error,
176 public:
double Cmd()
const;
182 public:
void Errors(
double &_pe,
double &_ie,
double &_de)
const;
193 private:
double pErrLast = 0.0;
196 private:
double pErr = 0.0;
199 private:
double iErr = 0.0;
202 private:
double dErr = 0.0;
205 private:
double pGain;
208 private:
double iGain = 0.0;
211 private:
double dGain = 0.0;
214 private:
double iMax = -1.0;
217 private:
double iMin = 0.0;
220 private:
double cmd = 0.0;
223 private:
double cmdMax = -1.0;
226 private:
double cmdMin = 0.0;
229 private:
double cmdOffset = 0.0;
Generic PID controller class. Generic proportional-integral-derivative controller class that keeps tr...
Definition: PID.hh:39
double Cmd() const
Return current command for this PID controller.
void SetCmdOffset(const double _c)
Set the offset value for the command, which is added to the result of the PID controller.
void SetIMax(const double _i)
Set the integral upper limit.
PID & operator=(const PID &_p)
Assignment operator.
void Reset()
Reset the errors and command.
void SetPGain(const double _p)
Set the proportional Gain.
void Init(const double _p=0.0, const double _i=0.0, const double _d=0.0, const double _imax=-1.0, const double _imin=0.0, const double _cmdMax=-1.0, const double _cmdMin=0.0, const double _cmdOffset=0.0)
Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2].
void Errors(double &_pe, double &_ie, double &_de) const
Return PID error terms for the controller.
double IGain() const
Get the integral Gain.
void SetIGain(const double _i)
Set the integral Gain.
void SetCmdMax(const double _c)
Set the maximum value for the command.
void SetCmdMin(const double _c)
Set the minimum value for the command.
void SetCmd(const double _cmd)
Set current target command for this PID controller.
double Update(const double _error, const std::chrono::duration< double > &_dt)
Update the Pid loop with nonuniform time step size.
~PID()=default
Destructor.
double CmdMin() const
Get the maximum value for the command.
double DGain() const
Get the derivative Gain.
void SetIMin(const double _i)
Set the integral lower limit.
void SetDGain(const double _d)
Set the derivtive Gain.
double CmdOffset() const
Get the offset value for the command.
double CmdMax() const
Get the maximum value for the command.
double IMax() const
Get the integral upper limit.
double IMin() const
Get the integral lower limit.
PID(const double _p=0.0, const double _i=0.0, const double _d=0.0, const double _imax=-1.0, const double _imin=0.0, const double _cmdMax=-1.0, const double _cmdMin=0.0, const double _cmdOffset=0.0)
Constructor, zeros out Pid values when created and initialize Pid-gains and integral term limits:[iMa...
double PGain() const
Get the proportional Gain.