Diff: PID_Cntrl.cpp

Revision:

2:1f8ddc46c578

Parent:

1:bf62e74fbcf3

Child:

3:27b75e7dd54a

--- a/PID_Cntrl.cpp	Fri Sep 28 09:01:42 2018 +0000
+++ b/PID_Cntrl.cpp	Fri Sep 28 12:37:19 2018 +0000
@@ -9,10 +9,9 @@
 */
 
 #include "PID_Cntrl.h"
-
 using namespace std;
 
-PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMax, float uMin)
+PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
 {
     setCoefficients(P, I, D, tau_f, Ts);
     this->uMax = (double)uMax;
@@ -35,15 +34,25 @@
     this->P = P;
     this->I = I;
     this->D = D;
+    this->Ts = Ts;
+    if(P!=0)
+        this->Ka=1/P;
+    else
+        this->Ka=1.0f;
     
 }
 
 float PID_Cntrl::doStep(double e)
 {
-    double u = P*e + D*(e-eold)+p*yold  +Iold+I*Ts*(e-del);          // unconstrained output
+    double Ipart = Iold+I*Ts*(e-del);
+    double Dpart = D*(e-eold)+p*yold;
+    double u = P*e + Dpart  + Ipart;          // unconstrained output
     double uc = u;                // constrained output
     if(u > uMax) uc = uMax;
     else if(u < uMin) uc = uMin;
-    del=(u-uc)/P;
+    del=(u-uc)*Ka;
+    eold=e;
+    Iold=Ipart;
+    yold=Dpart;
     return (float)uc;
 }
\ No newline at end of file