From 8f96a32b578f183c548080632f4a71382b46559f Mon Sep 17 00:00:00 2001
From: 0xFEEDC0DE64 <daniel@brunner.ninja>
Date: Mon, 1 Jun 2020 01:36:11 +0200
Subject: [PATCH 1/2] Implemented basic self driving behaviour with gametrak

---
 platformio.ini           |  4 ++--
 src/modes/gametrakmode.h | 11 ++++++++++-
 2 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index 55b3f2a..e4cf432 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -99,8 +99,8 @@ build_flags =
   -DFEATURE_BMS
   -DFEATURE_GAMETRAK
   -DPINS_GAMETRAKX=34
-  -DPINS_GAMETRAKY=36
-  -DPINS_GAMETRAKDIST=39
+  -DPINS_GAMETRAKY=39
+  -DPINS_GAMETRAKDIST=36
   -DDEFAULT_GAMETRAKXMIN=0
   -DDEFAULT_GAMETRAKXMAX=4095
   -DDEFAULT_GAMETRAKYMIN=0
diff --git a/src/modes/gametrakmode.h b/src/modes/gametrakmode.h
index 475e611..160cd8b 100644
--- a/src/modes/gametrakmode.h
+++ b/src/modes/gametrakmode.h
@@ -3,6 +3,7 @@
 #include "modeinterface.h"
 #include "globals.h"
 #include "utils.h"
+#include "defaultmode.h"
 
 #include "bobbycar-protocol/protocol.h"
 
@@ -22,11 +23,19 @@ GametrakMode gametrakMode;
 
 void GametrakMode::update()
 {
+    if (gas > 500. || brems > 500.)
+    {
+        modes::defaultMode.waitForGasLoslass = true;
+        modes::defaultMode.waitForBremsLoslass = true;
+        currentMode = &modes::defaultMode;
+        return;
+    }
+
     for (MotorState &motor : motors())
     {
         motor.ctrlTyp = ControlType::FieldOrientedControl;
         motor.ctrlMod = ControlMode::Speed;
-        motor.pwm = 0;
+        motor.pwm = gametrakDist > 200 ? 20 : 0;
     }
 
     fixCommonParams();

From bb289f6e7302625fa82e9bea084ace5c49839bad Mon Sep 17 00:00:00 2001
From: 0xFEEDC0DE64 <daniel@brunner.ninja>
Date: Mon, 1 Jun 2020 03:41:18 +0200
Subject: [PATCH 2/2] Implemented better control with gametrak

---
 src/modes/gametrakmode.h | 37 ++++++++++++++++++++++++++++++++++++-
 1 file changed, 36 insertions(+), 1 deletion(-)

diff --git a/src/modes/gametrakmode.h b/src/modes/gametrakmode.h
index 160cd8b..959619b 100644
--- a/src/modes/gametrakmode.h
+++ b/src/modes/gametrakmode.h
@@ -1,5 +1,14 @@
 #pragma once
 
+namespace {
+template<class T>
+constexpr const T& clamp( const T& v, const T& lo, const T& hi )
+{
+    assert( !(hi < lo) );
+    return (v < lo) ? lo : (hi < v) ? hi : v;
+}
+}
+
 #include "modeinterface.h"
 #include "globals.h"
 #include "utils.h"
@@ -12,15 +21,24 @@ namespace {
 class GametrakMode : public ModeInterface
 {
 public:
+    void start() override;
     void update() override;
 
     const char *displayName() const override { return "Gametrak"; }
+
+private:
+    bool m_flag;
 };
 
 namespace modes {
 GametrakMode gametrakMode;
 }
 
+void GametrakMode::start()
+{
+    m_flag = false;
+}
+
 void GametrakMode::update()
 {
     if (gas > 500. || brems > 500.)
@@ -31,11 +49,28 @@ void GametrakMode::update()
         return;
     }
 
+    int16_t pwm;
+    if (gametrakDist < 150)
+    {
+        pwm = 0;
+        m_flag = false;
+    }
+    else
+    {
+        if (m_flag || gametrakDist >= 400)
+        {
+            m_flag = true;
+            pwm = clamp<int>((gametrakDist - 400) / 2, -200, 200);
+        }
+        else
+            pwm = 0;
+    }
+
     for (MotorState &motor : motors())
     {
         motor.ctrlTyp = ControlType::FieldOrientedControl;
         motor.ctrlMod = ControlMode::Speed;
-        motor.pwm = gametrakDist > 200 ? 20 : 0;
+        motor.pwm = pwm;
     }
 
     fixCommonParams();