Matthias Grob / Mbed 2 deprecated FlyBed3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

PID/PID.cpp@5:8ea99e98de73, 2015-09-11 (annotated)

Committer:
maetugr
Date:
Fri Sep 11 08:43:35 2015 +0000
Revision:
5:8ea99e98de73
Parent:
1:60882db03b0f
Child:
8:609a2ad4c30e
level mode with 12" props pretty stable indoors; some very small wobles remaining

Who changed what in which revision?

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