working-est copy with class-based code. still open loop
Fork of analoghalls6 by
pidcontroller.cpp
- Committer:
- nki
- Date:
- 2015-03-08
- Revision:
- 10:b4abecccec7a
- Parent:
- 9:d3b70c15baa9
File content as of revision 10:b4abecccec7a:
#include "includes.h" #include "meta.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) { //starts positive, integrator is adding up the error to get there. cranking throttle makes it go negative. that makes sense. float error = ref - in; //_integral += error; _integral = 0.0f; if (_integral*_ki > _out_max) _integral = _out_max/_ki; if (_integral*_ki < _out_min) _integral = _out_min/_ki; 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; }