From d3478fb272ab6f503445bb0ef51a0f7c59118df7 Mon Sep 17 00:00:00 2001
From: PatribotsProgramming <patribotsprogramming4738@gmail.com>
Date: Fri, 25 Oct 2024 16:47:19 -0700
Subject: [PATCH] priority tag,

---
 src/main/java/frc/robot/Robot.java                       | 5 +++++
 src/main/java/frc/robot/RobotContainer.java              | 6 ++++++
 src/main/java/frc/robot/subsystems/vision/Limelight.java | 4 ++++
 src/main/java/frc/robot/util/Constants.java              | 2 ++
 4 files changed, 17 insertions(+)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 80ae86fb..c4a632a7 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.util.Constants.AutoConstants;
 import frc.robot.util.Constants.DriveConstants;
+import frc.robot.util.Constants.FieldConstants;
 import frc.robot.util.Constants.NeoMotorConstants;
 import frc.robot.util.Constants.LoggingConstants;
 import frc.robot.util.rev.Neo;
@@ -125,7 +126,11 @@ public void disabledPeriodic() {
         // Now while this may not necessarily be a constant...
         // it needs to be updated.
         DriverStation.refreshData();
+        if (!Robot.alliance.equals(Optional.of(DriverStation.getAlliance()))) {
+            robotContainer.set3GPriorityTagID(FieldConstants.SPEAKER_MID_TAGS[Robot.isRedAlliance() ? 0 : 1]);
+        }
         Robot.alliance = DriverStation.getAlliance();
+
     }
 
     @Override
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ae41b1d6..6cf09609 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -248,6 +248,8 @@ public RobotContainer() {
 
         pdh.setSwitchableChannel(false);
 
+        set3GPriorityTagID(FieldConstants.SPEAKER_MID_TAGS[Robot.isRedAlliance() ? 0 : 1]);
+
     }
     
     private void configureButtonBindings() {
@@ -908,6 +910,10 @@ private Command enableVision() {
         return Commands.runOnce(() -> enableVision = true).ignoringDisable(true);
     }
 
+    public void set3GPriorityTagID(int id) {
+        limelight3g.setPriorityTagID(id);
+    }
+
     private void registerPathToPathCommands() {
         for (int i = 1; i <= FieldConstants.CENTER_NOTE_COUNT; i++) {
             for (int j = 1; j <= FieldConstants.CENTER_NOTE_COUNT; j++) {
diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java
index 8bed4b93..1362f80d 100644
--- a/src/main/java/frc/robot/subsystems/vision/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java
@@ -372,6 +372,10 @@ public void disableLEDS() {
         LimelightHelpers.setLEDMode_ForceOff(limelightName);
     }
 
+    public void setPriorityTagID(int id) {
+        LimelightHelpers.setPriorityTagID(limelightName, id);
+    }
+
     @Override
     public void updateInputs(LimelightIOInputs inputs) {
         Results results = getResults();
diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
index 2cf2fa06..957ee675 100644
--- a/src/main/java/frc/robot/util/Constants.java
+++ b/src/main/java/frc/robot/util/Constants.java
@@ -800,6 +800,8 @@ public static final class FieldConstants {
 
         public static final double PASS_HEIGHT_METERS = Units.feetToMeters(9.5);
 
+        public static final int[] SPEAKER_MID_TAGS = {4, 7};
+
         // Field:
         // https://cad.onshape.com/documents/dcbe49ce579f6342435bc298/w/b93673f5b2ec9c9bdcfec487/e/6ecb2d6b7590f4d1c820d5e3
         // Chain Positions: Blue alliance left