✨

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)