Merge branch 'main' of https://github.com/patribots4738/Crescendo2024

Patribots4738 · Oct 25, 2024 · 067d2c4 · 067d2c4
2 parents 3027438 + e5dd4d1
commit 067d2c4
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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
@@ -738,6 +738,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