adds two functions to find level of nearest algae

utilities/game.py

@@ -17,6 +17,9 @@
 
 APRILTAGS = apriltag_layout.getTags()
 
+L3_TAGS = [7,9,11,18,20,22]
+L2_TAGS = [6,8,10,17,19,21]
+
 FIELD_WIDTH = apriltag_layout.getFieldWidth()
 FIELD_LENGTH = apriltag_layout.getFieldLength()
 
@@ -50,3 +53,22 @@ def field_flip_translation2d(t: Translation2d):
 # This will default to the blue alliance if a proper link to the driver station has not yet been established
 def is_red() -> bool:
     return wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed
+
+def nearest_reef_tag(pose :Pose2d) -> int:
+    distance = math.inf
+    closest_tag_id = 0
+
+    for tag_id in L2_TAGS + L3_TAGS:
+        tag_pose = apriltag_layout.getTagPose(tag_id)
+
+        assert tag_pose
+
+        robot_to_tag = tag_pose.toPose2d() - pose
+        if robot_to_tag.translation().norm() < distance:
+            distance = robot_to_tag.translation().norm()
+            closest_tag_id = tag_id
+
+    return closest_tag_id
+
+def is_L3(tag_id :int) -> bool:
+    return tag_id in L3_TAGS