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@2:03e5f7ab473f, 2013-09-09 (annotated)

Committer:
maetugr
Date:
Mon Sep 09 20:01:13 2013 +0000
Revision:
2:03e5f7ab473f
Parent:
1:5e2b81f2d0b4
Child:
5:06e978fd147a
SUCCESS! First stable flight! (A bit hard to controll, now working on controlling coefficients and yaw input)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:12950aa67f2a 1 #include "PID.h"
maetugr 0:12950aa67f2a 2
maetugr 2:03e5f7ab473f 3 PID::PID(float P, float I, float D, float Integral_Max) {
maetugr 2:03e5f7ab473f 4 Value = 0;
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 2:03e5f7ab473f 15 void PID::compute(float DesiredAngle, float Angle, float Gyro_data) {
maetugr 0:12950aa67f2a 16 // meassure dt
maetugr 0:12950aa67f2a 17 float dt = dtTimer.read() - LastTime; // time in us since last loop
maetugr 0:12950aa67f2a 18 LastTime = dtTimer.read(); // set new time for next measurement
maetugr 0:12950aa67f2a 19
maetugr 2:03e5f7ab473f 20 // Derivative (most important for QC stability and Gyro only because very sensitive!)
maetugr 2:03e5f7ab473f 21 float Derivative = Gyro_data;
maetugr 2:03e5f7ab473f 22 //PreviousGyro_data = Gyro_data;
maetugr 2:03e5f7ab473f 23
maetugr 0:12950aa67f2a 24 // Proportional
maetugr 2:03e5f7ab473f 25 float Error = Angle - DesiredAngle;
maetugr 0:12950aa67f2a 26
maetugr 0:12950aa67f2a 27 // Integral
maetugr 0:12950aa67f2a 28 if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations
maetugr 0:12950aa67f2a 29 Integral = 0;
maetugr 0:12950aa67f2a 30 else if (abs(Integral + Error) <= Integral_Max)
maetugr 0:12950aa67f2a 31 Integral += Error * dt;
maetugr 0:12950aa67f2a 32
maetugr 0:12950aa67f2a 33 // Final Formula
maetugr 2:03e5f7ab473f 34 Value = P * Error + I * Integral + D * Derivative;
maetugr 0:12950aa67f2a 35 }
maetugr 0:12950aa67f2a 36
maetugr 2:03e5f7ab473f 37 void PID::setPID(float P, float I, float D) {
maetugr 0:12950aa67f2a 38 PID::P = P;
maetugr 0:12950aa67f2a 39 PID::I = I;
maetugr 0:12950aa67f2a 40 PID::D = D;
maetugr 0:12950aa67f2a 41 }
maetugr 0:12950aa67f2a 42
maetugr 2:03e5f7ab473f 43 void PID::setIntegrate(bool Integrate) {
maetugr 0:12950aa67f2a 44 PID::Integrate = Integrate;
maetugr 0:12950aa67f2a 45 }