Diff: PIDController.h

Revision:

40:8e852115fe55

Child:

46:fd5a62296b12

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDController.h	Tue May 26 11:28:37 2015 +0000
@@ -0,0 +1,77 @@
+#ifndef _H_PIDCONTROLLER_H
+#define _H_PIDCONTROLLER_H
+
+/** \class PIDController
+ *  \brief Implements a PID controller.
+ * A PID controller can be used to control some variable parameter (the 'output') so that
+ * the input reaches a particular 'set point.' This is done by changing the output a little
+ * and checking the error still remaining. The parameters Kp, Ki and Kd control the time behaviour.
+ */
+class PIDController
+{
+public:
+    /// Constructor
+    /// @param _Kp initial Kp, default = 1.0
+    /// @param _Ki initial Ki, default = 0.0
+    /// @param _Kd initial Kd, default = 0.0
+    /// @param _setpoint initial setpoint, default = 0.0
+    /// @param _clippingEnabled whether the PID controller integral term will be clipped
+    /// @param _maximum if clipping is enabled, the maximum value of the integral term
+    PIDController(  float const _Kp = 1.0f,
+                    float const _Ki = 0.0f,
+                    float const _Kd = 0.0f,
+                    float const _setpoint = 0.0f,
+                    bool  const _clippingEnabled = false,
+                    float const _maximum = 0.0)
+                    :   Kp(_Kp), Ki(_Ki), Kd(_Kd),
+                        setPoint(_setpoint),
+                        clippingEnabled(_clippingEnabled), maximum(_maximum),
+                        e(0), int_e(0), diff_e(0), prev_e(0) { }
+
+    float Kp;               ///< The proportional term of the PID controller
+    float Ki;               ///< The integral term of the PID controller
+    float Kd;               ///< The differential term of the PID controller
+    float setPoint;         ///< The set point of the PID controller (the output value that it will try to maintain)
+    bool  clippingEnabled;  ///< Whether to clip the integral term so that it never exceeds the maximum
+    float maximum;          ///< The maximum value the integral term is allowed to reach, if clipping is enabled
+    float e;                ///< error signal (setpoint minus input)
+    float int_e;            ///< integral of error signal
+    float diff_e;           ///< derivative of error signal
+    float prev_e;           ///< error at t_-1
+
+    /// Reset PID controller (integral, differential) to zero
+    void reset()
+    {
+        prev_e = 0.0f;
+        int_e  = 0.0f;
+    }
+
+    /// Performs one iteration of the PID control loop. If clipping
+    /// @param input The input to the PID controller
+    /// @return The PID controller's output
+    float output(float const input)
+    {
+        // find error from set point
+        e = setPoint - input;
+        
+        // differentiate error
+        diff_e = (e - prev_e);
+        prev_e = e;
+        
+        // integral term
+        int_e = int_e + e;
+        
+        float const maxInte = maximum / Ki;
+        
+        if (clippingEnabled) {
+            if (int_e > maxInte)
+                int_e = maxInte;
+            if (int_e < -maxInte)
+                int_e = -maxInte;
+        }
+    
+        return (e * Kp + int_e * Ki + diff_e * Kd);
+    }
+};
+
+#endif//_H_PIDCONTROLLER_H
\ No newline at end of file