Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

ThrottleMapper/ThrottleMapper.cpp@160:6948bb7bcabd, 2017-07-01 (annotated)

Committer:
bwang
Date:
Sat Jul 01 21:16:33 2017 +0000
Revision:
160:6948bb7bcabd
Parent:
111:451e40aed753
Child:
185:5c102874b490
added AutoMapper, AutoThrottleMapper classes for automated stall testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 42:030e0ec4eac5 1 #include "ThrottleMapper.h"
bwang 42:030e0ec4eac5 2
bwang 42:030e0ec4eac5 3 #include "config_motor.h"
bwang 42:030e0ec4eac5 4 #include "config_inverter.h"
bwang 42:030e0ec4eac5 5 #include "config_driving.h"
bwang 42:030e0ec4eac5 6
bwang 42:030e0ec4eac5 7 float DrivingThrottleMapper::map(float throttle, float w) {
bwang 111:451e40aed753 8 if (throttle < 0.0f) throttle = 0.0f;
bwang 111:451e40aed753 9
bwang 42:030e0ec4eac5 10 float z = getZeroTqThrottle(w);
bwang 42:030e0ec4eac5 11 float tq, tqmax;
bwang 42:030e0ec4eac5 12
bwang 42:030e0ec4eac5 13 if (throttle > z) tqmax = getMaxTqpctPlus(w);
bwang 42:030e0ec4eac5 14 if (throttle <= z) tqmax = getMaxTqpctMinus(w);
bwang 42:030e0ec4eac5 15
bwang 42:030e0ec4eac5 16 if (throttle > z) tq = (throttle - z) / (1.0f - z);
bwang 42:030e0ec4eac5 17 if (throttle <= z) tq = (throttle - z) / z;
bwang 42:030e0ec4eac5 18
bwang 42:030e0ec4eac5 19 if (tq > tqmax) tq = tqmax;
bwang 42:030e0ec4eac5 20 return tq;
bwang 42:030e0ec4eac5 21 }
bwang 42:030e0ec4eac5 22
bwang 42:030e0ec4eac5 23 float DrivingThrottleMapper::getMaxTqpctPlus(float w) {
bwang 42:030e0ec4eac5 24 return MAX_TQPCT_PLUS;
bwang 42:030e0ec4eac5 25 }
bwang 42:030e0ec4eac5 26
bwang 42:030e0ec4eac5 27 float DrivingThrottleMapper::getMaxTqpctMinus(float w) {
bwang 42:030e0ec4eac5 28 return MAX_TQPCT_MINUS;
bwang 42:030e0ec4eac5 29 }
bwang 42:030e0ec4eac5 30
bwang 42:030e0ec4eac5 31 float DrivingThrottleMapper::getZeroTqThrottle(float w) {
bwang 42:030e0ec4eac5 32 float tmp = w / W_MAX;
bwang 42:030e0ec4eac5 33 return tmp < 1.0f ? tmp : 1.0f;
bwang 56:c681001dfa46 34 }
bwang 56:c681001dfa46 35
bwang 56:c681001dfa46 36 LimitingThrottleMapper::LimitingThrottleMapper(float wmax) {
bwang 56:c681001dfa46 37 __wmax = wmax;
bwang 56:c681001dfa46 38 __wlim = 0.95f * __wmax;
bwang 56:c681001dfa46 39 }
bwang 56:c681001dfa46 40
bwang 56:c681001dfa46 41 float LimitingThrottleMapper::map(float throttle, float w) {
bwang 111:451e40aed753 42 if (throttle < 0.0f) throttle = 0.0f;
bwang 111:451e40aed753 43
bwang 56:c681001dfa46 44 if (w <= __wlim) {
bwang 56:c681001dfa46 45 return throttle;
bwang 56:c681001dfa46 46 }
bwang 56:c681001dfa46 47 if (w > __wmax) {
bwang 56:c681001dfa46 48 w = __wmax;
bwang 56:c681001dfa46 49 }
bwang 56:c681001dfa46 50 return (__wmax - w) / (__wmax - __wlim) * throttle;
bwang 160:6948bb7bcabd 51 }
bwang 160:6948bb7bcabd 52
bwang 160:6948bb7bcabd 53 float AutoThrottleMapper::map(float throttle, float w) {
bwang 160:6948bb7bcabd 54 if (throttle < 0.5f) return 0.0f;
bwang 160:6948bb7bcabd 55 val += 1.0f / _period / _rate;
bwang 160:6948bb7bcabd 56 if (val > 1.0f) val = 0.0f;
bwang 160:6948bb7bcabd 57 return val;
bwang 42:030e0ec4eac5 58 }