PID.cpp

00001 #include "PID.h"
00002 
00003 PID::PID(float P, float I, float D, float Integral_Max)
00004 {
00005     Integral = 0;
00006     LastTime = 0;
00007     SetPoint = 0;
00008     Integrate = true;
00009     PID::P = P;
00010     PID::I = I;
00011     PID::D = D;
00012     PID::Integral_Max = Integral_Max;
00013     dtTimer.start();
00014 }
00015 
00016 float PID::compute(float SetPoint, float ProcessValue)
00017 {
00018     // meassure dt
00019     float dt = dtTimer.read() - LastTime;    // time in us since last loop
00020     LastTime = dtTimer.read();                   // set new time for next measurement
00021     
00022     // Proportional
00023     float Error =  ProcessValue - SetPoint;
00024     
00025     // Integral
00026     if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations
00027         Integral = 0;
00028     else if (abs(Integral + Error) <= Integral_Max)
00029         Integral += Error * dt;
00030         
00031     // Derivative
00032     float Derivative = (Error - PreviousError) / dt;
00033     
00034     // Final Formula
00035     float Result = P * Error + I * Integral + D * Derivative;
00036     
00037     PreviousError = Error;
00038     
00039     return Result;
00040 }
00041 
00042 void PID::setPID(float P, float I, float D)
00043 {
00044     PID::P = P;
00045     PID::I = I;
00046     PID::D = D;
00047 }
00048 
00049 void PID::setIntegrate(bool Integrate)
00050 {
00051     PID::Integrate = Integrate;
00052 }