Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

ThrottleMapper/ThrottleMapper.cpp

Committer:
bwang
Date:
2017-01-06
Revision:
42:030e0ec4eac5
Child:
56:c681001dfa46

File content as of revision 42:030e0ec4eac5:

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