Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
Diff: PID/PID.cpp
- Revision:
- 0:12950aa67f2a
- Child:
- 1:5e2b81f2d0b4
diff -r 000000000000 -r 12950aa67f2a PID/PID.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID/PID.cpp Mon Sep 02 15:04:22 2013 +0000 @@ -0,0 +1,52 @@ +#include "PID.h" + +PID::PID(float P, float I, float D, float Integral_Max) +{ + Integral = 0; + LastTime = 0; + SetPoint = 0; + Integrate = true; + PID::P = P; + PID::I = I; + PID::D = D; + PID::Integral_Max = Integral_Max; + dtTimer.start(); +} + +float PID::compute(float SetPoint, float ProcessValue) +{ + // meassure dt + float dt = dtTimer.read() - LastTime; // time in us since last loop + LastTime = dtTimer.read(); // set new time for next measurement + + // Proportional + float Error = ProcessValue - SetPoint; + + // Integral + if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations + Integral = 0; + else if (abs(Integral + Error) <= Integral_Max) + Integral += Error * dt; + + // Derivative + float Derivative = (Error - PreviousError) / dt; + + // Final Formula + float Result = P * Error + I * Integral + D * Derivative; + + PreviousError = Error; + + return Result; +} + +void PID::setPID(float P, float I, float D) +{ + PID::P = P; + PID::I = I; + PID::D = D; +} + +void PID::setIntegrate(bool Integrate) +{ + PID::Integrate = Integrate; +} \ No newline at end of file