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] 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();