File content as of revision 46:fd5a62296b12:

#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