Greg Abdo / PID

Problems with motor #2

Dependents:   quadCommand2

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 }