From 4a4de3789a7489a62c5dd8ad6a4838974211796b Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Thu, 20 Jun 2019 16:57:24 +0200
Subject: [PATCH] limit backward driving with gametrak

---
 controller/controller.ino | 23 +++++++++++++++++++----
 1 file changed, 19 insertions(+), 4 deletions(-)

diff --git a/controller/controller.ino b/controller/controller.ino
index 679390c..a271198 100644
--- a/controller/controller.ino
+++ b/controller/controller.ino
@@ -62,9 +62,12 @@ 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_speedbackward_p=0.7;
 float gt_steer_p=2.0;
-#define GT_SPEED_LIMIT 300 //maximum out_speed value +-
+#define GT_SPEED_LIMIT 300 //maximum out_speed value +
+#define GT_SPEEDBACKWARD_LIMIT 100//maximum out_speed value (for backward driving) -
 #define GT_STEER_LIMIT 300 //maximum out_steer value +-
+#define GT_LENGTH_MAXIMUMDIFFBACKWARD -200 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800
 
 
 #include <IMUGY85.h>
@@ -393,12 +396,24 @@ void loop() {
       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){
+    if (_gt_length_diff>-GT_LENGTH_MINDIFF & _gt_length_diff<GT_LENGTH_MINDIFF){ //minimum difference to drive
       _gt_length_diff=0; //threshold
     }
-    
-    set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),-GT_SPEED_LIMIT,GT_SPEED_LIMIT);
+
     set_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 (_gt_length_diff>0) { //needs to drive forward
+      set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,GT_SPEED_LIMIT);
+    }else{ //drive backward
+      if (_gt_length_diff > GT_LENGTH_MAXIMUMDIFFBACKWARD){ //only drive if not pulled back too much
+        set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-GT_SPEEDBACKWARD_LIMIT,0);
+      }else{
+        set_speed = 0; //stop
+        set_steer = 0;
+      }
+    }
+    
+    
 
     //calculate speed l and r from speed and steer
     #define SPEED_COEFFICIENT_GT   1  // higher value == stronger