Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

PIDController.h@46:fd5a62296b12, 2015-05-27 (annotated)

Committer:
pvaibhav
Date:
Wed May 27 13:01:43 2015 +0000
Revision:
46:fd5a62296b12
Parent:
40:8e852115fe55 
Code reformatted

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pvaibhav 40:8e852115fe55 1 #ifndef _H_PIDCONTROLLER_H
pvaibhav 40:8e852115fe55 2 #define _H_PIDCONTROLLER_H
pvaibhav 40:8e852115fe55 3
pvaibhav 40:8e852115fe55 4 /** \class PIDController
pvaibhav 40:8e852115fe55 5 * \brief Implements a PID controller.
pvaibhav 40:8e852115fe55 6 * A PID controller can be used to control some variable parameter (the 'output') so that
pvaibhav 40:8e852115fe55 7 * the input reaches a particular 'set point.' This is done by changing the output a little
pvaibhav 40:8e852115fe55 8 * and checking the error still remaining. The parameters Kp, Ki and Kd control the time behaviour.
pvaibhav 40:8e852115fe55 9 */
pvaibhav 40:8e852115fe55 10 class PIDController
pvaibhav 40:8e852115fe55 11 {
pvaibhav 40:8e852115fe55 12 public:
pvaibhav 40:8e852115fe55 13 /// Constructor
pvaibhav 40:8e852115fe55 14 /// @param _Kp initial Kp, default = 1.0
pvaibhav 40:8e852115fe55 15 /// @param _Ki initial Ki, default = 0.0
pvaibhav 40:8e852115fe55 16 /// @param _Kd initial Kd, default = 0.0
pvaibhav 40:8e852115fe55 17 /// @param _setpoint initial setpoint, default = 0.0
pvaibhav 40:8e852115fe55 18 /// @param _clippingEnabled whether the PID controller integral term will be clipped
pvaibhav 40:8e852115fe55 19 /// @param _maximum if clipping is enabled, the maximum value of the integral term
pvaibhav 40:8e852115fe55 20 PIDController( float const _Kp = 1.0f,
pvaibhav 40:8e852115fe55 21 float const _Ki = 0.0f,
pvaibhav 40:8e852115fe55 22 float const _Kd = 0.0f,
pvaibhav 40:8e852115fe55 23 float const _setpoint = 0.0f,
pvaibhav 40:8e852115fe55 24 bool const _clippingEnabled = false,
pvaibhav 40:8e852115fe55 25 float const _maximum = 0.0)
pvaibhav 46:fd5a62296b12 26 : Kp(_Kp), Ki(_Ki), Kd(_Kd),
pvaibhav 46:fd5a62296b12 27 setPoint(_setpoint),
pvaibhav 46:fd5a62296b12 28 clippingEnabled(_clippingEnabled), maximum(_maximum),
pvaibhav 46:fd5a62296b12 29 e(0), int_e(0), diff_e(0), prev_e(0) { }
pvaibhav 40:8e852115fe55 30
pvaibhav 40:8e852115fe55 31 float Kp; ///< The proportional term of the PID controller
pvaibhav 40:8e852115fe55 32 float Ki; ///< The integral term of the PID controller
pvaibhav 40:8e852115fe55 33 float Kd; ///< The differential term of the PID controller
pvaibhav 40:8e852115fe55 34 float setPoint; ///< The set point of the PID controller (the output value that it will try to maintain)
pvaibhav 40:8e852115fe55 35 bool clippingEnabled; ///< Whether to clip the integral term so that it never exceeds the maximum
pvaibhav 40:8e852115fe55 36 float maximum; ///< The maximum value the integral term is allowed to reach, if clipping is enabled
pvaibhav 40:8e852115fe55 37 float e; ///< error signal (setpoint minus input)
pvaibhav 40:8e852115fe55 38 float int_e; ///< integral of error signal
pvaibhav 40:8e852115fe55 39 float diff_e; ///< derivative of error signal
pvaibhav 40:8e852115fe55 40 float prev_e; ///< error at t_-1
pvaibhav 40:8e852115fe55 41
pvaibhav 40:8e852115fe55 42 /// Reset PID controller (integral, differential) to zero
pvaibhav 46:fd5a62296b12 43 void reset() {
pvaibhav 40:8e852115fe55 44 prev_e = 0.0f;
pvaibhav 40:8e852115fe55 45 int_e = 0.0f;
pvaibhav 40:8e852115fe55 46 }
pvaibhav 40:8e852115fe55 47
pvaibhav 40:8e852115fe55 48 /// Performs one iteration of the PID control loop. If clipping
pvaibhav 40:8e852115fe55 49 /// @param input The input to the PID controller
pvaibhav 40:8e852115fe55 50 /// @return The PID controller's output
pvaibhav 46:fd5a62296b12 51 float output(float const input) {
pvaibhav 40:8e852115fe55 52 // find error from set point
pvaibhav 40:8e852115fe55 53 e = setPoint - input;
pvaibhav 46:fd5a62296b12 54
pvaibhav 40:8e852115fe55 55 // differentiate error
pvaibhav 40:8e852115fe55 56 diff_e = (e - prev_e);
pvaibhav 40:8e852115fe55 57 prev_e = e;
pvaibhav 46:fd5a62296b12 58
pvaibhav 40:8e852115fe55 59 // integral term
pvaibhav 40:8e852115fe55 60 int_e = int_e + e;
pvaibhav 46:fd5a62296b12 61
pvaibhav 40:8e852115fe55 62 float const maxInte = maximum / Ki;
pvaibhav 46:fd5a62296b12 63
pvaibhav 40:8e852115fe55 64 if (clippingEnabled) {
pvaibhav 40:8e852115fe55 65 if (int_e > maxInte)
pvaibhav 40:8e852115fe55 66 int_e = maxInte;
pvaibhav 40:8e852115fe55 67 if (int_e < -maxInte)
pvaibhav 40:8e852115fe55 68 int_e = -maxInte;
pvaibhav 40:8e852115fe55 69 }
pvaibhav 46:fd5a62296b12 70
pvaibhav 40:8e852115fe55 71 return (e * Kp + int_e * Ki + diff_e * Kd);
pvaibhav 40:8e852115fe55 72 }
pvaibhav 40:8e852115fe55 73 };
pvaibhav 40:8e852115fe55 74
pvaibhav 40:8e852115fe55 75 #endif//_H_PIDCONTROLLER_H