pidcontroller.cpp

00001 #include "includes.h"
00002 #include "filters.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) {
00015     float error = ref - in;
00016 
00017     _integral += error;
00018     
00019     if (_integral * _ki > _out_max) _integral = _out_max / _ki;
00020     if (_integral * _ki < _out_min) _integral = _out_min / _ki;
00021     
00022     float deriv = _last_in - in;
00023     _last_in = in;
00024     float tmp = _ki * _integral + _kp * error + _kd * deriv;
00025     
00026     if (tmp > _out_max) tmp = _out_max;
00027     if (tmp < _out_min) tmp = _out_min;
00028     
00029     return tmp;
00030 }
00031 
00032 void PidController::ZeroIntegrator() {
00033     _integral = 0.0f;
00034     }