From 876afa05b7afa7b027ec930e890031511232598a Mon Sep 17 00:00:00 2001
From: Max <Hunt>
Date: Mon, 4 Oct 2021 11:37:21 +0100
Subject: [PATCH] Update

---
 Control/Jetson/main.py | 21 ++++++++++++++++-----
 1 file changed, 16 insertions(+), 5 deletions(-)

diff --git a/Control/Jetson/main.py b/Control/Jetson/main.py
index b0447e4..b758962 100755
--- a/Control/Jetson/main.py
+++ b/Control/Jetson/main.py
@@ -24,15 +24,16 @@ from pallet_hole_alignment_c import HoleAligner
 #     subprocess.run(["route", "-n"])
 
 DO_ZERO = True
-DO_GPS_THERE = True
-DO_GOTO_TARGET_HEADING = True
+DO_GPS_THERE = False
+DO_GOTO_TARGET_HEADING = False
 DO_VISION_APPROACH = True
 DO_FORK_ALIGN = True
 DO_PICKUP_CARGO = True
 DO_REVERSE_TURN = True
-DO_GPS_RETURN = True
+DO_GPS_RETURN = False
+DO_FAKE_FORWARD = True
 DO_DROPOFF_CARGO = True
-DO_RETURN_TO_NEUTRAL = True
+DO_RETURN_TO_NEUTRAL = False
 DO_LOOP = True
 
 
@@ -80,7 +81,7 @@ def __go_to_gps(vehicle_ctrl, lat: float, lon: float):
         executor_logger.info(f'We are in radius ({distance})')
         return True
     else:
-        executor_logger.critical(f"5 minuted have elapsed and we are not in the target area ({distance}), aborting")
+        executor_logger.critical(f"5 minutes have elapsed and we are not in the target area ({distance}), aborting")
         return False
 
 def __go_to_cargo_vision(vision_system):
@@ -114,6 +115,13 @@ def __reverse_turn(vehicle_ctrl):
 
     return True
 
+def __fake_forward(vehicle_ctrl):
+    executor_logger.info(f"moving forward 10s")
+    vehicle_ctrl.nudge_drive(True, 6)
+    vehicle_ctrl.nudge_drive(True, 6)
+
+    return True
+
 #--- <Pseudoprivate methods
 
 
@@ -206,6 +214,9 @@ def queue_executor_thread():
             else:
                 executor_logger.warning("DO_REVERSE_TURN flag is false, skipping reverse turn")
 
+            if DO_FAKE_FORWARD:
+                __fake_forward(FORKLIFT)
+
 
             if DO_GPS_RETURN:
                 FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")