Revision 0:7e5fe0bea780, committed 2013-06-06

Comitter:

MitchJCarlson

Date:

Thu Jun 06 18:58:08 2013 +0000

Child:

1:01f3ec7bd956

Commit message:

Added pid.h and pid.cpp to library

Changed in this revision

--- /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;
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Thu Jun 06 18:58:08 2013 +0000
@@ -0,0 +1,58 @@
+/** PID.h */
+
+#ifndef UWBQuad__PID__h
+#define UWBQuad__PID__h
+
+/**
+ * UWB Quadcopter project
+ *
+ * Implementation of the Proportional Integral Derivative
+ * control system for quadcopter stabilization.
+ * http://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/
+ *
+ * @author UWB Quadcopter group
+ */
+class PID
+{
+
+public:
+    /**
+     * Initialize the controller.
+     *
+     * @param proportionalGain Tuning value for adjusting the copter
+     *                          towards a target position.
+     * @param integralGain Tuning value for compensating for
+     *                      environment imperfections that provide
+                            resistance such as friction.
+     * @param derivativeGain Tuning value for compensating for
+     *                          environment imperfections that cause
+     *                          the system to overshoot the target,
+     *                          such as momentum.
+     * @param windupGainGuard Cap for the maximum error value.
+     */
+    PID(const float, const float, const float, const float);
+
+    /**
+     * Determine how to correct the system to the desired position.
+     *
+     * @param currentPosition The current vector of the system.
+     * @param targetPosition The desired vector of the system.
+     * @param dt The change in time since the last adjustment.
+     *              (time determined by caller,
+     *               eg. PID can be switched off for manual control)
+     * @return Adjustment to apply to the motors.
+     */
+    float correct(const float, const float, const float);
+
+private:
+    const float proportionalGain;
+    const float integralGain;
+    const float derivativeGain;
+    const float windupGainGuard;
+
+    float previousError;
+    float integralError;
+
+};
+
+#endif