Rate limiter class.
Dependents: BLDC_mainProgram L298N-Breakout-Test HBridgeDCMotor STMF302R8_MotorDrive ... more
RateLimiter.cpp@0:d735360f91f9, 2015-01-14 (annotated)
- Committer:
- tbjazic
- Date:
- Wed Jan 14 08:49:46 2015 +0000
- Revision:
- 0:d735360f91f9
Initial commit.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tbjazic | 0:d735360f91f9 | 1 | #include "RateLimiter.h" |
tbjazic | 0:d735360f91f9 | 2 | |
tbjazic | 0:d735360f91f9 | 3 | RateLimiter::RateLimiter () { |
tbjazic | 0:d735360f91f9 | 4 | setLimits(1, -1, 0, 10e-3); |
tbjazic | 0:d735360f91f9 | 5 | } |
tbjazic | 0:d735360f91f9 | 6 | |
tbjazic | 0:d735360f91f9 | 7 | RateLimiter::RateLimiter (float R, float F, float y0, float T_d) { |
tbjazic | 0:d735360f91f9 | 8 | setLimits(R, F, y0, T_d); |
tbjazic | 0:d735360f91f9 | 9 | } |
tbjazic | 0:d735360f91f9 | 10 | |
tbjazic | 0:d735360f91f9 | 11 | void RateLimiter::reset() { |
tbjazic | 0:d735360f91f9 | 12 | y0 = 0; |
tbjazic | 0:d735360f91f9 | 13 | } |
tbjazic | 0:d735360f91f9 | 14 | |
tbjazic | 0:d735360f91f9 | 15 | void RateLimiter::setLimits(float R, float F) { |
tbjazic | 0:d735360f91f9 | 16 | if (R < 0) { |
tbjazic | 0:d735360f91f9 | 17 | R = -R; |
tbjazic | 0:d735360f91f9 | 18 | } |
tbjazic | 0:d735360f91f9 | 19 | if (F > 0) { |
tbjazic | 0:d735360f91f9 | 20 | F = -F; |
tbjazic | 0:d735360f91f9 | 21 | } |
tbjazic | 0:d735360f91f9 | 22 | this->R = R; |
tbjazic | 0:d735360f91f9 | 23 | this->F = F; |
tbjazic | 0:d735360f91f9 | 24 | } |
tbjazic | 0:d735360f91f9 | 25 | |
tbjazic | 0:d735360f91f9 | 26 | void RateLimiter::setLimits(float R, float F, float y0, float T_d) { |
tbjazic | 0:d735360f91f9 | 27 | if (R < 0) { |
tbjazic | 0:d735360f91f9 | 28 | R = -R; |
tbjazic | 0:d735360f91f9 | 29 | } |
tbjazic | 0:d735360f91f9 | 30 | if (F > 0) { |
tbjazic | 0:d735360f91f9 | 31 | F = -F; |
tbjazic | 0:d735360f91f9 | 32 | } |
tbjazic | 0:d735360f91f9 | 33 | if (T_d <= 1e-6) { |
tbjazic | 0:d735360f91f9 | 34 | T_d = 1e-3; |
tbjazic | 0:d735360f91f9 | 35 | } |
tbjazic | 0:d735360f91f9 | 36 | this->T_d = T_d; |
tbjazic | 0:d735360f91f9 | 37 | this->y0 = y0; |
tbjazic | 0:d735360f91f9 | 38 | this->R = R; |
tbjazic | 0:d735360f91f9 | 39 | this->F = F; |
tbjazic | 0:d735360f91f9 | 40 | } |
tbjazic | 0:d735360f91f9 | 41 | |
tbjazic | 0:d735360f91f9 | 42 | float RateLimiter::out (float u) { |
tbjazic | 0:d735360f91f9 | 43 | rate = (u - y0) / T_d; |
tbjazic | 0:d735360f91f9 | 44 | if (rate > R) { |
tbjazic | 0:d735360f91f9 | 45 | y = R * T_d + y0; |
tbjazic | 0:d735360f91f9 | 46 | } else if (rate < F) { |
tbjazic | 0:d735360f91f9 | 47 | y = F * T_d + y0; |
tbjazic | 0:d735360f91f9 | 48 | } else { |
tbjazic | 0:d735360f91f9 | 49 | y = u; |
tbjazic | 0:d735360f91f9 | 50 | } |
tbjazic | 0:d735360f91f9 | 51 | y0 = y; |
tbjazic | 0:d735360f91f9 | 52 | return y; |
tbjazic | 0:d735360f91f9 | 53 | } |