Greg Abdo / PID

Problems with motor #2

Dependents:   quadCommand2

Fork of PID by Derek McLean

PID.cpp@2:b214010a0c1b, 2013-07-02 (annotated)

Committer:
gabdo
Date:
Tue Jul 02 21:03:30 2013 +0000
Revision:
2:b214010a0c1b
Parent:
1:01f3ec7bd956
Child:
3:8172f254dff4
Problems with motor #2

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;
dereklmc 1:01f3ec7bd956 31 float proportionalControl, integralControl, derivativeControl;
MitchJCarlson 0:7e5fe0bea780 32
MitchJCarlson 0:7e5fe0bea780 33 // How far away we are from the target
gabdo 2:b214010a0c1b 34 currentError += targetPosition - currentPosition;
MitchJCarlson 0:7e5fe0bea780 35
MitchJCarlson 0:7e5fe0bea780 36 // Sum of the errors the controller has incurred over time
gabdo 2:b214010a0c1b 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
dereklmc 1:01f3ec7bd956 53 return proportionalControl + integralControl + derivativeControl;
MitchJCarlson 0:7e5fe0bea780 54
MitchJCarlson 0:7e5fe0bea780 55 }