N K
a fork of priustroller
Fork of
priustroller_current
by 
N K
meta/pidcontroller.cpp@11:dccbaa9274c5, 2015-03-08 (annotated)
- Committer:
- bwang
- Date:
- Sun Mar 08 08:37:38 2015 +0000
- Revision:
- 11:dccbaa9274c5
- Child:
- 21:da41c08e51bc
--Initial Commit: 03-08-2015 new context-based code; no more timers (=easier debugging)
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| bwang | 11:dccbaa9274c5 | 1 | #include "includes.h" |
| bwang | 11:dccbaa9274c5 | 2 | #include "filters.h" |
| bwang | 11:dccbaa9274c5 | 3 | |
| bwang | 11:dccbaa9274c5 | 4 | PidController::PidController(float ki, float kp, float kd, float out_max, float out_min) { |
| bwang | 11:dccbaa9274c5 | 5 | _ki = ki; |
| bwang | 11:dccbaa9274c5 | 6 | _kp = kp; |
| bwang | 11:dccbaa9274c5 | 7 | _kd = kd; |
| bwang | 11:dccbaa9274c5 | 8 | _last_in = 0.0f; |
| bwang | 11:dccbaa9274c5 | 9 | _integral = 0.0f; |
| bwang | 11:dccbaa9274c5 | 10 | _out_max = out_max; |
| bwang | 11:dccbaa9274c5 | 11 | _out_min = out_min; |
| bwang | 11:dccbaa9274c5 | 12 | } |
| bwang | 11:dccbaa9274c5 | 13 | |
| bwang | 11:dccbaa9274c5 | 14 | float PidController::Update(float ref, float in) { |
| bwang | 11:dccbaa9274c5 | 15 | float error = ref - in; |
| bwang | 11:dccbaa9274c5 | 16 | _integral += error; |
| bwang | 11:dccbaa9274c5 | 17 | if (_integral * _ki > _out_max) _integral = _out_max / _ki; |
| bwang | 11:dccbaa9274c5 | 18 | if (_integral * _ki < _out_min) _integral = _out_min / _ki; |
| bwang | 11:dccbaa9274c5 | 19 | float deriv = _last_in - in; |
| bwang | 11:dccbaa9274c5 | 20 | _last_in = in; |
| bwang | 11:dccbaa9274c5 | 21 | float tmp = _ki * _integral + _kp * error + _kd * deriv; |
| bwang | 11:dccbaa9274c5 | 22 | if (tmp > _out_max) tmp = _out_max; |
| bwang | 11:dccbaa9274c5 | 23 | if (tmp < _out_min) tmp = _out_min; |
| bwang | 11:dccbaa9274c5 | 24 | return tmp; |
| bwang | 11:dccbaa9274c5 | 25 | } |