Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
Diff: PID/PID.cpp
- Revision:
- 2:03e5f7ab473f
- Parent:
- 1:5e2b81f2d0b4
- Child:
- 5:06e978fd147a
diff -r 5e2b81f2d0b4 -r 03e5f7ab473f PID/PID.cpp --- a/PID/PID.cpp Sun Sep 08 20:53:33 2013 +0000 +++ b/PID/PID.cpp Mon Sep 09 20:01:13 2013 +0000 @@ -1,7 +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; Integrate = true; @@ -12,40 +12,34 @@ dtTimer.start(); } -float PID::compute(float SetPoint, float ProcessValue) -{ +void PID::compute(float DesiredAngle, float Angle, float Gyro_data) { // 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 = ProcessValue - SetPoint; + float Error = Angle - DesiredAngle; // 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; + 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