Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Thu Jul 14 2022 18:09:09 by 1.7.2