Bayley Wang
/
foc-ed_in_the_bot_compact
robot
ThrottleMapper/ThrottleMapper.cpp
- Committer:
- bwang
- Date:
- 2017-04-06
- Revision:
- 92:a9dac72d8cac
- Parent:
- 56:c681001dfa46
- Child:
- 111:451e40aed753
File content as of revision 92:a9dac72d8cac:
#include "ThrottleMapper.h" #include "config_motor.h" #include "config_inverter.h" #include "config_driving.h" float DrivingThrottleMapper::map(float throttle, float w) { float z = getZeroTqThrottle(w); float tq, tqmax; if (throttle > z) tqmax = getMaxTqpctPlus(w); if (throttle <= z) tqmax = getMaxTqpctMinus(w); if (throttle > z) tq = (throttle - z) / (1.0f - z); if (throttle <= z) tq = (throttle - z) / z; if (tq > tqmax) tq = tqmax; return tq; } float DrivingThrottleMapper::getMaxTqpctPlus(float w) { return MAX_TQPCT_PLUS; } float DrivingThrottleMapper::getMaxTqpctMinus(float w) { return MAX_TQPCT_MINUS; } float DrivingThrottleMapper::getZeroTqThrottle(float w) { float tmp = w / W_MAX; return tmp < 1.0f ? tmp : 1.0f; } LimitingThrottleMapper::LimitingThrottleMapper(float wmax) { __wmax = wmax; __wlim = 0.95f * __wmax; } float LimitingThrottleMapper::map(float throttle, float w) { if (w <= __wlim) { return throttle; } if (w > __wmax) { w = __wmax; } return (__wmax - w) / (__wmax - __wlim) * throttle; }