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