diff --git a/Code/OpenMV Code/Test_code/April_tag.py b/Code/OpenMV Code/Test_code/April_tag.py
index df9f5eb903f49736aeefb17dd9f9a83251be74a5..54ac4fb81cc1c061968fbd1e152c6eb49bf2892b 100644
--- a/Code/OpenMV Code/Test_code/April_tag.py	
+++ b/Code/OpenMV Code/Test_code/April_tag.py	
@@ -1,6 +1,3 @@
-# Untitled - By: polar - Wed Oct 6 2021
-
-import sensor, image, time
 
 import image, network, math, rpc, sensor, struct, tf, ucollections, pyb
 #from pyb import UART,
@@ -29,56 +26,92 @@ TAG_SIZE = 138 #length of inner black border of tag in mm (if printing to fill n
 MAX_TAGS = 2
 XRES = 320
 YRES = 240
-SIZE = 16.3
+SIZE = 15.8 # unit: cm 
 f_x = (2.8 / 3.673) * XRES # find_apriltags defaults to this if not set 3.984
 f_y = (2.8 / 2.738) * YRES # find_apriltags defaults to this if not set
 c_x = XRES * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
 c_y = YRES * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
 
-def apriltags():
-    red_led.off()
-    green_led.on()
+#################### constants and functions for AprilTags ####################
+
+
+def change_tag_size(size):
+    TAG_SIZE = size
+    #return size
+
+def degrees(radians):
+    return (180 * radians) / math.pi
+
+def dist_conversion(z):
+    #returns the distance in mm
+    z = z*100*2
+    scale = TAG_SIZE/138
+    res_scale = (240/X_RES + 240/Y_RES)/2
+    return z*scale*res_scale
+
+def xy_to_mm(tag1_cx, tag1_cy, tag2_cx, tag2_cy, tag3_cx, tag3_cy):
+    #find the actual distance between tag1 and tag3
+    mm_distx = 590 #x dist in mm between two tags (premeasured)
+    mm_disty = 1 #y dist ^ (lol can't be 0 or the eqn breaks)
+    pixel_distx = tag1_cx - tag2_cx #dist between two tags cx (in pixels)
+    pixel_disty = tag1_cy - tag2_cy #same but cy
+    target_distx = tag1_cx - tag3_cx #dist between tag1 cx and object cx
+    target_disty = tag1_cy - tag3_cy #same but cy
+    #unless that shit is constant or passed in, yikes
+    #the 2.8 is a scaling factor based on lens size of cam
+    distx = target_distx*mm_distx/pixel_distx/2.8
+    disty = target_disty*mm_disty/pixel_disty/2.8
+    return (distx, disty) #in mm
+
+#returns a list of the x,y centroid, distance, and rotation for 2 tags of a certain ID
+
+
+def ATDistance(val,size):   
+    #val = value from apriltag reading
+    #size = apriltag square length in cm
+    
+    new_val = -10*val*size/16.3
+    return new_val
+
+def goalfinder(data):
     sensor.set_pixformat(sensor.RGB565)
-    sensor.set_framesize(sensor.QQVGA)
+    sensor.set_framesize(sensor.QVGA) # we run out of memory if the resolution is much bigger...
     sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
     sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
-    #need to hardcode tag size for each ID
-    LENS_MM = 2.8
-    LENS_TO_CAM_MM = 22
-    
-    # datas = struct.unpack("<bb", data)
-    # id1 = datas[0]
-    id1 = 1
-    #add more options based on what tag size will actually be
-    #this is default size for now
-    tagsize1 = 157
-    if id1 == 0:
-        tagsize1 = 138
-    #sensor.set_pixformat(sensor.GRAYSCALE)
-    #sensor.set_framesize(sensor.QVGA) # we run out of memory if the resolution is much bigger...
-    #sensor.set_windowing((X_RES, Y_RES)) # Look at center 160x120 pixels of the VGA resolution.
+    # goal_id = struct.unpack("<bbb", data)
+    goal_id = data
+    nearest_tag = [0,0,0,float('inf'),0,0,0]
     img = sensor.snapshot()
-
-    f_x = (LENS_MM / 3.984) * X_RES # find_apriltags defaults to this if not set
-    f_y = (LENS_MM / 2.952) * Y_RES # find_apriltags defaults to this if not set
-    c_x = X_RES * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
-    c_y = Y_RES * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
-
-    tags = [0, 0, 0]
-    #n_tags = 0
+    img.draw_cross(160, 120, color = (255, 0, 0))
     for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11
         img.draw_rectangle(tag.rect(), color = (255, 0, 0))
         img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
-        dist = dist_conversion(tag.z_translation())
-        img.draw_string(tag.cx(), tag.cy(), str(dist))
-        if tag.id() == id1:
-            print(tag.id())
-            TAG_SIZE = tagsize1
-            tags[0:2] = [int(tag.cx()), int(tag.cy()), int(degrees(tag.z_rotation()))]
-    red_led.on()
-    green_led.off()
-    return struct.pack("<hhh", tags[0], tags[1], tags[2])
-    
-if __name__ == '__main__':
-    while True:
-        apriltags()
\ No newline at end of file
+        
+        for i in goal_id:
+            if tag.id() == i and ATDistance(tag.z_translation(),SIZE) < nearest_tag[3]:
+                nearest_tag[0] = tag.id()
+                #nearest_tag[1] = tag.x_translation()
+                #nearest_tag[2] = tag.y_translation()
+                nearest_tag[1] = tag.cx()
+                nearest_tag[2] = tag.cy()
+                nearest_tag[3] = ATDistance(tag.z_translation(),SIZE) 
+                nearest_tag[4] = tag.x_rotation()
+                nearest_tag[5] = tag.y_rotation()
+                nearest_tag[6] = tag.z_rotation()
+    #check to see if there's a tag in frame
+    if nearest_tag[3] == float('inf'):
+        # return struct.pack("<hhhhhhh", nearest_tag)
+        print(nearest_tag)
+    else:
+        print("nearest tag id is ", nearest_tag[0], " with the distance", nearest_tag[3], " cm.")
+        print("position x,y:",tag.x_translation(),tag.y_translation())
+        print("position x,y:",tag.cx(), tag.cy())
+        # return struct.pack("<hhhhhhh", nearest_tag)
+
+
+# test
+
+while True:
+    data = [0,1,2] 
+
+    goalfinder(data)
diff --git a/Code/OpenMV Code/Test_code/April_tag_with_CtlBd.py b/Code/OpenMV Code/Test_code/April_tag_with_CtlBd.py
new file mode 100644
index 0000000000000000000000000000000000000000..4af523e6b748821d2f7b97ecff9eebcc59d29122
--- /dev/null
+++ b/Code/OpenMV Code/Test_code/April_tag_with_CtlBd.py	
@@ -0,0 +1,122 @@
+
+import image, network, math, rpc, sensor, struct, tf, ucollections, pyb
+#from pyb import UART,
+import ubinascii
+
+sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+sensor.set_framesize(sensor.QVGA)
+sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
+sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
+sensor.skip_frames(time = 2000)
+
+red_led = pyb.LED(1)
+green_led = pyb.LED(2)
+blue_led = pyb.LED(3)
+red_led.off()
+green_led.off()
+blue_led.off() #put the LEDs into a known state
+red_led.on() #turn the LED to red so that we know it is on
+
+interface = rpc.rpc_i2c_slave(slave_addr=0x12)
+
+#Constants needed
+MAX_BLOBS = 4
+TAG_SIZE = 138 #length of inner black border of tag in mm (if printing to fill normal paper, = 138mm)
+MAX_TAGS = 2
+XRES = 320
+YRES = 240
+SIZE = 15.8 # unit: cm 
+f_x = (2.8 / 3.673) * XRES # find_apriltags defaults to this if not set 3.984
+f_y = (2.8 / 2.738) * YRES # find_apriltags defaults to this if not set
+c_x = XRES * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
+c_y = YRES * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
+
+#################### constants and functions for AprilTags ####################
+
+
+def change_tag_size(size):
+    TAG_SIZE = size
+    #return size
+
+def degrees(radians):
+    return (180 * radians) / math.pi
+
+def dist_conversion(z):
+    #returns the distance in mm
+    z = z*100*2
+    scale = TAG_SIZE/138
+    res_scale = (240/X_RES + 240/Y_RES)/2
+    return z*scale*res_scale
+
+def xy_to_mm(tag1_cx, tag1_cy, tag2_cx, tag2_cy, tag3_cx, tag3_cy):
+    #find the actual distance between tag1 and tag3
+    mm_distx = 590 #x dist in mm between two tags (premeasured)
+    mm_disty = 1 #y dist ^ (lol can't be 0 or the eqn breaks)
+    pixel_distx = tag1_cx - tag2_cx #dist between two tags cx (in pixels)
+    pixel_disty = tag1_cy - tag2_cy #same but cy
+    target_distx = tag1_cx - tag3_cx #dist between tag1 cx and object cx
+    target_disty = tag1_cy - tag3_cy #same but cy
+    #unless that shit is constant or passed in, yikes
+    #the 2.8 is a scaling factor based on lens size of cam
+    distx = target_distx*mm_distx/pixel_distx/2.8
+    disty = target_disty*mm_disty/pixel_disty/2.8
+    return (distx, disty) #in mm
+
+#returns a list of the x,y centroid, distance, and rotation for 2 tags of a certain ID
+
+
+def ATDistance(val,size):   
+    #val = value from apriltag reading
+    #size = apriltag square length in cm
+    
+    new_val = -10*val*size/16.3
+    return new_val
+
+def goalfinder(data):
+    sensor.set_pixformat(sensor.RGB565)
+    sensor.set_framesize(sensor.QVGA) # we run out of memory if the resolution is much bigger...
+    sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
+    sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
+    # goal_id = struct.unpack("<bbb", data)
+    goal_id = data
+    nearest_tag = [0,0,0,float('inf'),0,0,0]
+    img = sensor.snapshot()
+    img.draw_cross(160, 120, color = (255, 0, 0))
+    for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11
+        img.draw_rectangle(tag.rect(), color = (255, 0, 0))
+        img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
+        
+        for i in goal_id:
+            if tag.id() == i and ATDistance(tag.z_translation(),SIZE) < nearest_tag[3]:
+                nearest_tag[0] = tag.id()
+                #nearest_tag[1] = tag.x_translation()
+                #nearest_tag[2] = tag.y_translation()
+                nearest_tag[1] = tag.cx()
+                nearest_tag[2] = tag.cy()
+                nearest_tag[3] = ATDistance(tag.z_translation(),SIZE) 
+                nearest_tag[4] = tag.x_rotation()
+                nearest_tag[5] = tag.y_rotation()
+                nearest_tag[6] = tag.z_rotation()
+    #check to see if there's a tag in frame
+    return struct.pack("<hhhhhhh", nearest_tag)
+    """
+    if nearest_tag[3] == float('inf'):
+        print(nearest_tag)
+    else:
+        print("nearest tag id is ", nearest_tag[0], " with the distance", nearest_tag[3], " cm.")
+        print("position x,y:",tag.x_translation(),tag.y_translation())
+        print("position x,y:",tag.cx(), tag.cy())
+    """
+        
+
+
+# Register call backs.
+
+interface.register_callback(goalfinder)
+
+# Once all call backs have been registered we can start
+# processing remote events. interface.loop() does not return.
+
+interface.loop()
+