From 5e17c2cf22e8badd699755ab817a54821bd05fa5 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Wed, 12 Jun 2019 00:30:53 +0200
Subject: [PATCH] implement gametrak function

---
 controller.ino | 48 ++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 40 insertions(+), 8 deletions(-)

diff --git a/controller.ino b/controller.ino
index 72826e0..f286d1a 100644
--- a/controller.ino
+++ b/controller.ino
@@ -59,8 +59,12 @@ int8_t gt_vertical=0; //0=center. joystick can rotate +-30 degrees. -127 = -30 d
 #define GT_HORIZONTAL_RANGE 2047 //adc value difference from center to maximum (30 deg)
 int8_t gt_horizontal=0; //0=center
 
-
-
+uint16_t gt_length_set=1000; //set length to keep [mm]
+#define GT_LENGTH_MINDIFF 10 //[mm] threshold, do not move within gt_length_set-GT_LENGTH_MINDIFF and gt_length_set+GT_LENGTH_MINDIFF
+float gt_speed_p=0.7; //value to multipy difference [mm] with -> out_speed
+float gt_steer_p=1.0;
+#define GT_SPEED_LIMIT 300 //maximum out_speed value +-
+#define GT_STEER_LIMIT 200 //maximum out_steer value +-
 
 
 #include <IMUGY85.h>
@@ -139,7 +143,8 @@ uint8_t out_checksum = 0; //0= disable motors, 255=reserved,  1<=checksum<255
 boolean armed = false;
 boolean lastpacketOK = false;
 
-
+//Gametrak
+boolean armed_gt = false;
 
 void setup() {
 
@@ -309,6 +314,8 @@ void loop() {
     #endif
   }
   if (armed) { //is armed
+    armed_gt = false; //if remote used disable gametrak
+    
     if (lastpacketOK) { //if lastnrfdata is valid
       if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) {
         last_controlupdate = millis();
@@ -349,20 +356,45 @@ void loop() {
     }//if pastpacket not ok, keep last out_steer and speed values until disarmed
 
 #ifdef DEBUG
-    if (!lastpacketOK)
+    if (!lastpacketOK) {
       Serial.println("Armed but packet not ok");
     }
 #endif
-
-  } else { //disarmed
-    out_steer = 0;
-    out_speed = 0;
+  }else{ //not armed, keep values reset
     setYaw = yaw;
     magalign_multiplier = 0;
   }
 
 
 
+  if (!armed_gt) { //check if gametrak will be armed
+    if (gt_length>gt_length_set && gt_length<gt_length_set+10) { //is in trackable length
+      armed_gt=true;
+    }
+  }else if (armed_gt && !armed){ //gametrak control active and not remote active
+    //Gametrak Control Code
+    motorenabled=true;
+    if (gt_length<=GT_LENGTH_MIN){ //let go
+      armed_gt=false;
+      motorenabled=false;
+    }
+    int16_t _gt_length_diff = gt_length-gt_length_set; //positive if needs to drive forward
+    if (_gt_length_diff>-GT_LENGTH_MINDIFF & _gt_length_diff<GT_LENGTH_MINDIFF){
+      _gt_length_diff=0; //threshold
+    }
+    
+    out_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),-GT_SPEED_LIMIT,GT_SPEED_LIMIT);
+    out_steer=constrain((int16_t)(-gt_horizontal*gt_steer_p),-GT_STEER_LIMIT,GT_STEER_LIMIT); //steer positive is left //gt_horizontal left is negative
+  }
+
+
+  if (!armed && !armed_gt){ //all disarmed
+    out_steer = 0;
+    out_speed = 0;
+  }
+
+
+
   
   if (millis() - last_send > SENDPERIOD) {
     //calculate checksum