🍆

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