Bayley Wang
/
foc-ed_in_the_bot_compact
robot
ThrottleMapper/ThrottleMapper.cpp@92:a9dac72d8cac, 2017-04-06 (annotated)
- Committer:
- bwang
- Date:
- Thu Apr 06 17:33:47 2017 +0000
- Revision:
- 92:a9dac72d8cac
- Parent:
- 56:c681001dfa46
- Child:
- 111:451e40aed753
--PwmIn now checks lower bounds for sanity and fall without rise; --switched to edge aligned pwm to work around prius module propagation delays
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 | 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 | 42:030e0ec4eac5 | 22 | return MAX_TQPCT_PLUS; |
bwang | 42:030e0ec4eac5 | 23 | } |
bwang | 42:030e0ec4eac5 | 24 | |
bwang | 42:030e0ec4eac5 | 25 | float DrivingThrottleMapper::getMaxTqpctMinus(float w) { |
bwang | 42:030e0ec4eac5 | 26 | return MAX_TQPCT_MINUS; |
bwang | 42:030e0ec4eac5 | 27 | } |
bwang | 42:030e0ec4eac5 | 28 | |
bwang | 42:030e0ec4eac5 | 29 | float DrivingThrottleMapper::getZeroTqThrottle(float w) { |
bwang | 42:030e0ec4eac5 | 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 | 56:c681001dfa46 | 40 | if (w <= __wlim) { |
bwang | 56:c681001dfa46 | 41 | return throttle; |
bwang | 56:c681001dfa46 | 42 | } |
bwang | 56:c681001dfa46 | 43 | if (w > __wmax) { |
bwang | 56:c681001dfa46 | 44 | w = __wmax; |
bwang | 56:c681001dfa46 | 45 | } |
bwang | 56:c681001dfa46 | 46 | return (__wmax - w) / (__wmax - __wlim) * throttle; |
bwang | 42:030e0ec4eac5 | 47 | } |