Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
Diff: PID/PID.cpp
- Revision:
- 7:ac2895479e34
- Parent:
- 5:06e978fd147a
diff -r 1aa4f722e8e0 -r ac2895479e34 PID/PID.cpp --- a/PID/PID.cpp Sun Jul 06 09:25:09 2014 +0000 +++ b/PID/PID.cpp Sat Jul 12 12:21:11 2014 +0000 @@ -1,6 +1,7 @@ #include "PID.h" -PID::PID(float P, float I, float D, float Integral_Max) { +PID::PID(float P, float I, float D, float Integral_Max) +{ Value = 0; Integral = 0; LastTime = 0; @@ -12,34 +13,37 @@ dtTimer.start(); } -void PID::compute(float DesiredAngle, float Angle, float Gyro_data) { +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 - // Derivative (most important for QC stability and Gyro only because very sensitive!) - float Derivative = Gyro_data; - //PreviousGyro_data = Gyro_data; - // Proportional - float Error = Angle - DesiredAngle; + float Error = ProcessValue - SetPoint; // Integral - if (dt > 2 || !Integrate) // TODO: 2 secs is the maximal time between two computations + 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; + PreviousError = Error; // Final Formula Value = P * Error + I * Integral + D * Derivative; } -void PID::setPID(float P, float I, float D) { +void PID::setPID(float P, float I, float D) +{ PID::P = P; PID::I = I; PID::D = D; } -void PID::setIntegrate(bool Integrate) { +void PID::setIntegrate(bool Integrate) +{ PID::Integrate = Integrate; } \ No newline at end of file