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@40:2ca410923691, 2014-02-14 (annotated)

Committer:
maetugr
Date:
Fri Feb 14 14:17:32 2014 +0000
Revision:
40:2ca410923691
Parent:
33:fd98776b6cc7 
now with MPU6050 before taking it too FlyBed2

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 }