PID.cpp

00001 #include "PID.h"
00002 
00003 PID::PID(float P, float I, float D, float Integral_Max)
00004 {
00005     Value = 0;
00006     Integral = 0;
00007     LastTime = 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     PreviousError = Error;
00034     
00035     // Final Formula
00036     Value = P * Error + I * Integral + D * Derivative;
00037 }
00038 
00039 void PID::setPID(float P, float I, float D)
00040 {
00041     PID::P = P;
00042     PID::I = I;
00043     PID::D = D;
00044 }
00045 
00046 void PID::setIntegrate(bool Integrate)
00047 {
00048     PID::Integrate = Integrate;
00049 }