Diff: PID_Cntrl.cpp

Revision:

20:b142ae11a12a

Parent:

16:ff375f62a95f

diff -r 2ffbd2af2b7f -r b142ae11a12a PID_Cntrl.cpp
--- a/PID_Cntrl.cpp	Wed Aug 19 14:05:22 2020 +0000
+++ b/PID_Cntrl.cpp	Thu Aug 20 13:50:22 2020 +0000
@@ -1,14 +1,14 @@
-/*  
+/*
     PI Controller class with anti windup reset in biquad transposed direct form 2
     see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
     everything is calculated in double
-    
-                  Tn*s + 1                      
+
+                  Tn*s + 1
       G(s) = Kp -------------  with s ~ (1 - z^-1)/Ts
-                    Ts*s              
-                    
+                    Ts*s
+
 #include "PI_Cntrl.h"
-using namespace std;       
+using namespace std;
 
 */
 
@@ -21,10 +21,10 @@
       G(s) = Kp + Ki --- + Kd ---------
                       s       T_f*s + p
 
-    Eigther reseting the Nucleo via the black button or save a new software on 
+    Eigther reseting the Nucleo via the black button or save a new software on
     the Nucleo sets the analog output to zero. Zero is equal to -4 Ampere!!!
     Therefor: NEVER !!! reset or save a new software while the VC is powered on
-    (the green button on the VC is glowing green)                      
+    (the green button on the VC is glowing green)
 
 */
 
@@ -41,14 +41,14 @@
     Ts_ = Ts;
     uMin_ = uMin;
     uMax_ = uMax;
-    
-    
+
+
     reset(0.0f);
 }
 
 PID_Cntrl::~PID_Cntrl() {}
 
-void PID_Cntrl::reset(float initValue)
+float PID_Cntrl::reset(float initValue)
 {
 
     // implement controller reset
@@ -61,55 +61,66 @@
     D_old = 0;
     delta_error = 0;
     e_old = 0;
-    
+    return I_old+I_new;
+
 }
 
-float PID_Cntrl::update(double e)
+float PID_Cntrl::update(double e) // WITHOUT D_PART
 {
-    
+
     // Controller Input Value --> e
+
+    // controller update function
+
+    // Delta Error (for D-Part)
+    //delta_error = e - e_old;
+
+    // calculate u
+     P_new = Kp_ * e;
     
-    // controller update function 
-    
-    // Delta Error (for D-Part)
-    delta_error = e - e_old;
+    // calculate I-Part Output  
+     I_new = I_old + Ts_ * Ki_ * e_old;
     
-    // calculate I-Part Output
-    float I_new = I_old + Ts_ * Ki_ * e_old;
-   
+    //
+    float PID_output_temp = P_new + I_new;
+    
+
     // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
-    if(I_new > uMax_) {
-        I_new = uMax_;
+    if(PID_output_temp > uMax_) {
+        PID_output = uMax_;
+    }
+    else if(PID_output_temp < uMin_) {
+        PID_output = uMin_;
     }
-    if(I_new < uMin_) {
-        I_new = uMin_;
+    else{
+        I_old = I_new;
+        PID_output = PID_output_temp;
     }
-    
+
     // calculate uD
     //D_new = (Kd_*delta_error + Tf_*D_old - Ts_*D_old)/(Tf_);
-    D_new = 0;
-        
-    // calculate u
-    P_new = Kp_ * e;
-     
-    
+    //D_new = 0;
+
+
+
+
     // update signal storage
-    P_old = P_new;
-    I_old = I_new;
-    D_old = D_new;
-    e_old = e;
-    
+    //P_old = P_new;
+    //I_old = I_new;
+    //D_old = D_new;
+    //e_old = e;
+
     // PID Output
-    PID_output = P_new + I_new + D_new;
-    
+    //PID_output = P_new + I_new + D_new;
+
     // saturate u, uMin <= u <= uMax
-    if(PID_output > uMax_) {
-        PID_output = uMax_;
-    }
-    if(PID_output < uMin_) {
-        PID_output = uMin_;
-    }
-    
+    //if(PID_output > uMax_) {
+    //    PID_output = uMax_;
+    //}
+    //if(PID_output < uMin_) {
+    //    PID_output = uMin_;
+    //}
+
     return PID_output;
 }