Merge branch 'main' into fresno-comp

Team766 · Mar 26, 2024 · 627e78b · 627e78b
2 parents 6675e2d + f1b869d
commit 627e78b
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java
@@ -37,6 +37,22 @@ public GrayScaleCamera getCamera() {
     }
 
     public void run() {
+        if (tagId == -1) {
+            Optional<Alliance> alliance = DriverStation.getAlliance();
+
+            if (alliance.isPresent()) {
+                if (alliance.get().equals(Alliance.Blue)) {
+                    tagId = 7;
+                } else {
+                    tagId = 4;
+                }
+                Robot.lights.signalCameraConnected();
+            } else {
+                LoggerExceptionUtils.logException(
+                        new AprilTagGeneralCheckedException("Couldn't find alliance correctly"));
+            }
+        }
+
         try {
             if (tagId == -1) {
                 Optional<Alliance> alliance = DriverStation.getAlliance();