![](/media/cache/profiles/altb_JGUSAOG.jpg.50x50_q85.jpg)
corrections altb2
Lib_Cntrl/PID_Cntrl.cpp
- Committer:
- ernstpre
- Date:
- 2021-08-05
- Revision:
- 1:25a2b47ca291
- Parent:
- 0:d2e117716219
File content as of revision 1:25a2b47ca291:
/* */ #include "PID_Cntrl.h" PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax) { this->P = P; this->I = I; this->D = D; this->tau_f = tau_f; this->Ts = Ts; this->uMin = uMin; this->uMax = uMax; reset(0); } PID_Cntrl::~PID_Cntrl() {} void PID_Cntrl::reset(float initValue) { Ipart = initValue; Dpart = 0.0f; e_old = 0.0f; } float PID_Cntrl::update(float e) { // the main update float Ppart = P * e; Ipart += I * Ts*e; Dpart = Dpart*tau_f/(tau_f + Ts) + D/(tau_f + Ts)*(e - e_old); Ipart = saturate(Ipart); return saturate(Ppart + Ipart + Dpart); } float PID_Cntrl::saturate(float x) { if(x > uMax) return x = uMax; else if (x < uMin) return x = uMin; return x; }