Diff: PID_Cntrl.cpp

Revision:

0:d49418189c5c

Child:

2:e7c9e50a2e46

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Cntrl.cpp	Mon Mar 04 11:03:08 2019 +0000
@@ -0,0 +1,69 @@
+/*  
+    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
+    
+                     Ts         z - 1             
+      G(s) = P + I ------- + D -------          D corresponds Kd/Tf in Matlab-formlism pid(...)
+                    z - 1       z - p              
+*/
+
+#include "PID_Cntrl.h"
+using namespace std;
+
+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->uMin = (double)uMin;
+    this->uMax = (double)uMax;
+    reset(0.0f);
+}
+
+PID_Cntrl::~PID_Cntrl() {}
+
+void PID_Cntrl::reset(float initValue)
+{
+    Iold = (double)initValue;
+    eold = 0.0;yold = 0.0;
+    del = 0.0;
+}
+
+void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
+{
+    this->p = 1.0 - (double)Ts/(double)tau_f;
+    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 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)*Ka;
+    eold=e;
+    Iold=Ipart;
+    yold=Dpart;
+    return (float)uc;
+}
+
+void PID_Cntrl::set_limits(double ll, double ul)
+{
+    this->uMin = (double)ll;
+    this->uMax = (double)ul;
+}
+
+float PID_Cntrl::get_ulimit(void)
+{
+    return (float)this->uMax;
+}
\ No newline at end of file