Diff: pid.h

Revision:

40:19d51f6e6800

Parent:

38:b760c09b311c

Child:

42:b69538bba4f9

--- a/pid.h	Wed Jun 28 16:36:16 2017 +0000
+++ b/pid.h	Sun Jul 02 19:01:07 2017 +0000
@@ -1,6 +1,10 @@
 #ifndef _PID_H_
 #define _PID_H_
 
+#include "mbed.h"
+
+
+DigitalOut      led2(LED2);
 //http://coder-tronics.com/pid-tutorial-c-code-example-pt2/
 
 class Pid
@@ -12,6 +16,9 @@
         double integrator, derivator;
         double maxInteg, minInteg;
         double maxPwm, minPwm;
+        double lastInput;
+        double lastPwm;
+        double middlePoint;
         
     public:
         Pid(double Kp, double Ki, double Kd)
@@ -19,30 +26,40 @@
             this->Kp = Kp;
             this->Ki = Ki;
             this->Kd = Kd;
-            
+            this->middlePoint = 0.08;
             integrator = 0;
             derivator = 0;
             
-            maxInteg = 3;
-            minInteg = -3;
+            maxInteg = 1.0;
+            minInteg = -1.0;
             
             maxPwm = 1;
             minPwm = 0.05;
+            lastInput = 0.5;
+            lastPwm = 0.5;
         }
-        
-        //desired value
-        void setSetPoint(double setPoint)
+        void addKp(double value) {Kp += value;}
+        void addKi(double value) {Ki += value;}
+        void addKd(double value) {Kd += value;}
+        double getKp() {return Kp;}
+        double getKi() {return Ki;}
+        double getKd() {return Kd;}
+        void setParameters(double Kp, double Ki, double Kd)
         {
-            this->setPoint = setPoint;
+            this->Kp = Kp;
+            this->Ki = Ki;
+            this->Kd = Kd;
         }
-        double getSetPoint()
-        {
-            return setPoint;
-        }
+        //desired value
+        void setSetPoint(double setPoint)   {this->setPoint = setPoint;}
+        double getSetPoint()                {return setPoint;}
+        double getLastInput()               {return lastInput;}
+        double getLastPwm()                 {return lastPwm;}
         
         //values from 0-1
         double getPwm(double input)
         {
+            lastInput = input;
             double errorValue = setPoint - input;
             
             //P term
@@ -51,20 +68,25 @@
             //I term
             integrator += errorValue;
             
-            integrator = integrator>maxInteg? maxInteg : integrator;
-            integrator = integrator<minInteg? minInteg : integrator;
+            double I = (Ki/60.0) * integrator;
             
-            double I = Ki * integrator;
+            if(I > maxInteg || I < minInteg)
+                led2 = 1;
+            else led2 = 0;
+            
+            I = I>maxInteg? maxInteg : I;
+            I = I<minInteg? minInteg : I;
             
             //D term
             double D = Kd * (derivator - errorValue);
             derivator = errorValue;
             
             //output
-            double pwm = P + I + D;
+            double pwm = P  + I + D + middlePoint;
             pwm = pwm>maxPwm? maxPwm : pwm;
             pwm = pwm<minPwm? minPwm : pwm;
             
+            lastPwm = pwm;
             return pwm;
         }