working-est copy with class-based code. still open loop
Fork of analoghalls6 by
pidcontroller.cpp
- Committer:
- nki
- Date:
- 2015-03-06
- Revision:
- 9:d3b70c15baa9
- Parent:
- 1:1f58bdcf2956
- Child:
- 10:b4abecccec7a
File content as of revision 9:d3b70c15baa9:
#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) { float error = in - ref; _integral += error; 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; }