diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py
index e717be1..a0da519 100644
--- a/selflocalization/selflocalize.py
+++ b/selflocalization/selflocalize.py
@@ -160,12 +160,13 @@ def drive_towards_middle(est_pose, arlo):
 
     position = np.array([est_pose.x, est_pose.y])
     theta = est_pose.theta
+    print(np.rad2deg(theta))
 
     relative_pos = middle - position
     print("relative_pos", relative_pos)
-    angle = np.arctan(relative_pos[1]/relative_pos[0])
+    angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0]))
     print(angle)
-    turn_angle = np.mod(np.rad2deg(angle - theta) - 180, 360)
+    turn_angle = np.mod(angle - np.rad2deg(theta) - 180, 360)
     print(turn_angle)
     drive_distance = np.sqrt(position[0]**2 + position[1]**2)
     if turn_angle < 180: