pid.h

00001 #ifndef _PID_H_
00002 #define _PID_H_
00003 
00004 #include "mbed.h"
00005 
00006 
00007 DigitalOut      led2(LED2);
00008 //http://coder-tronics.com/pid-tutorial-c-code-example-pt2/
00009 
00010 class Pid
00011 {
00012     private:
00013         double setPoint;
00014         double Kp, Ki, Kd;
00015         
00016         double integrator, derivator;
00017         double maxInteg, minInteg;
00018         double maxPwm, minPwm;
00019         double lastInput;
00020         double lastPwm;
00021         double middlePoint;
00022         
00023     public:
00024         Pid(double Kp, double Ki, double Kd)
00025         {
00026             this->Kp = Kp;
00027             this->Ki = Ki;
00028             this->Kd = Kd;
00029             this->middlePoint = 0.08;
00030             integrator = 0;
00031             derivator = 0;
00032             
00033             maxInteg = 1.0;
00034             minInteg = -1.0;
00035             
00036             maxPwm = 1;
00037             minPwm = 0.05;
00038             lastInput = 0.5;
00039             lastPwm = 0.5;
00040         }
00041         void addKp(double value) {Kp += value;}
00042         void addKi(double value) {Ki += value;}
00043         void addKd(double value) {Kd += value;}
00044         double getKp() {return Kp;}
00045         double getKi() {return Ki;}
00046         double getKd() {return Kd;}
00047         void setParameters(double Kp, double Ki, double Kd)
00048         {
00049             this->Kp = Kp;
00050             this->Ki = Ki;
00051             this->Kd = Kd;
00052         }
00053         //desired value
00054         void setSetPoint(double setPoint)   {this->setPoint = setPoint;}
00055         double getSetPoint()                {return setPoint;}
00056         double getLastInput()               {return lastInput;}
00057         double getLastPwm()                 {return lastPwm;}
00058         
00059         //values from 0-1
00060         double getPwm(double input)
00061         {
00062             lastInput = input;
00063             double errorValue = setPoint - input;
00064             
00065             //P term
00066             double P = Kp * errorValue;
00067             
00068             //I term
00069             integrator += errorValue;
00070             
00071             double I = (Ki/6000.0) * integrator;
00072             
00073             if(I > maxInteg || I < minInteg)
00074                 led2.write(1);
00075             else led2.write(0);
00076             
00077             I = I>maxInteg? maxInteg : I;
00078             I = I<minInteg? minInteg : I;
00079             
00080             //D term
00081             double D = Kd * (derivator - errorValue);
00082             derivator = errorValue;
00083             
00084             //output
00085             double pwm = P  + I + D + middlePoint;
00086             pwm = pwm>maxPwm? maxPwm : pwm;
00087             pwm = pwm<minPwm? minPwm : pwm;
00088             
00089             lastPwm = pwm;
00090             return pwm;
00091         }
00092        
00093 };
00094 #endif