✨

selflocalization/selflocalize.py

@@ -2,13 +2,13 @@ import cv2
 import particle as particle
 import camera as camera
 import numpy as np
-import time
+from time import sleep
 from timeit import default_timer as timer
 import sys
 
 
 # Flags
-showGUI = False  # Whether or not to open GUI windows
+showGUI = True  # Whether or not to open GUI windows
 onRobot = True # Whether or not we are running on the Arlo robot
 
 
@@ -53,16 +53,16 @@ CBLACK = (0, 0, 0)
 
 # Landmarks.
 # The robot knows the position of 2 landmarks. Their coordinates are in the unit centimeters [cm].
-landmarkIDs = [4, 6]
+landmarkIDs = [1, 5]
 landmarks = {
-    4: (0.0, 0.0),  # Coordinates for landmark 1
-    6: (300.0, 0.0)  # Coordinates for landmark 2
+    1: (0.0, 0.0),  # Coordinates for landmark 1
+    5: (300.0, 0.0)  # Coordinates for landmark 2
 }
 landmark_colors = [CRED, CGREEN] # Colors used when drawing the landmarks
 
 
-SIGMA = 1
-SIGMA_THETA = 0.2
+SIGMA = 1.5
+SIGMA_THETA = 0.3
 
 
 def normal(x, mean, std):
@@ -155,6 +155,28 @@ def calc_weight(particle, landmark_values):
 
     particle.setWeight(np.product(weights))
 
+def drive_to_middle(est_pose, arlo):
+    middle = np.array([150, 0])
+
+    position = np.array([est_pose.x, est_pose.y])
+    theta = est_pose.theta
+
+    relative_pos = middle - position
+    angle = np.arctan(relative_pos[1]/relative_pos[0])
+    turn_angle = np.mod(np.rad2deg(angle - theta), 360)
+    drive_distance = np.sqrt(position[0]**2 + position[1]**2)
+    if turn_angle < 180:
+        arlo.go_diff(POWER, POWER, 0, 1)
+        sleep((abs(turn_angle) * TURN_T)/1000)
+        arlo.stop()
+    else:
+        arlo.go_diff(POWER, POWER, 1, 0)
+        sleep((abs(180 - turn_angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
+        arlo.stop()
+
+    arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+    sleep((drive_distance * DRIVE_T)/1000)
+    arlo.stop()
 
 # Main program #
 try:
@@ -170,7 +192,7 @@ try:
 
 
     # Initialize particles
-    num_particles = 1000
+    num_particles = 2000
     particles = initialize_particles(num_particles)
 
     est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
@@ -200,17 +222,6 @@ try:
         if action == ord('q'): # Quit
             break
 
-        arlo.stop()
-        if action == ord(','): # Forward
-            arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
-        elif action == ord('o'): # Backwards
-            arlo.go_diff(POWER, POWER, 0, 0)
-        elif action == ord('a'): # Left
-            arlo.go_diff(POWER, POWER, 0, 1)
-        elif action == ord('e'): # Right
-            arlo.go_diff(POWER, POWER, 1, 0)
-
-
         # Fetch next frame
         colour = cam.get_next_frame()
 
@@ -241,6 +252,7 @@ try:
 
         particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
         est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
+        drive_to_middle(est_pose, arlo)
 
         if showGUI:
             # Draw map