working-est copy with class-based code. still open loop
Fork of analoghalls6 by
pidcontroller.cpp@9:d3b70c15baa9, 2015-03-06 (annotated)
- Committer:
- nki
- Date:
- Fri Mar 06 19:12:53 2015 +0000
- Revision:
- 9:d3b70c15baa9
- Parent:
- 1:1f58bdcf2956
- Child:
- 10:b4abecccec7a
loop is closed.;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 0:54cf32d35f4d | 1 | #include "includes.h" |
bwang | 1:1f58bdcf2956 | 2 | #include "meta.h" |
bwang | 0:54cf32d35f4d | 3 | |
bwang | 0:54cf32d35f4d | 4 | PidController::PidController(float ki, float kp, float kd, float out_max, float out_min) { |
bwang | 0:54cf32d35f4d | 5 | _ki = ki; |
bwang | 0:54cf32d35f4d | 6 | _kp = kp; |
bwang | 0:54cf32d35f4d | 7 | _kd = kd; |
bwang | 0:54cf32d35f4d | 8 | _last_in = 0.0f; |
bwang | 0:54cf32d35f4d | 9 | _integral = 0.0f; |
bwang | 0:54cf32d35f4d | 10 | _out_max = out_max; |
bwang | 0:54cf32d35f4d | 11 | _out_min = out_min; |
bwang | 0:54cf32d35f4d | 12 | } |
bwang | 0:54cf32d35f4d | 13 | |
bwang | 0:54cf32d35f4d | 14 | float PidController::Update(float ref, float in) { |
nki | 9:d3b70c15baa9 | 15 | float error = in - ref; |
bwang | 0:54cf32d35f4d | 16 | _integral += error; |
nki | 9:d3b70c15baa9 | 17 | if (_integral*_ki > _out_max) _integral = _out_max/_ki; |
nki | 9:d3b70c15baa9 | 18 | if (_integral*_ki < _out_min) _integral = _out_min/_ki; |
bwang | 0:54cf32d35f4d | 19 | float deriv = _last_in - in; |
bwang | 0:54cf32d35f4d | 20 | _last_in = in; |
bwang | 0:54cf32d35f4d | 21 | float tmp = _ki * _integral + _kp * error + _kd * deriv; |
bwang | 0:54cf32d35f4d | 22 | if (tmp > _out_max) tmp = _out_max; |
bwang | 0:54cf32d35f4d | 23 | if (tmp < _out_min) tmp = _out_min; |
bwang | 0:54cf32d35f4d | 24 | return tmp; |
bwang | 0:54cf32d35f4d | 25 | } |