Diff: PID.cpp

Revision:

0:7e5fe0bea780

Child:

1:01f3ec7bd956

diff -r 000000000000 -r 7e5fe0bea780 PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Thu Jun 06 18:58:08 2013 +0000
@@ -0,0 +1,55 @@
+/** PID.cpp
+ * UWB Quadcopter project
+ *
+ * Implementation of the Proportional Integral Derivative
+ * control system for the quadcopter.
+ * http://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/
+ *
+ * @author UWB Quadcopter group
+ */
+
+#include "PID.h"
+
+PID::PID(const float proportionalGain,
+         const float integralGain,
+         const float derivativeGain,
+         const float windupGainGuard) :
+    proportionalGain(proportionalGain),
+    integralGain(integralGain),
+    derivativeGain(derivativeGain),
+    windupGainGuard(windupGainGuard)
+{
+    integralError = 0.0f;
+    previousError = 0.0f;
+}
+
+float PID::correct(const float currentPosition,
+                   const float targetPosition,
+                   const float dt)
+{
+    float currentError;
+    float proportialControl, integralControl, derivativeControl;
+
+    // How far away we are from the target
+    currentError = targetPosition - currentPosition;
+
+    // Sum of the errors the controller has incurred over time
+    integralError += currentError * dt;
+    // Cap the sum of errors to prevent excessive correction
+    if (integralError < -windupGainGuard) {
+        integralError = -windupGainGuard;
+    } else if (integralError > windupGainGuard) {
+        integralError = windupGainGuard;
+    }
+
+    // Calculate the correction, based on the tuning values
+    proportionalControl = proportionalGain * currentError;
+    integralControl = integralError * integralGain;
+    derivativeControl = derivativeGain * ((currentError - previousError) / dt);
+
+    // Save current error for next call to correction
+    previousError = currentError;
+
+    return proportialControl + integralControl + derivativeControl;
+
+}