Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

PIDController.h@40:8e852115fe55, 2015-05-26 (annotated)

Committer:
pvaibhav
Date:
Tue May 26 11:28:37 2015 +0000
Revision:
40:8e852115fe55
Child:
46:fd5a62296b12
SensorFusion base class and 6 axis derived class. utils, filter and pid classes moved inside SML2.

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 40:8e852115fe55 26 : Kp(_Kp), Ki(_Ki), Kd(_Kd),
pvaibhav 40:8e852115fe55 27 setPoint(_setpoint),
pvaibhav 40:8e852115fe55 28 clippingEnabled(_clippingEnabled), maximum(_maximum),
pvaibhav 40:8e852115fe55 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 40:8e852115fe55 43 void reset()
pvaibhav 40:8e852115fe55 44 {
pvaibhav 40:8e852115fe55 45 prev_e = 0.0f;
pvaibhav 40:8e852115fe55 46 int_e = 0.0f;
pvaibhav 40:8e852115fe55 47 }
pvaibhav 40:8e852115fe55 48
pvaibhav 40:8e852115fe55 49 /// Performs one iteration of the PID control loop. If clipping
pvaibhav 40:8e852115fe55 50 /// @param input The input to the PID controller
pvaibhav 40:8e852115fe55 51 /// @return The PID controller's output
pvaibhav 40:8e852115fe55 52 float output(float const input)
pvaibhav 40:8e852115fe55 53 {
pvaibhav 40:8e852115fe55 54 // find error from set point
pvaibhav 40:8e852115fe55 55 e = setPoint - input;
pvaibhav 40:8e852115fe55 56
pvaibhav 40:8e852115fe55 57 // differentiate error
pvaibhav 40:8e852115fe55 58 diff_e = (e - prev_e);
pvaibhav 40:8e852115fe55 59 prev_e = e;
pvaibhav 40:8e852115fe55 60
pvaibhav 40:8e852115fe55 61 // integral term
pvaibhav 40:8e852115fe55 62 int_e = int_e + e;
pvaibhav 40:8e852115fe55 63
pvaibhav 40:8e852115fe55 64 float const maxInte = maximum / Ki;
pvaibhav 40:8e852115fe55 65
pvaibhav 40:8e852115fe55 66 if (clippingEnabled) {
pvaibhav 40:8e852115fe55 67 if (int_e > maxInte)
pvaibhav 40:8e852115fe55 68 int_e = maxInte;
pvaibhav 40:8e852115fe55 69 if (int_e < -maxInte)
pvaibhav 40:8e852115fe55 70 int_e = -maxInte;
pvaibhav 40:8e852115fe55 71 }
pvaibhav 40:8e852115fe55 72
pvaibhav 40:8e852115fe55 73 return (e * Kp + int_e * Ki + diff_e * Kd);
pvaibhav 40:8e852115fe55 74 }
pvaibhav 40:8e852115fe55 75 };
pvaibhav 40:8e852115fe55 76
pvaibhav 40:8e852115fe55 77 #endif//_H_PIDCONTROLLER_H