Matthias Grob / Mbed 2 deprecated FlyBed2

My fully self designed first stable working Quadrocopter Software.

Dependencies:   mbed

Dependents:   fluy343

/media/uploads/maetugr/dsc09031.jpg

PID/PID.cpp@7:ac2895479e34, 2014-07-12 (annotated)

Committer:
maetugr
Date:
Sat Jul 12 12:21:11 2014 +0000
Revision:
7:ac2895479e34
Parent:
5:06e978fd147a 
A stable working Acro-mode :); some little oscillations to be solved and then the angle-mode draft will get used

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:12950aa67f2a 1 #include "PID.h"
maetugr 0:12950aa67f2a 2
maetugr 7:ac2895479e34 3 PID::PID(float P, float I, float D, float Integral_Max)
maetugr 7:ac2895479e34 4 {
maetugr 2:03e5f7ab473f 5 Value = 0;
maetugr 0:12950aa67f2a 6 Integral = 0;
maetugr 0:12950aa67f2a 7 LastTime = 0;
maetugr 0:12950aa67f2a 8 Integrate = true;
maetugr 0:12950aa67f2a 9 PID::P = P;
maetugr 0:12950aa67f2a 10 PID::I = I;
maetugr 0:12950aa67f2a 11 PID::D = D;
maetugr 0:12950aa67f2a 12 PID::Integral_Max = Integral_Max;
maetugr 0:12950aa67f2a 13 dtTimer.start();
maetugr 0:12950aa67f2a 14 }
maetugr 0:12950aa67f2a 15
maetugr 7:ac2895479e34 16 float PID::compute(float SetPoint, float ProcessValue)
maetugr 7:ac2895479e34 17 {
maetugr 0:12950aa67f2a 18 // meassure dt
maetugr 0:12950aa67f2a 19 float dt = dtTimer.read() - LastTime; // time in us since last loop
maetugr 0:12950aa67f2a 20 LastTime = dtTimer.read(); // set new time for next measurement
maetugr 0:12950aa67f2a 21
maetugr 0:12950aa67f2a 22 // Proportional
maetugr 7:ac2895479e34 23 float Error = ProcessValue - SetPoint;
maetugr 0:12950aa67f2a 24
maetugr 0:12950aa67f2a 25 // Integral
maetugr 7:ac2895479e34 26 if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations
maetugr 0:12950aa67f2a 27 Integral = 0;
maetugr 0:12950aa67f2a 28 else if (abs(Integral + Error) <= Integral_Max)
maetugr 0:12950aa67f2a 29 Integral += Error * dt;
maetugr 7:ac2895479e34 30
maetugr 7:ac2895479e34 31 // Derivative
maetugr 7:ac2895479e34 32 float Derivative = (Error - PreviousError) / dt;
maetugr 7:ac2895479e34 33 PreviousError = Error;
maetugr 0:12950aa67f2a 34
maetugr 0:12950aa67f2a 35 // Final Formula
maetugr 2:03e5f7ab473f 36 Value = P * Error + I * Integral + D * Derivative;
maetugr 0:12950aa67f2a 37 }
maetugr 0:12950aa67f2a 38
maetugr 7:ac2895479e34 39 void PID::setPID(float P, float I, float D)
maetugr 7:ac2895479e34 40 {
maetugr 0:12950aa67f2a 41 PID::P = P;
maetugr 0:12950aa67f2a 42 PID::I = I;
maetugr 0:12950aa67f2a 43 PID::D = D;
maetugr 0:12950aa67f2a 44 }
maetugr 0:12950aa67f2a 45
maetugr 7:ac2895479e34 46 void PID::setIntegrate(bool Integrate)
maetugr 7:ac2895479e34 47 {
maetugr 0:12950aa67f2a 48 PID::Integrate = Integrate;
maetugr 0:12950aa67f2a 49 }