diff --git a/find_aruco.py b/find_aruco.py
index 6b27bb2..dbe0e06 100644
--- a/find_aruco.py
+++ b/find_aruco.py
@@ -36,10 +36,10 @@ def find_aruco(arlo):
 
 def main():
     arlo = robot.Robot()
-    for _ in range(5):
-        find_aruco(arlo)
+    for i in range(5):
+        image = find_aruco(arlo)
         sleep(1)
-    # cv2.imwrite("test.png", image)
+        cv2.imwrite(f"test_{i}.png", image)
 
 if __name__ == "__main__":
     main()
diff --git a/robot/arlo.py b/robot/arlo.py
index 918e936..80d3ba1 100644
--- a/robot/arlo.py
+++ b/robot/arlo.py
@@ -8,6 +8,8 @@ START_VALUES = [60, 60]
 THRESHOLD = 1.05
 SLEEP_TIME = 2
 
+FOCAL_LENGTH = 0
+
 def test_forward(arlo, l_power, r_power):
     arlo.reset_encoder_counts()
     arlo.go_diff(l_power, r_power, 1, 1)
@@ -81,9 +83,10 @@ class Arlo():
         sleep((angle * self.timing[3])/1000)
         self.robot.stop()
 
-    def find_aruco(self):
-        image = self.robot.take_photo()
+    def estimate_distance(self, bounding_box):
+        average_left = bounding_box[0][0]
 
+    def find_aruco(self, image):
         aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
         aruco_params = cv2.aruco.DetectorParameters_create()
         corners, ids, _ = cv2.aruco.detectMarkers(
@@ -92,8 +95,13 @@ class Arlo():
             parameters=aruco_params
         )
 
-        for bounding, n in zip(corners, ids):
-            bounding_box = [(int(x), int(y)) for x, y in bounding.reshape((4,2))]
+        return zip(corners, ids)
+
+    def draw_arucos(self, image, bounding_boxes):
+        for bounding, n in bounding_boxes:
+            bounding_box = [
+                (int(x), int(y)) for x, y in bounding.reshape((4,2))
+            ]
             print(bounding_box)
 
             cv2.line(image, bounding_box[0], bounding_box[1], (0,255,0), 2)