Diff: Lib_Cntrl/PID_Cntrl.cpp

Revision:

1:25a2b47ca291

Parent:

0:d2e117716219

--- 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