File content as of revision 155:7c6005933d4c:

#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;
}