Preston Ernst / Mbed OS mirror_actuator_VT

-data logging revision

Dependencies:   FastPWM

Lib_Cntrl/PID_Cntrl.cpp@2:92c25cb669f4, 2021-08-24 (annotated)

Committer:
ernstpre
Date:
Tue Aug 24 08:51:13 2021 +0000
Revision:
2:92c25cb669f4
Parent:
1:25a2b47ca291 
Publish Commit 24/8/21

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:d2e117716219 1 /*
altb2 0:d2e117716219 2 */
altb2 0:d2e117716219 3
altb2 0:d2e117716219 4 #include "PID_Cntrl.h"
altb2 0:d2e117716219 5
altb2 0:d2e117716219 6 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
altb2 0:d2e117716219 7 {
ernstpre 1:25a2b47ca291 8 this->P = P;
ernstpre 1:25a2b47ca291 9 this->I = I;
ernstpre 1:25a2b47ca291 10 this->D = D;
ernstpre 1:25a2b47ca291 11 this->tau_f = tau_f;
ernstpre 1:25a2b47ca291 12 this->Ts = Ts;
ernstpre 1:25a2b47ca291 13 this->uMin = uMin;
ernstpre 1:25a2b47ca291 14 this->uMax = uMax;
ernstpre 1:25a2b47ca291 15 reset(0);
ernstpre 1:25a2b47ca291 16
ernstpre 1:25a2b47ca291 17
altb2 0:d2e117716219 18 }
altb2 0:d2e117716219 19
altb2 0:d2e117716219 20 PID_Cntrl::~PID_Cntrl() {}
altb2 0:d2e117716219 21
altb2 0:d2e117716219 22 void PID_Cntrl::reset(float initValue)
altb2 0:d2e117716219 23 {
ernstpre 1:25a2b47ca291 24 Ipart = initValue;
ernstpre 1:25a2b47ca291 25 Dpart = 0.0f;
ernstpre 1:25a2b47ca291 26 e_old = 0.0f;
altb2 0:d2e117716219 27 }
altb2 0:d2e117716219 28
altb2 0:d2e117716219 29
altb2 0:d2e117716219 30 float PID_Cntrl::update(float e)
altb2 0:d2e117716219 31 {
altb2 0:d2e117716219 32 // the main update
ernstpre 1:25a2b47ca291 33 float Ppart = P * e;
ernstpre 1:25a2b47ca291 34 Ipart += I * Ts*e;
ernstpre 1:25a2b47ca291 35 Dpart = Dpart*tau_f/(tau_f + Ts) + D/(tau_f + Ts)*(e - e_old);
ernstpre 1:25a2b47ca291 36 Ipart = saturate(Ipart);
ernstpre 1:25a2b47ca291 37 return saturate(Ppart + Ipart + Dpart);
ernstpre 1:25a2b47ca291 38 }
ernstpre 1:25a2b47ca291 39 float PID_Cntrl::saturate(float x)
ernstpre 1:25a2b47ca291 40 {
ernstpre 1:25a2b47ca291 41 if(x > uMax)
ernstpre 1:25a2b47ca291 42 return x = uMax;
ernstpre 1:25a2b47ca291 43 else if (x < uMin)
ernstpre 1:25a2b47ca291 44 return x = uMin;
ernstpre 1:25a2b47ca291 45 return x;
altb2 0:d2e117716219 46 }