Prosper Van / PID

from greg

Fork of PID by Greg Abdo

PID.cpp@12:9d078cd8ca83, 2014-10-15 (annotated)

Committer:
oprospero
Date:
Wed Oct 15 04:59:05 2014 +0000
Revision:
12:9d078cd8ca83
Parent:
11:0854d9ff17f8 
minor changes

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"
oprospero 9:244f8204d1fa 12 //
oprospero 9:244f8204d1fa 13 //#include "mbed.h"
oprospero 9:244f8204d1fa 14 //Serial pc(USBTX, USBRX);
MitchJCarlson 0:7e5fe0bea780 15
MitchJCarlson 0:7e5fe0bea780 16 PID::PID(const float proportionalGain,
oprospero 9:244f8204d1fa 17 const float integralGain,
oprospero 9:244f8204d1fa 18 const float derivativeGain,
oprospero 10:3296a2150efe 19 const float windupGainGuard ) :
oprospero 9:244f8204d1fa 20 proportionalGain(proportionalGain),
oprospero 9:244f8204d1fa 21 integralGain(integralGain),
oprospero 9:244f8204d1fa 22 derivativeGain(derivativeGain),
oprospero 12:9d078cd8ca83 23 windupGainGuard(windupGainGuard/integralGain)
MitchJCarlson 0:7e5fe0bea780 24 {
oprospero 9:244f8204d1fa 25 // pc.baud(38400);
MitchJCarlson 0:7e5fe0bea780 26 integralError = 0.0f;
MitchJCarlson 0:7e5fe0bea780 27 previousError = 0.0f;
oprospero 7:1b1c2ded95f5 28 currentError = 0.0f;
oprospero 7:1b1c2ded95f5 29 proportionalControl = integralControl = derivativeControl = 0;
MitchJCarlson 0:7e5fe0bea780 30 }
MitchJCarlson 0:7e5fe0bea780 31
oprospero 10:3296a2150efe 32 float PID::correct(const float currentError,const int dt)
MitchJCarlson 0:7e5fe0bea780 33 {
MitchJCarlson 0:7e5fe0bea780 34 // Sum of the errors the controller has incurred over time
oprospero 11:0854d9ff17f8 35 integralError += currentError * dt / 1000.0f;
oprospero 9:244f8204d1fa 36
MitchJCarlson 0:7e5fe0bea780 37 // Cap the sum of errors to prevent excessive correction
MitchJCarlson 0:7e5fe0bea780 38 if (integralError < -windupGainGuard) {
MitchJCarlson 0:7e5fe0bea780 39 integralError = -windupGainGuard;
MitchJCarlson 0:7e5fe0bea780 40 } else if (integralError > windupGainGuard) {
MitchJCarlson 0:7e5fe0bea780 41 integralError = windupGainGuard;
MitchJCarlson 0:7e5fe0bea780 42 }
oprospero 5:43a44c2880a3 43
MitchJCarlson 0:7e5fe0bea780 44 // Calculate the correction, based on the tuning values
MitchJCarlson 0:7e5fe0bea780 45 proportionalControl = proportionalGain * currentError;
MitchJCarlson 0:7e5fe0bea780 46 integralControl = integralError * integralGain;
oprospero 11:0854d9ff17f8 47 derivativeControl = derivativeGain * (currentError - previousError) / dt * 1000.0f;
MitchJCarlson 0:7e5fe0bea780 48
MitchJCarlson 0:7e5fe0bea780 49 // Save current error for next call to correction
MitchJCarlson 0:7e5fe0bea780 50 previousError = currentError;
oprospero 12:9d078cd8ca83 51
oprospero 12:9d078cd8ca83 52 controlTotal = proportionalControl + integralControl + derivativeControl;
oprospero 12:9d078cd8ca83 53
oprospero 12:9d078cd8ca83 54 if (controlTotal > CONTROL_LIMIT)
oprospero 12:9d078cd8ca83 55 return CONTROL_LIMIT;
oprospero 12:9d078cd8ca83 56 else if (controlTotal < -CONTROL_LIMIT)
oprospero 12:9d078cd8ca83 57 return -CONTROL_LIMIT;
oprospero 12:9d078cd8ca83 58
oprospero 12:9d078cd8ca83 59 return controlTotal;
MitchJCarlson 0:7e5fe0bea780 60
MitchJCarlson 0:7e5fe0bea780 61 }
oprospero 8:a020a225bc65 62
oprospero 8:a020a225bc65 63 void PID::updateP(const float pGain)
oprospero 8:a020a225bc65 64 {
oprospero 8:a020a225bc65 65 proportionalGain = pGain;
oprospero 8:a020a225bc65 66 }
oprospero 8:a020a225bc65 67
oprospero 8:a020a225bc65 68 void PID::updateD(const float dGain)
oprospero 8:a020a225bc65 69 {
oprospero 8:a020a225bc65 70 derivativeGain = dGain;
oprospero 12:9d078cd8ca83 71 }