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@0:12950aa67f2a, 2013-09-02 (annotated)

Committer:
maetugr
Date:
Mon Sep 02 15:04:22 2013 +0000
Revision:
0:12950aa67f2a
Child:
1:5e2b81f2d0b4
first commit of new version (only one axis for test until now); note: no more Ticker! (made problems because of interrupts)

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 SetPoint = 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 0:12950aa67f2a 16 float PID::compute(float SetPoint, float ProcessValue)
maetugr 0:12950aa67f2a 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 0:12950aa67f2a 23 float Error = ProcessValue - SetPoint;
maetugr 0:12950aa67f2a 24
maetugr 0:12950aa67f2a 25 // Integral
maetugr 0:12950aa67f2a 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 0:12950aa67f2a 30
maetugr 0:12950aa67f2a 31 // Derivative
maetugr 0:12950aa67f2a 32 float Derivative = (Error - PreviousError) / dt;
maetugr 0:12950aa67f2a 33
maetugr 0:12950aa67f2a 34 // Final Formula
maetugr 0:12950aa67f2a 35 float Result = P * Error + I * Integral + D * Derivative;
maetugr 0:12950aa67f2a 36
maetugr 0:12950aa67f2a 37 PreviousError = Error;
maetugr 0:12950aa67f2a 38
maetugr 0:12950aa67f2a 39 return Result;
maetugr 0:12950aa67f2a 40 }
maetugr 0:12950aa67f2a 41
maetugr 0:12950aa67f2a 42 void PID::setPID(float P, float I, float D)
maetugr 0:12950aa67f2a 43 {
maetugr 0:12950aa67f2a 44 PID::P = P;
maetugr 0:12950aa67f2a 45 PID::I = I;
maetugr 0:12950aa67f2a 46 PID::D = D;
maetugr 0:12950aa67f2a 47 }
maetugr 0:12950aa67f2a 48
maetugr 0:12950aa67f2a 49 void PID::setIntegrate(bool Integrate)
maetugr 0:12950aa67f2a 50 {
maetugr 0:12950aa67f2a 51 PID::Integrate = Integrate;
maetugr 0:12950aa67f2a 52 }