Merge pull request #43 from bobbycar-graz/gametrak

Gametrak self driving

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

src/modes/gametrakmode.h

@@ -1,8 +1,18 @@
 #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"
+#include "defaultmode.h"
 
 #include "bobbycar-protocol/protocol.h"
 
@@ -11,22 +21,56 @@ 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.)
+    {
+        modes::defaultMode.waitForGasLoslass = true;
+        modes::defaultMode.waitForBremsLoslass = true;
+        currentMode = &modes::defaultMode;
+        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 = 0;
+        motor.pwm = pwm;
     }
 
     fixCommonParams();