Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: ThrottleMapper/ThrottleMapper.cpp
- Revision:
- 42:030e0ec4eac5
- Child:
- 56:c681001dfa46
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ThrottleMapper/ThrottleMapper.cpp Fri Jan 06 07:54:22 2017 +0000 @@ -0,0 +1,32 @@ +#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; +} \ No newline at end of file