Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

ThrottleMapper/ThrottleMapper.cpp@185:5c102874b490, 2018-02-09 (annotated)

Committer:
bwang
Date:
Fri Feb 09 22:19:52 2018 +0000
Revision:
185:5c102874b490
Parent:
160:6948bb7bcabd
Child:
186:c18db1e31da6
moved all the hard-coded values to defaults.h

Who changed what in which revision?

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