From 6c27ad4805ebdbf886664fce6b224aa2e2c2a8fb Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Mon, 31 Oct 2022 14:40:28 +0100
Subject: [PATCH] :sparkles: Quick fix for imports

---
 rally2.py | 81 ++++++++++++++++++++++++++++++++++++++++++++++++-------
 1 file changed, 71 insertions(+), 10 deletions(-)

diff --git a/rally2.py b/rally2.py
index fdaaa05..46547bc 100644
--- a/rally2.py
+++ b/rally2.py
@@ -105,6 +105,7 @@ def turn_towards_landmark(noah, particles, est_pose, landmark):
         time.sleep((abs(360 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
         noah.stop()
 
+    particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
     # The estimate of the robots current pose
     est_pose = particle.estimate_pose(particles)
     return particles, est_pose
@@ -120,7 +121,7 @@ def time_to_landmark(est_pose, landmark):
     return (DRIVE_T * drive_distance)/1000
 
 
-def drive_until_stopped(noah):
+def drive_until_stopped(noah, particles):
     noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
     start = time.time()
     while True:
@@ -136,10 +137,24 @@ def drive_until_stopped(noah):
         if right_dist < 400:
             noah.stop()
             break
+        if time.time() - start > 5:
+            noah.stop()
+            break
         time.sleep(0.01)
 
     end = time.time()
-    return end - start
+    time_driven = end - start
+    distance_driven = (time_driven*1000)/DRIVE_T
+    for p in particles:
+        x = np.cos(p.theta) * distance_driven
+        y = np.sin(p.theta) * distance_driven
+        p.x = p.x + x
+        p.y = p.y + y
+
+    particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
+    est_pose = particle.estimate_pose(particles)
+
+    return time_driven, est_pose, particles
 
 
 def drunk_drive(noah):
@@ -164,7 +179,8 @@ def drunk_drive(noah):
         time.sleep(0.01)
 
 
-def inch_closer(noah):
+def inch_closer(noah, particles):
+    start = time.time()
     noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
     while True:
         forward_dist = noah.read_front_ping_sensor()
@@ -172,6 +188,19 @@ def inch_closer(noah):
             noah.stop()
             break
 
+    end = time.time()
+    time_driven = end - start
+    distance_driven = (time_driven*1000)/DRIVE_T
+    for p in particles:
+        x = np.cos(p.theta) * distance_driven
+        y = np.sin(p.theta) * distance_driven
+        p.x = p.x + x
+        p.y = p.y + y
+
+    particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
+    est_pose = particle.estimate_pose(particles)
+    return est_pose, particles
+
 
 def initialize_particles(num_particles):
     particles = []
@@ -217,6 +246,36 @@ def calc_weight(particle, landmark_values):
 
     particle.setWeight(np.product(weights))
 
+def turn_90_degrees(noah, est_pose, particles):
+    x_values = [i[0] for i in LANDMARK_POSITIONS.values()]
+    y_values = [i[1] for i in LANDMARK_POSITIONS.values()]
+    middle = np.array([max(x_values)/2, max(y_values)/2])
+
+    current_position = np.array([est_pose.x, est_pose.y])
+    relative_pos = middle - current_position
+    angle = np.arctan(relative_pos[1]/relative_pos[0])
+    relative_angle = np.mod(angle - p.theta(), 2.0*np.pi)
+
+    clockwise = relative_angle > 180
+
+    if clockwise:
+        noah.go_diff(POWER, POWER, 1, 0)
+        time.sleep((90 * TURN_T * CLOCKWISE_OFFSET)/1000)
+    else:
+        noah.go_diff(POWER, POWER, 0, 1)
+        time.sleep((90 * TURN_T)/1000)
+
+    noah.stop()
+
+    for p in particles:
+        if clockwise:
+            p.setTheta(p.theta - np.deg2rad(90))
+        else:
+            p.setTheta(p.theta - np.deg2rad(90))
+    particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
+    _, est_pose, particles = drive_until_stopped(noah, particles)
+    return est_pose, particles
+
 
 def main():
     landmark_order = LANDMARKS + [
@@ -225,23 +284,25 @@ def main():
 
     noah = robot.Robot()  # Noah er vores robots navn
     cam = camera.Camera(0, 'arlo', useCaptureThread=True)
+    particles = initialize_particles(NUM_PARTICLES)
+
+    # The estimate of the robots current pose
+    est_pose = particle.estimate_pose(particles)
 
     for landmark in landmark_order:
         print(f"Going to landmark {landmark}")
         while True:
-            particles = initialize_particles(NUM_PARTICLES)
-
-            # The estimate of the robots current pose
-            est_pose = particle.estimate_pose(particles)
             particles, est_pose = look_around(noah, particles, cam, est_pose)
             print(est_pose)
             particles, est_pose = turn_towards_landmark(noah, particles, est_pose, landmark)
             drive_time = time_to_landmark(est_pose, landmark)
-            if not abs(drive_until_stopped(noah) - drive_time) < 0.5:
-                drunk_drive(noah)
+            time_driven, est_pose, particles = drive_until_stopped(noah)
+            if not abs(time_driven - drive_time) < 0.5:
+                # drunk_drive(noah)
+                est_pose, particles = turn_90_degrees(noah, est_pose, particles)
                 continue
 
-            inch_closer(noah)
+            est_pose, particles = inch_closer(noah)
             break