From 0bad669ba52f3f6831ab0ab18197569c6370c880 Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Mon, 10 Oct 2022 14:10:22 +0200
Subject: [PATCH] :sparkles:

---
 selflocalization/selflocalize.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py
index 7780709..56e1a20 100644
--- a/selflocalization/selflocalize.py
+++ b/selflocalization/selflocalize.py
@@ -162,7 +162,9 @@ def drive_towards_middle(est_pose, arlo):
     theta = est_pose.theta
 
     relative_pos = middle - position
+    print("relative_pos", relative_pos)
     angle = np.arctan(relative_pos[1]/relative_pos[0])
+    print(angle)
     turn_angle = np.mod(np.rad2deg(angle - theta), 360)
     drive_distance = np.sqrt(position[0]**2 + position[1]**2)
     if turn_angle < 180:
@@ -175,7 +177,7 @@ def drive_towards_middle(est_pose, arlo):
         arlo.stop()
 
     arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
-    sleep(0.5)
+    sleep((drive_distance * DRIVE_T)/1000)
     arlo.stop()
 
 # Main program #
@@ -263,6 +265,7 @@ try:
             # Show world
             cv2.imshow(WIN_World, world)
 
+    print(est_pose.x, est_pose.y, est_pose.theta)
     drive_towards_middle(est_pose, arlo)