Diff: PID_Cntrl.cpp

Revision:

2:1ded9d10f322

Parent:

1:92f175969d90

--- a/PID_Cntrl.cpp	Fri May 10 14:25:24 2019 +0000
+++ b/PID_Cntrl.cpp	Tue May 14 15:29:24 2019 +0000
@@ -18,7 +18,15 @@
 PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax)
 {
     // link member variables
-    // ???
+    this->Kp = Kp;
+    this->Ki = Ki;
+    this->Kd = Kd;
+    
+    this->Tf = Tf;
+    this->Ts = Ts;
+    
+    this->uMin = uMin;
+    this->uMax = uMax;
     
     reset(0.0f);
 }
@@ -29,7 +37,9 @@
 {
 
     // implement controller reset
-    // ???
+    e_old = 0.0f;
+    uI_old = initValue;
+    uD_old = 0.0f;
 
 }
 
@@ -39,23 +49,31 @@
     // controller update function 
     
     // calculate uI
-    // ???
+    float uI = Ki * Ts * e + uI_old;
     
     // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
-    // ???
+    if (uI > uMax)
+        uI = uMax;
+    else if (uI < uMin)
+        uI = uMin;
 
     // calculate uD
-    // ???
+    float uD = (Kd * (e - e_old) + Tf * uD_old) / (Tf + Ts);
     
     // calculate u
-    // ???
+    float u = (Kp * e) + uI + uD;
     
     // saturate u, uMin <= u <= uMax
-    // ???
+    if (u > uMax)
+        u = uMax;
+    else if (u < uMin)
+        u = uMin;
     
     // update signal storage
-    // ???
+    e_old = e;
+    uI_old = uI;
+    uD_old = uD;
 
-    return 0.0f;
+    return u;
 }