diff --git a/drive_between_arucos.py b/drive_between_arucos.py
index af75491..9f51593 100644
--- a/drive_between_arucos.py
+++ b/drive_between_arucos.py
@@ -59,22 +59,25 @@ def main():
         np.array([aruco[0]]), 14.5, CAMERA_MATRIX, DIST_COEF
     )[1][0][0] for aruco in arucos])
 
-    print(aruco_positions)
+    position = [
+        np.average(aruco_positions[:,0]),
+        np.average(aruco_positions[:,2])
+    ]
 
-    # angle = np.rad2deg(np.arctan(position[0]/position[2]))
-    # drive_distance = np.sqrt(position[0]**2 + position[2]**2)
-    # if angle < 0:
-    #     arlo.go_diff(POWER, POWER, 0, 1)
-    #     sleep((abs(angle) * TURN_T)/1000)
-    #     arlo.stop()
-    # else:
-    #     arlo.go_diff(POWER, POWER, 1, 0)
-    #     sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
-    #     arlo.stop()
+    angle = np.rad2deg(np.arctan(position[0]/position[2]))
+    drive_distance = np.sqrt(position[0]**2 + position[2]**2)
+    if angle < 0:
+        arlo.go_diff(POWER, POWER, 0, 1)
+        sleep((abs(angle) * TURN_T)/1000)
+        arlo.stop()
+    else:
+        arlo.go_diff(POWER, POWER, 1, 0)
+        sleep((abs(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()
+    arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+    sleep((drive_distance * DRIVE_T)/1000)
+    arlo.stop()
 
 
 if __name__ == "__main__":