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@1:5e2b81f2d0b4, 2013-09-08 (annotated)

Committer:
maetugr
Date:
Sun Sep 08 20:53:33 2013 +0000
Revision:
1:5e2b81f2d0b4
Parent:
0:12950aa67f2a
Child:
2:03e5f7ab473f
D only with Gyro as source is very stable on one axis;; next changing the PID class to get only gyro input for derivative

Who changed what in which revision?

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