Ruprecht Altenburger
/
mirror_actuator_preston
corrections altb2
Diff: Lib_Cntrl/PID_Cntrl.cpp
- Revision:
- 1:25a2b47ca291
- Parent:
- 0:d2e117716219
diff -r d2e117716219 -r 25a2b47ca291 Lib_Cntrl/PID_Cntrl.cpp --- a/Lib_Cntrl/PID_Cntrl.cpp Sun May 02 19:32:30 2021 +0000 +++ b/Lib_Cntrl/PID_Cntrl.cpp Thu Aug 05 08:27:51 2021 +0000 @@ -5,20 +5,42 @@ 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 - return 0.0; + 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; } \ No newline at end of file