Derek McLean / PID

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.

Fork of PID by Derek McLean

PID.cpp@0:7e5fe0bea780, 2013-06-06 (annotated)

Committer:
MitchJCarlson
Date:
Thu Jun 06 18:58:08 2013 +0000
Revision:
0:7e5fe0bea780
Child:
1:01f3ec7bd956
Added pid.h and pid.cpp to library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MitchJCarlson 0:7e5fe0bea780 1 /** PID.cpp
MitchJCarlson 0:7e5fe0bea780 2 * UWB Quadcopter project
MitchJCarlson 0:7e5fe0bea780 3 *
MitchJCarlson 0:7e5fe0bea780 4 * Implementation of the Proportional Integral Derivative
MitchJCarlson 0:7e5fe0bea780 5 * control system for the quadcopter.
MitchJCarlson 0:7e5fe0bea780 6 * http://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/
MitchJCarlson 0:7e5fe0bea780 7 *
MitchJCarlson 0:7e5fe0bea780 8 * @author UWB Quadcopter group
MitchJCarlson 0:7e5fe0bea780 9 */
MitchJCarlson 0:7e5fe0bea780 10
MitchJCarlson 0:7e5fe0bea780 11 #include "PID.h"
MitchJCarlson 0:7e5fe0bea780 12
MitchJCarlson 0:7e5fe0bea780 13 PID::PID(const float proportionalGain,
MitchJCarlson 0:7e5fe0bea780 14 const float integralGain,
MitchJCarlson 0:7e5fe0bea780 15 const float derivativeGain,
MitchJCarlson 0:7e5fe0bea780 16 const float windupGainGuard) :
MitchJCarlson 0:7e5fe0bea780 17 proportionalGain(proportionalGain),
MitchJCarlson 0:7e5fe0bea780 18 integralGain(integralGain),
MitchJCarlson 0:7e5fe0bea780 19 derivativeGain(derivativeGain),
MitchJCarlson 0:7e5fe0bea780 20 windupGainGuard(windupGainGuard)
MitchJCarlson 0:7e5fe0bea780 21 {
MitchJCarlson 0:7e5fe0bea780 22 integralError = 0.0f;
MitchJCarlson 0:7e5fe0bea780 23 previousError = 0.0f;
MitchJCarlson 0:7e5fe0bea780 24 }
MitchJCarlson 0:7e5fe0bea780 25
MitchJCarlson 0:7e5fe0bea780 26 float PID::correct(const float currentPosition,
MitchJCarlson 0:7e5fe0bea780 27 const float targetPosition,
MitchJCarlson 0:7e5fe0bea780 28 const float dt)
MitchJCarlson 0:7e5fe0bea780 29 {
MitchJCarlson 0:7e5fe0bea780 30 float currentError;
MitchJCarlson 0:7e5fe0bea780 31 float proportialControl, integralControl, derivativeControl;
MitchJCarlson 0:7e5fe0bea780 32
MitchJCarlson 0:7e5fe0bea780 33 // How far away we are from the target
MitchJCarlson 0:7e5fe0bea780 34 currentError = targetPosition - currentPosition;
MitchJCarlson 0:7e5fe0bea780 35
MitchJCarlson 0:7e5fe0bea780 36 // Sum of the errors the controller has incurred over time
MitchJCarlson 0:7e5fe0bea780 37 integralError += currentError * dt;
MitchJCarlson 0:7e5fe0bea780 38 // Cap the sum of errors to prevent excessive correction
MitchJCarlson 0:7e5fe0bea780 39 if (integralError < -windupGainGuard) {
MitchJCarlson 0:7e5fe0bea780 40 integralError = -windupGainGuard;
MitchJCarlson 0:7e5fe0bea780 41 } else if (integralError > windupGainGuard) {
MitchJCarlson 0:7e5fe0bea780 42 integralError = windupGainGuard;
MitchJCarlson 0:7e5fe0bea780 43 }
MitchJCarlson 0:7e5fe0bea780 44
MitchJCarlson 0:7e5fe0bea780 45 // Calculate the correction, based on the tuning values
MitchJCarlson 0:7e5fe0bea780 46 proportionalControl = proportionalGain * currentError;
MitchJCarlson 0:7e5fe0bea780 47 integralControl = integralError * integralGain;
MitchJCarlson 0:7e5fe0bea780 48 derivativeControl = derivativeGain * ((currentError - previousError) / dt);
MitchJCarlson 0:7e5fe0bea780 49
MitchJCarlson 0:7e5fe0bea780 50 // Save current error for next call to correction
MitchJCarlson 0:7e5fe0bea780 51 previousError = currentError;
MitchJCarlson 0:7e5fe0bea780 52
MitchJCarlson 0:7e5fe0bea780 53 return proportialControl + integralControl + derivativeControl;
MitchJCarlson 0:7e5fe0bea780 54
MitchJCarlson 0:7e5fe0bea780 55 }