TVZ Mechatronics Team / RateLimiter

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?

UserRevisionLine numberNew 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 }