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