Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: PIDController.h
- Revision:
- 46:fd5a62296b12
- Parent:
- 40:8e852115fe55
--- a/PIDController.h Wed May 27 11:45:00 2015 +0000 +++ b/PIDController.h Wed May 27 13:01:43 2015 +0000 @@ -23,10 +23,10 @@ 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) { } + : 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 @@ -40,8 +40,7 @@ float prev_e; ///< error at t_-1 /// Reset PID controller (integral, differential) to zero - void reset() - { + void reset() { prev_e = 0.0f; int_e = 0.0f; } @@ -49,27 +48,26 @@ /// 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) - { + 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); } };