Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
PIDController.h
- Committer:
- pvaibhav
- Date:
- 2015-05-26
- Revision:
- 40:8e852115fe55
- Child:
- 46:fd5a62296b12
File content as of revision 40:8e852115fe55:
#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