Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
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?
User | Revision | Line number | New 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 |