Bayley Wang
/
foc-ed_in_the_bot_compact
robot
ThrottleMapper/ThrottleMapper.cpp
- Committer:
- bwang
- Date:
- 2017-07-01
- Revision:
- 160:6948bb7bcabd
- Parent:
- 111:451e40aed753
- Child:
- 185:5c102874b490
File content as of revision 160:6948bb7bcabd:
#include "ThrottleMapper.h" #include "config_motor.h" #include "config_inverter.h" #include "config_driving.h" float DrivingThrottleMapper::map(float throttle, float w) { if (throttle < 0.0f) throttle = 0.0f; 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 (throttle < 0.0f) throttle = 0.0f; if (w <= __wlim) { return throttle; } if (w > __wmax) { w = __wmax; } return (__wmax - w) / (__wmax - __wlim) * throttle; } float AutoThrottleMapper::map(float throttle, float w) { if (throttle < 0.5f) return 0.0f; val += 1.0f / _period / _rate; if (val > 1.0f) val = 0.0f; return val; }