From ad57381bc0d193b741e0ed36f17d4cfd713d792f Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Wed, 26 Oct 2022 10:47:04 +0200
Subject: [PATCH] :eggplant:

---
 rally2.py | 31 ++++++++++++++++++++++++-------
 1 file changed, 24 insertions(+), 7 deletions(-)

diff --git a/rally2.py b/rally2.py
index 70fa554..930927b 100644
--- a/rally2.py
+++ b/rally2.py
@@ -40,28 +40,45 @@ def time_to_landmark(est_pose, landmark):
     """Kør indenfor 1 meter"""
     pass
 
-def drive_until_stopped(est_pose):
+def drive_until_stopped(noah):
+    noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
+    start = time.time()
+    while True:
+        forward_dist = noah.read_front_sensor()
+        if forward_dist < 1000:
+            noah.stop()
+            break
+
+    end = time.time()
+    return end - start
+
+def drunk_drive(noah):
     pass
 
-def inch_closer():
-    pass
+def inch_closer(noah):
+    noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
+    while True:
+        forward_dist = noah.read_front_sensor()
+        if forward_dist < 300:
+            noah.stop()
+            break
 
 def main():
     landmark_order = LANDMARKS + [
         LANDMARKS[0]
     ]
 
-    noah = robot.Robot()
+    noah = robot.Robot() # Noah er vores robots navn
 
     for landmark in landmark_order:
         while True:
             est_pose = look_around()
             est_pose = turn_towards_landmark(est_pose, landmark)
             drive_time = time_to_landmark(est_pose, landmark)
-            if not drive_until_stopped(drive_time) == drive_time:
-                # drive around
+            if not abs(drive_until_stopped(noah) - drive_time) < 0.5:
+                drunk_drive(noah)
                 continue
-            inch_closer()
+            inch_closer(noah)
             break