Diff: PID_Cntrl.cpp

Revision:

2:394782101261

Parent:

1:92f175969d90

--- a/PID_Cntrl.cpp	Fri May 10 14:25:24 2019 +0000
+++ b/PID_Cntrl.cpp	Thu May 16 15:34:29 2019 +0000
@@ -15,10 +15,15 @@
 #include "PID_Cntrl.h"
 using namespace std;
 
-PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax)
+PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float yMin, float yMax)
 {
     // link member variables
-    // ???
+    this->Kp = Kp;
+    this->Ki = Ki;
+    this->Tf = Tf;
+    this->Ts = Ts;
+    this->yMin = yMin;
+    this->yMax = yMax;
     
     reset(0.0f);
 }
@@ -29,33 +34,46 @@
 {
 
     // implement controller reset
-    // ???
-
+    e_old = 0.0;
+    y_Dold = 0.0;
+    y_I = initValue; 
 }
 
-float PID_Cntrl::update(double e)
+float PID_Cntrl::update(float e)
 {
 
     // controller update function 
     
     // calculate uI
-    // ???
+    y_I = y_I + Ki*Ts*e; 
     
     // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
-    // ???
+    if(y_I > yMax)
+        y_I = yMax;
+    else if(y_I < yMin)
+        y_I = yMin; 
 
     // calculate uD
-    // ???
+    y_Dold = (1.0f - Ts/Tf)*y_Dold + Kd / Tf * (e-e_old);
+    
+    float y = Kp * e + y_I + y_Dold;
+    
+    // update signal storage
+    e_old = e; 
+    
     
     // calculate u
     // ???
     
     // saturate u, uMin <= u <= uMax
-    // ???
+    if(y > yMax)
+        y = yMax;
+    else if(y < yMin)
+        y = yMin; 
     
     // update signal storage
     // ???
 
-    return 0.0f;
+    return y;
 }