From 1f6eb467718d562501b6de52d1173ae594b235d8 Mon Sep 17 00:00:00 2001
From: 0xFEEDC0DE64 <daniel@brunner.ninja>
Date: Fri, 8 Apr 2022 20:54:02 +0200
Subject: [PATCH] Fixed seatbot

---
 main/displays/joystickdebugdisplay.cpp |  4 +--
 main/potis.cpp                         |  4 +--
 main/utils.cpp                         | 48 ++++++++++++++++----------
 main/utils.h                           |  2 +-
 4 files changed, 35 insertions(+), 23 deletions(-)

diff --git a/main/displays/joystickdebugdisplay.cpp b/main/displays/joystickdebugdisplay.cpp
index 482ec63..c728447 100644
--- a/main/displays/joystickdebugdisplay.cpp
+++ b/main/displays/joystickdebugdisplay.cpp
@@ -40,13 +40,13 @@ void JoystickDebugDisplay::update()
         m_x = std::nullopt;
     else
     {
-        m_x = map_analog_stick(m_gasMitte, m_gasMin, m_gasMax, *raw_gas);
+        m_x = map_analog_stick(m_gasMitte, m_gasMin, m_gasMax, configs.deadband.value, *raw_gas);
     }
 
     if (!raw_brems)
         m_y = std::nullopt;
     else
-        m_y = map_analog_stick(m_bremsMitte, m_bremsMin, m_bremsMax, *raw_brems);
+        m_y = map_analog_stick(m_bremsMitte, m_bremsMin, m_bremsMax, configs.deadband.value, *raw_brems);
 }
 
 void JoystickDebugDisplay::redraw()
diff --git a/main/potis.cpp b/main/potis.cpp
index 256ed41..dcba266 100644
--- a/main/potis.cpp
+++ b/main/potis.cpp
@@ -78,13 +78,13 @@ void readPotis()
 #else
     if (raw_gas)
     {
-        gas = map_analog_stick(configs.gasMitte.value, configs.gasMin.value, configs.gasMax.value, *raw_gas);
+        gas = map_analog_stick(configs.gasMitte.value, configs.gasMin.value, configs.gasMax.value, configs.deadband.value, *raw_gas);
     }
     else
         gas = std::nullopt;
     if (raw_brems)
     {
-        brems = map_analog_stick(configs.bremsMitte.value, configs.bremsMin.value, configs.bremsMax.value, *raw_brems);
+        brems = map_analog_stick(configs.bremsMitte.value, configs.bremsMin.value, configs.bremsMax.value, configs.deadband.value, *raw_brems);
     }
     else
         brems = std::nullopt;
diff --git a/main/utils.cpp b/main/utils.cpp
index e87c634..0f3abdd 100644
--- a/main/utils.cpp
+++ b/main/utils.cpp
@@ -283,34 +283,46 @@ std::string local_clock_string()
     return fmt::format("{:02d}:{:02d}:{:02d}", dt.hour, dt.minute, dt.second);
 }
 
-int16_t map_analog_stick(uint16_t middle, uint16_t start, uint16_t end, uint16_t raw)
+int16_t map_analog_stick(uint16_t middle, uint16_t start, uint16_t end, uint16_t deadband, uint16_t raw)
 {
     if (abs(raw - middle) < configs.deadband.value)
     {
         return 0;
     }
 
-    if (raw < middle)
+    if (start < end)
     {
-        raw += configs.deadband.value;
-        start += configs.deadband.value;
-        const auto return_val = map(raw, start, middle, -1000, 0);
-        if (return_val > 0)
-            return 0;
-        if (return_val < -1000)
-            return -1000;
-        return return_val;
+        if (raw < middle)
+        {
+            raw += configs.deadband.value;
+            start += configs.deadband.value;
+            const auto return_val = cpputils::mapValueClamped<float>(raw, start, middle, -1000, 0);
+            return return_val;
+        }
+        else
+        {
+            raw -= configs.deadband.value;
+            end -= configs.deadband.value;
+            const auto return_val = cpputils::mapValueClamped<float>(raw, middle, end, 0, 1000);
+            return return_val;
+        }
     }
     else
     {
-        raw -= configs.deadband.value;
-        end -= configs.deadband.value;
-        const auto return_val = map(raw, middle, end, 0, 1000);
-        if (return_val < 0)
-            return 0;
-        if (return_val > 1000)
-            return 1000;
-        return return_val;
+        if (raw < middle)
+        {
+            raw += configs.deadband.value;
+            end  += configs.deadband.value;
+            const auto return_val = cpputils::mapValueClamped<float>(raw, end, middle, 1000, 0);
+            return return_val;
+        }
+        else
+        {
+            raw -= configs.deadband.value;
+            start -= configs.deadband.value;
+            const auto return_val = cpputils::mapValueClamped<float>(raw, middle, start, 0, -1000);
+            return return_val;
+        }
     }
 }
 
diff --git a/main/utils.h b/main/utils.h
index 032ea38..4811a0b 100644
--- a/main/utils.h
+++ b/main/utils.h
@@ -60,7 +60,7 @@ float wattToAmpere(float watt);
 float wattToMotorCurrent(float watt);
 uint8_t time_to_percent(espchrono::milliseconds32 repeat, espchrono::milliseconds32 riseTime, espchrono::milliseconds32 fullTime, size_t numLeds, bool invert);
 std::string local_clock_string();
-int16_t map_analog_stick(uint16_t middle, uint16_t start, uint16_t end, uint16_t raw);
+int16_t map_analog_stick(uint16_t middle, uint16_t start, uint16_t end, uint16_t deadband, uint16_t raw);
 
 inline uint32_t CRGB_TO_UINT32(CRGB crgb)
 {