N K
/
analoghalls6
motor spins
Fork of analoghalls5 by
Diff: pidcontroller.cpp
- Revision:
- 0:54cf32d35f4d
- Child:
- 1:1f58bdcf2956
diff -r 000000000000 -r 54cf32d35f4d pidcontroller.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pidcontroller.cpp Sun Mar 01 06:16:01 2015 +0000 @@ -0,0 +1,25 @@ +#include "includes.h" +#include "classes.h" + +PidController::PidController(float ki, float kp, float kd, float out_max, float out_min) { + _ki = ki; + _kp = kp; + _kd = kd; + _last_in = 0.0f; + _integral = 0.0f; + _out_max = out_max; + _out_min = out_min; +} + +float PidController::Update(float ref, float in) { + float error = ref - in; + _integral += error; + if (_integral > _out_max) _integral = _out_max; + if (_integral < _out_min) _integral = _out_min; + float deriv = _last_in - in; + _last_in = in; + float tmp = _ki * _integral + _kp * error + _kd * deriv; + if (tmp > _out_max) tmp = _out_max; + if (tmp < _out_min) tmp = _out_min; + return tmp; +} \ No newline at end of file