Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
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