My Controller Library

Files at this revision

API Documentation at this revision

Comitter:
altb
Date:
Fri Sep 28 09:01:42 2018 +0000
Parent:
0:e2a7d7f91e49
Commit message:
.

Changed in this revision

LinearCharacteristics.cpp Show diff for this revision Revisions of this file
LinearCharacteristics.h Show diff for this revision Revisions of this file
PID_Cntrl.cpp Show annotated file Show diff for this revision Revisions of this file
PID_Cntrl.h Show annotated file Show diff for this revision Revisions of this file
diff -r e2a7d7f91e49 -r bf62e74fbcf3 LinearCharacteristics.cpp
--- a/LinearCharacteristics.cpp	Fri Sep 28 08:34:20 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,36 +0,0 @@
-#include "LinearCharacteristics.h"
-
-using namespace std;
-
-LinearCharacteristics::LinearCharacteristics(float gain,float offset){    // standard lin characteristics
-    this->gain = gain;
-    this->offset = offset;
-}
-
-LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax){    // standard lin characteristics
-    this->gain = (ymax - ymin)/(xmax - xmin);
-    this->offset = xmax - ymax/this->gain;
-    this->ulim = 999999.0;
-    this->llim = -999999.0;
-    
-}
-LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax,float ll, float ul){    // standard lin characteristics
-    this->gain = (ymax - ymin)/(xmax - xmin);
-    this->offset = xmax - ymax/this->gain;
-    this->llim = ll;
-    this->ulim = ul;
-    
-}
-
-LinearCharacteristics::~LinearCharacteristics() {}
-
-
-float LinearCharacteristics::evaluate(float x)
-{   
-float dum = this->gain*(x - this->offset);
-if(dum > this->ulim)
-    dum = this->ulim;
-if(dum < this->llim)
-    dum = this->llim;
-return dum;
-    }
diff -r e2a7d7f91e49 -r bf62e74fbcf3 LinearCharacteristics.h
--- a/LinearCharacteristics.h	Fri Sep 28 08:34:20 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,30 +0,0 @@
-// Linear Characteristics for different purposes (map Voltage to acc etc.)
-
-
-#ifndef LINEAR_CHARACTERISTICS_H_
-#define LINEAR_CHARACTERISTICS_H_   
-
-
-class LinearCharacteristics{
-     public:
-            LinearCharacteristics(float, float);
-            LinearCharacteristics(float, float, float, float);
-            LinearCharacteristics(float, float, float, float, float, float);
-            float evaluate(float);
-            float operator()(float x){
-                return evaluate(x);
-                } 
-                //...
-                virtual     ~LinearCharacteristics();
-                // here: the calculation function
-    
-    private:
-        // here: private functions and values...
-        float gain;
-        float offset;
-        float ulim;
-        float llim;
-};
-
-
-#endif      // LINEAR_CHARACTERISTICS_H_
\ No newline at end of file
diff -r e2a7d7f91e49 -r bf62e74fbcf3 PID_Cntrl.cpp
--- a/PID_Cntrl.cpp	Fri Sep 28 08:34:20 2018 +0000
+++ b/PID_Cntrl.cpp	Fri Sep 28 09:01:42 2018 +0000
@@ -3,59 +3,47 @@
     see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
     everything is calculated in double
     
-                  Tn*s + 1                      
-      G(s) = Kp -------------  with s ~ (1 - z^-1)/Ts
-                    Ts*s                     
+                     Ts         z - 1             
+      G(s) = P + I ------- + D -------
+                    z - 1       z - p              
 */
 
-#include "PI_Cntrl.h"
+#include "PID_Cntrl.h"
 
 using namespace std;
 
-PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts)
-{
-    setCoefficients(Kp, Tn, Ts);
-    uMax = 10000000000.0;
-    uMin = -uMax;
-    reset(0.0f);
-}
-
-PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax)
+PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMax, float uMin)
 {
-    setCoefficients(Kp, Tn, Ts);
-    this->uMax = (double)uMax;
-    uMin = -(double)uMax;
-    reset(0.0f);
-}
-
-PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin)
-{
-    setCoefficients(Kp, Tn, Ts);
+    setCoefficients(P, I, D, tau_f, Ts);
     this->uMax = (double)uMax;
     this->uMin = (double)uMin;
     reset(0.0f);
 }
 
-PI_Cntrl::~PI_Cntrl() {}
+PID_Cntrl::~PID_Cntrl() {}
 
-void PI_Cntrl::reset(float initValue)
+void PID_Cntrl::reset(float initValue)
 {
-    s = (double)initValue;
+    Iold = (double)initValue;
+    eold = 0.0;yold = 0.0;
+    del = 0.0;
 }
 
-void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts)
+void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
 {
-    b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn);
-    b1 = -(double)Kp;
-    b2 = (double)Ts/(double)Tn;    
+    this->p = 1.0 - (double)Ts/(double)tau_f;
+    this->P = P;
+    this->I = I;
+    this->D = D;
+    
 }
 
-float PI_Cntrl::doStep(double e)
+float PID_Cntrl::doStep(double e)
 {
-    double u = b0*e + s;          // unconstrained output
+    double u = P*e + D*(e-eold)+p*yold  +Iold+I*Ts*(e-del);          // unconstrained output
     double uc = u;                // constrained output
     if(u > uMax) uc = uMax;
     else if(u < uMin) uc = uMin;
-    s = b1*e + u - b2*(u - uc);
+    del=(u-uc)/P;
     return (float)uc;
 }
\ No newline at end of file
diff -r e2a7d7f91e49 -r bf62e74fbcf3 PID_Cntrl.h
--- a/PID_Cntrl.h	Fri Sep 28 08:34:20 2018 +0000
+++ b/PID_Cntrl.h	Fri Sep 28 09:01:42 2018 +0000
@@ -1,8 +1,8 @@
-class PI_Cntrl
+class PID_Cntrl
 {
 public:
 
-    PI_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMax, float uMin);
+    PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMax, float uMin);
 
     float operator()(float error) {
         return doStep((double)error);
@@ -16,10 +16,12 @@
 private:
 
     double Iold;
-    double uk,yk;
+    double eold,yold,del;
     double uMax;
     double uMin;
     double Ts;
-    void setCoefficients(float D, float tau_f, float Ts);
+    double P,I,D;
+    double p;
+    void setCoefficients(float P, float I, float D, float tau_f, float Ts);
 
 };
\ No newline at end of file