From 09f564086bd377c2b4b84c956c3a160fe3490f6c Mon Sep 17 00:00:00 2001
From: Max <Hunt>
Date: Tue, 21 Sep 2021 15:51:40 +0100
Subject: [PATCH] Update

---
 Control/Jetson/dronekit_server_client.py |  6 ++++--
 Control/Jetson/main.py                   |  2 +-
 Control/Pi/dronekit_class.py             | 20 ++++++++++++++++++--
 Control/Pi/main_server.py                |  5 +++++
 4 files changed, 28 insertions(+), 5 deletions(-)

diff --git a/Control/Jetson/dronekit_server_client.py b/Control/Jetson/dronekit_server_client.py
index 74dfd50..4b02d04 100644
--- a/Control/Jetson/dronekit_server_client.py
+++ b/Control/Jetson/dronekit_server_client.py
@@ -44,6 +44,7 @@ class DroneKitComms:
         self.nudge_fork_horiz_endpoint = '/nudge_fork_horiz'
         self.nudge_drive_endpoint = '/nudge_drive'
         self.approach_pallet_endpoint = '/approach_pallet'
+        self.reverse_from_pallet_endpoint = '/reverse_from_pallet'
         self.gps_target_radius_km = 0.0025
         rsp = self.make_request(self.heartbeat_endpoint)
         if rsp.get('Success', False):
@@ -164,10 +165,11 @@ class DroneKitComms:
         self.nudge_fork_horiz(go_left=False, amount=0.5)
         self.nudge_fork_horiz(go_left=False, amount=0.65)
 
-
     def approach_pallet(self):
         _ = self.make_request(self.approach_pallet_endpoint)
-        # print(rsp)
+
+    def reverse_from_pallet(self):
+        _ = self.make_request(self.reverse_from_pallet_endpoint)
 
 
     # def close_vehicle(self):
diff --git a/Control/Jetson/main.py b/Control/Jetson/main.py
index 51a9166..61b55fd 100755
--- a/Control/Jetson/main.py
+++ b/Control/Jetson/main.py
@@ -107,7 +107,7 @@ def __pick_up_cargo(vehicle_ctrl):
 def __put_down_cargo(vehicle_ctrl):
     vehicle_ctrl.zero_fork_vert_pickup()
     time.sleep(1)
-    vehicle_ctrl.nudge_drive(False, 9.9)
+    vehicle_ctrl.reverse_from_pallet()
     return True
 
 
diff --git a/Control/Pi/dronekit_class.py b/Control/Pi/dronekit_class.py
index 89dfb51..aec53c8 100644
--- a/Control/Pi/dronekit_class.py
+++ b/Control/Pi/dronekit_class.py
@@ -62,6 +62,7 @@ class DroneInterface:
         self.log.info("Connected to ardurover")
         self.pallet_proximity_sensor = PalletProximity()
         self.pallet_min_distance = 22  # TODO: calibrate this
+        self.pallet_max_distance = 55  # TODO: calibrate this
         self.log.info("Connected to pallet proximity sensor")
         self.print_status()
 
@@ -404,10 +405,10 @@ class DroneInterface:
     def approach_pallet(self):
         pallet_approached = False
         while not pallet_approached:
-            time.sleep(0.2)
+            time.sleep(0.1)
             distance_to_pallet = self.pallet_proximity_sensor.get_prox_reading()
             if distance_to_pallet < self.pallet_min_distance:
-                self.log.info(f"We are close enough to pallet ({distance_to_pallet} m)")
+                self.log.info(f"We are close enough to pallet ({distance_to_pallet} cm)")
                 pallet_approached = True
                 self.override_stop()
                 self.clear_override_channels()
@@ -415,3 +416,18 @@ class DroneInterface:
                 self.log.info(f"Distance to pallet ({distance_to_pallet} cm), moving forward...")
                 target_speed_pwm = 1930
                 self.set_vehicle_speed(target_speed_pwm)
+
+    def reverse_from_pallet(self):
+        pallet_reversed = False
+        while not pallet_reversed:
+            time.sleep(0.1)
+            distance_to_pallet = self.pallet_proximity_sensor.get_prox_reading()
+            if distance_to_pallet > self.pallet_max_distance:
+                self.log.info(f"We are far enough from pallet ({distance_to_pallet} cm)")
+                pallet_reversed = True
+                self.override_stop()
+                self.clear_override_channels()
+            else:
+                self.log.info(f"Distance to pallet ({distance_to_pallet} cm), moving back...")
+                target_speed_pwm = 1070
+                self.set_vehicle_speed(target_speed_pwm)
diff --git a/Control/Pi/main_server.py b/Control/Pi/main_server.py
index 3defd8b..2e894c9 100755
--- a/Control/Pi/main_server.py
+++ b/Control/Pi/main_server.py
@@ -314,6 +314,11 @@ def approach_pallet():
     FORKLIFT.approach_pallet()
     return jsonify({'Success': True})
 
+@app.route('/reverse_from_pallet', methods=['POST'])
+def reverse_from_pallet():
+    FORKLIFT.reverse_from_pallet()
+    return jsonify({'Success': True})
+
 # @app.route('/close', methods=['POST'])
 # def close_forklift():
 #     logger.info(f"Closing forklift")