Bayley Wang
/
foc-ed_in_the_bot_compact
robot
ThrottleMapper/ThrottleMapper.cpp@252:38644631ed97, 2018-11-13 (annotated)
- Committer:
- bwang
- Date:
- Tue Nov 13 17:46:23 2018 +0000
- Revision:
- 252:38644631ed97
- Parent:
- 192:3152a86cd108
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 42:030e0ec4eac5 | 1 | #include "ThrottleMapper.h" |
bwang | 185:5c102874b490 | 2 | #include "derived.h" |
bwang | 192:3152a86cd108 | 3 | #include "prefs.h" |
bwang | 42:030e0ec4eac5 | 4 | |
bwang | 42:030e0ec4eac5 | 5 | float DrivingThrottleMapper::map(float throttle, float w) { |
bwang | 111:451e40aed753 | 6 | if (throttle < 0.0f) throttle = 0.0f; |
bwang | 111:451e40aed753 | 7 | |
bwang | 42:030e0ec4eac5 | 8 | float z = getZeroTqThrottle(w); |
bwang | 42:030e0ec4eac5 | 9 | float tq, tqmax; |
bwang | 42:030e0ec4eac5 | 10 | |
bwang | 42:030e0ec4eac5 | 11 | if (throttle > z) tqmax = getMaxTqpctPlus(w); |
bwang | 42:030e0ec4eac5 | 12 | if (throttle <= z) tqmax = getMaxTqpctMinus(w); |
bwang | 42:030e0ec4eac5 | 13 | |
bwang | 42:030e0ec4eac5 | 14 | if (throttle > z) tq = (throttle - z) / (1.0f - z); |
bwang | 42:030e0ec4eac5 | 15 | if (throttle <= z) tq = (throttle - z) / z; |
bwang | 42:030e0ec4eac5 | 16 | |
bwang | 42:030e0ec4eac5 | 17 | if (tq > tqmax) tq = tqmax; |
bwang | 42:030e0ec4eac5 | 18 | return tq; |
bwang | 42:030e0ec4eac5 | 19 | } |
bwang | 42:030e0ec4eac5 | 20 | |
bwang | 42:030e0ec4eac5 | 21 | float DrivingThrottleMapper::getMaxTqpctPlus(float w) { |
bwang | 192:3152a86cd108 | 22 | return _MAX_TQPCT_PLUS; |
bwang | 42:030e0ec4eac5 | 23 | } |
bwang | 42:030e0ec4eac5 | 24 | |
bwang | 42:030e0ec4eac5 | 25 | float DrivingThrottleMapper::getMaxTqpctMinus(float w) { |
bwang | 192:3152a86cd108 | 26 | return _MAX_TQPCT_MINUS; |
bwang | 42:030e0ec4eac5 | 27 | } |
bwang | 42:030e0ec4eac5 | 28 | |
bwang | 42:030e0ec4eac5 | 29 | float DrivingThrottleMapper::getZeroTqThrottle(float w) { |
bwang | 192:3152a86cd108 | 30 | float tmp = w / _W_MAX; |
bwang | 42:030e0ec4eac5 | 31 | return tmp < 1.0f ? tmp : 1.0f; |
bwang | 56:c681001dfa46 | 32 | } |
bwang | 56:c681001dfa46 | 33 | |
bwang | 56:c681001dfa46 | 34 | LimitingThrottleMapper::LimitingThrottleMapper(float wmax) { |
bwang | 56:c681001dfa46 | 35 | __wmax = wmax; |
bwang | 56:c681001dfa46 | 36 | __wlim = 0.95f * __wmax; |
bwang | 56:c681001dfa46 | 37 | } |
bwang | 56:c681001dfa46 | 38 | |
bwang | 56:c681001dfa46 | 39 | float LimitingThrottleMapper::map(float throttle, float w) { |
bwang | 111:451e40aed753 | 40 | if (throttle < 0.0f) throttle = 0.0f; |
bwang | 111:451e40aed753 | 41 | |
bwang | 56:c681001dfa46 | 42 | if (w <= __wlim) { |
bwang | 56:c681001dfa46 | 43 | return throttle; |
bwang | 56:c681001dfa46 | 44 | } |
bwang | 56:c681001dfa46 | 45 | if (w > __wmax) { |
bwang | 56:c681001dfa46 | 46 | w = __wmax; |
bwang | 56:c681001dfa46 | 47 | } |
bwang | 56:c681001dfa46 | 48 | return (__wmax - w) / (__wmax - __wlim) * throttle; |
bwang | 160:6948bb7bcabd | 49 | } |
bwang | 160:6948bb7bcabd | 50 | |
bwang | 160:6948bb7bcabd | 51 | float AutoThrottleMapper::map(float throttle, float w) { |
bwang | 160:6948bb7bcabd | 52 | if (throttle < 0.5f) return 0.0f; |
bwang | 160:6948bb7bcabd | 53 | val += 1.0f / _period / _rate; |
bwang | 160:6948bb7bcabd | 54 | if (val > 1.0f) val = 0.0f; |
bwang | 160:6948bb7bcabd | 55 | return val; |
bwang | 42:030e0ec4eac5 | 56 | } |