Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

PID/PID.cpp@33:fd98776b6cc7, 2013-04-04 (annotated)

Committer:
maetugr
Date:
Thu Apr 04 14:25:21 2013 +0000
Revision:
33:fd98776b6cc7
Parent:
29:8b7362a2ee14 
New version developed in eastern holidays, ported Madgwick Filter, added support for chaning PID values while flying over bluetooth, still not flying stable or even controllable

Who changed what in which revision?

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