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     RollBufferIndex = 0;
00010     PID::P = P;
00011     PID::I = I;
00012     PID::D = D;
00013     PID::Integral_Max = Integral_Max;
00014     dtTimer.start();
00015 }
00016 
00017 void PID::compute(float SetPoint, float ProcessValue)
00018 {
00019     // meassure dt
00020     float dt = dtTimer.read() - LastTime;    // time in us since last loop
00021     LastTime = dtTimer.read();                   // set new time for next measurement
00022     
00023     // Proportional
00024     float Error =  ProcessValue - SetPoint;
00025     
00026     // Integral
00027     if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations
00028         Integral = 0;
00029     else if (abs(Integral + Error) <= Integral_Max)
00030         Integral += Error * dt;
00031         
00032     // Derivative
00033     RollBuffer[RollBufferIndex] = (Error - PreviousError) / dt;
00034     RollBufferIndex++;
00035     if (RollBufferIndex == BUFFERSIZE)
00036         RollBufferIndex = 0;
00037     float Derivative = 0;
00038     for(int i=0; i<BUFFERSIZE ;i++)
00039         Derivative += RollBuffer[i];
00040     Derivative /= BUFFERSIZE;
00041     PreviousError = Error;
00042     
00043     // Final Formula
00044     Value = P * Error + I * Integral + D * Derivative;
00045 }
00046 
00047 void PID::setPID(float P, float I, float D)
00048 {
00049     PID::P = P;
00050     PID::I = I;
00051     PID::D = D;
00052 }
00053 
00054 void PID::setIntegrate(bool Integrate)
00055 {
00056     PID::Integrate = Integrate;
00057 }