Bayley Wang
/
foc-ed_in_the_bot_compact
robot
ThrottleMapper/ThrottleMapper.cpp@155:7c6005933d4c, 2017-05-04 (annotated)
- Committer:
- bwang
- Date:
- Thu May 04 17:35:10 2017 +0000
- Revision:
- 155:7c6005933d4c
- Parent:
- 111:451e40aed753
- Child:
- 160:6948bb7bcabd
05/04/2017 13:34 - changed logic in PwmIn::handle_fall, which no longer resets timer on invalid edges
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 42:030e0ec4eac5 | 1 | #include "ThrottleMapper.h" |
bwang | 42:030e0ec4eac5 | 2 | |
bwang | 42:030e0ec4eac5 | 3 | #include "config_motor.h" |
bwang | 42:030e0ec4eac5 | 4 | #include "config_inverter.h" |
bwang | 42:030e0ec4eac5 | 5 | #include "config_driving.h" |
bwang | 42:030e0ec4eac5 | 6 | |
bwang | 42:030e0ec4eac5 | 7 | float DrivingThrottleMapper::map(float throttle, float w) { |
bwang | 111:451e40aed753 | 8 | if (throttle < 0.0f) throttle = 0.0f; |
bwang | 111:451e40aed753 | 9 | |
bwang | 42:030e0ec4eac5 | 10 | float z = getZeroTqThrottle(w); |
bwang | 42:030e0ec4eac5 | 11 | float tq, tqmax; |
bwang | 42:030e0ec4eac5 | 12 | |
bwang | 42:030e0ec4eac5 | 13 | if (throttle > z) tqmax = getMaxTqpctPlus(w); |
bwang | 42:030e0ec4eac5 | 14 | if (throttle <= z) tqmax = getMaxTqpctMinus(w); |
bwang | 42:030e0ec4eac5 | 15 | |
bwang | 42:030e0ec4eac5 | 16 | if (throttle > z) tq = (throttle - z) / (1.0f - z); |
bwang | 42:030e0ec4eac5 | 17 | if (throttle <= z) tq = (throttle - z) / z; |
bwang | 42:030e0ec4eac5 | 18 | |
bwang | 42:030e0ec4eac5 | 19 | if (tq > tqmax) tq = tqmax; |
bwang | 42:030e0ec4eac5 | 20 | return tq; |
bwang | 42:030e0ec4eac5 | 21 | } |
bwang | 42:030e0ec4eac5 | 22 | |
bwang | 42:030e0ec4eac5 | 23 | float DrivingThrottleMapper::getMaxTqpctPlus(float w) { |
bwang | 42:030e0ec4eac5 | 24 | return MAX_TQPCT_PLUS; |
bwang | 42:030e0ec4eac5 | 25 | } |
bwang | 42:030e0ec4eac5 | 26 | |
bwang | 42:030e0ec4eac5 | 27 | float DrivingThrottleMapper::getMaxTqpctMinus(float w) { |
bwang | 42:030e0ec4eac5 | 28 | return MAX_TQPCT_MINUS; |
bwang | 42:030e0ec4eac5 | 29 | } |
bwang | 42:030e0ec4eac5 | 30 | |
bwang | 42:030e0ec4eac5 | 31 | float DrivingThrottleMapper::getZeroTqThrottle(float w) { |
bwang | 42:030e0ec4eac5 | 32 | float tmp = w / W_MAX; |
bwang | 42:030e0ec4eac5 | 33 | return tmp < 1.0f ? tmp : 1.0f; |
bwang | 56:c681001dfa46 | 34 | } |
bwang | 56:c681001dfa46 | 35 | |
bwang | 56:c681001dfa46 | 36 | LimitingThrottleMapper::LimitingThrottleMapper(float wmax) { |
bwang | 56:c681001dfa46 | 37 | __wmax = wmax; |
bwang | 56:c681001dfa46 | 38 | __wlim = 0.95f * __wmax; |
bwang | 56:c681001dfa46 | 39 | } |
bwang | 56:c681001dfa46 | 40 | |
bwang | 56:c681001dfa46 | 41 | float LimitingThrottleMapper::map(float throttle, float w) { |
bwang | 111:451e40aed753 | 42 | if (throttle < 0.0f) throttle = 0.0f; |
bwang | 111:451e40aed753 | 43 | |
bwang | 56:c681001dfa46 | 44 | if (w <= __wlim) { |
bwang | 56:c681001dfa46 | 45 | return throttle; |
bwang | 56:c681001dfa46 | 46 | } |
bwang | 56:c681001dfa46 | 47 | if (w > __wmax) { |
bwang | 56:c681001dfa46 | 48 | w = __wmax; |
bwang | 56:c681001dfa46 | 49 | } |
bwang | 56:c681001dfa46 | 50 | return (__wmax - w) / (__wmax - __wlim) * throttle; |
bwang | 42:030e0ec4eac5 | 51 | } |