pidcontroller.cpp

00001 #include "includes.h"
00002 #include "meta.h"
00003 
00004 PidController::PidController(float ki, float kp, float kd, float out_max, float out_min) {
00005     _ki = ki;
00006     _kp = kp;
00007     _kd = kd;
00008     _last_in = 0.0f;
00009     _integral = 0.0f;
00010     _out_max = out_max;
00011     _out_min = out_min;
00012 }
00013 
00014 float PidController::Update(float ref, float in) {  //starts positive, integrator is adding up the error to get there.  cranking throttle makes it go negative.  that makes sense.
00015     float error = ref - in;
00016     //_integral += error;
00017     _integral = 0.0f;
00018     if (_integral*_ki > _out_max) _integral = _out_max/_ki;
00019     if (_integral*_ki < _out_min) _integral = _out_min/_ki;
00020     float deriv = _last_in - in;
00021     _last_in = in;
00022     float tmp = _ki * _integral + _kp * error + _kd * deriv;
00023     if (tmp > _out_max) tmp = _out_max;
00024     if (tmp < _out_min) tmp = _out_min;
00025     return tmp;
00026 }