File content as of revision 43:5123f24e0b2c:

#ifndef _PID_H_
#define _PID_H_

#include "mbed.h"


DigitalOut      led2(LED2);
//http://coder-tronics.com/pid-tutorial-c-code-example-pt2/

class Pid
{
    private:
        double setPoint;
        double Kp, Ki, Kd;
        
        double integrator, derivator;
        double maxInteg, minInteg;
        double maxPwm, minPwm;
        double lastInput;
        double lastPwm;
        double middlePoint;
        
    public:
        Pid(double Kp, double Ki, double Kd)
        {
            this->Kp = Kp;
            this->Ki = Ki;
            this->Kd = Kd;
            this->middlePoint = 0.08;
            integrator = 0;
            derivator = 0;
            
            maxInteg = 1.0;
            minInteg = -1.0;
            
            maxPwm = 1;
            minPwm = 0.05;
            lastInput = 0.5;
            lastPwm = 0.5;
        }
        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->Kp = Kp;
            this->Ki = Ki;
            this->Kd = Kd;
        }
        //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
            double P = Kp * errorValue;
            
            //I term
            integrator += errorValue;
            
            double I = (Ki/6000.0) * integrator;
            
            if(I > maxInteg || I < minInteg)
                led2.write(1);
            else led2.write(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 + middlePoint;
            pwm = pwm>maxPwm? maxPwm : pwm;
            pwm = pwm<minPwm? minPwm : pwm;
            
            lastPwm = pwm;
            return pwm;
        }
       
};
#endif