Diff: PID_Cntrl.cpp

Revision:

10:447f6a864598

Parent:

5:d8c53cece01b

Child:

11:7ea3efaf0469

--- a/PID_Cntrl.cpp	Fri Sep 20 09:26:23 2019 +0000
+++ b/PID_Cntrl.cpp	Mon Sep 23 08:59:29 2019 +0000
@@ -24,13 +24,13 @@
 void PID_Cntrl::reset(float initValue)
 {
     Iold = initValue;
-    eold = 0.0;yold = 0.0;
+    eold = 0.0;yold = 0.0;Dpart_old = 0.0;
     del = 0.0;
 }
 
 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
 {
-    this->p = 1.0 - Ts/tau_f;
+    this->p = 1.0f - Ts/tau_f;
     this->P = P;
     this->P_init = P;
     this->I = I;
@@ -67,7 +67,7 @@
 float PID_Cntrl::doStep(float e)
 {
     float Ipart = Iold+I*Ts*(e-del);
-    float Dpart = D_*(e-eold)+p*yold;
+    float Dpart = D_*(e-eold)+p*Dpart_old;
     float u = P*e + Dpart  + Ipart;          // unconstrained output
     float uc = u;                // constrained output
     if(u > uMax) uc = uMax;
@@ -75,7 +75,22 @@
     del=(u-uc)*Ka;
     eold=e;
     Iold=Ipart;
-    yold=Dpart;
+    Dpart_old=Dpart;
+    return uc;
+}
+float PID_Cntrl::doStep(float e,float y)
+{
+    float Ipart = Iold+I*Ts*(e-del);
+    float Dpart = D_ * (y-yold) + p * Dpart_old;
+    float u = P*e - Dpart  + Ipart;          // unconstrained output
+    float uc = u;                // constrained output
+    if(u > uMax) uc = uMax;
+    else if(u < uMin) uc = uMin;
+    del=(u-uc)*Ka;
+    eold=e;
+    Iold=Ipart;
+    Dpart_old = Dpart;
+    yold = y;
     return uc;
 }