Zone check? I dunno, tired :3

src/main/java/frc/robot/Constants.java

@@ -1,36 +1,39 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
+import com.ctre.phoenix6.signals.InvertedValue;
+
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.units.Units;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.units.measure.Angle;
 
-import com.ctre.phoenix6.signals.InvertedValue;
-
 /**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * The Constants class provides a convenient place for teams to hold robot-wide
- * constants. This class should not be used for any other purpose. All constants should be declared
+ * numerical or boolean constants. This class should not be used for any other
- * globally (i.e. public static). Do not put anything functional in this class.
+ * purpose. All constants should be declared globally (i.e. public static). Do
+ * not put anything functional in this class.
  *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * <p>
- * constants are needed, to reduce verbosity.
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the constants are needed, to reduce verbosity.
  */
 public final class Constants {
+
     public static class OperatorConstants {
+
         public static final int kDriverControllerPort = 0;
     }
 
     public static class CameraConstanst {
 
         public static class IDFilters {
+
             public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11};
             public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
         }
@@ -42,10 +45,13 @@ public final class Constants {
 
         public static final String FRONT_CAMERA_NAME = "limelight-front";
         public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
-  
+        public static final double[] BLUE_ZONE_COOR = new double[]{1};
+        public static final double[] RED_ZONE_COOR = new double[]{1};
+        public static final double[] NEUTRAL_ZONE_COOR = new double[]{1};
     }
 
     public static class AdvantageKitConstants {
+
         public enum RobotType {
             SIM,
             REAL,
@@ -54,12 +60,15 @@ public final class Constants {
         public static final RobotType CURRENT_MODE = RobotType.REAL;
     }
     // Swerve offsets as of 3/2/26
+
     public static class SwerveDriveOffsets {
+
         public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
         public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
         public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
         public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
     }
+
     /*
    * Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
    * Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
@@ -69,6 +78,7 @@ public final class Constants {
    * Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
      */
     public static class SwerveDriveIDs {
+
         public static final int FRONT_LEFT_STEER_ID = 11;
         public static final int FRONT_LEFT_DRIVE_ID = 12;
         public static final int FRONT_LEFT_ENCODER_ID = 13;
@@ -85,6 +95,7 @@ public final class Constants {
 
     public static class RobotChassisLimits {
         // Max velocity in meters per second
+
         public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
         // In meters per second squared
         public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
@@ -97,10 +108,12 @@ public final class Constants {
 
     //Bird
     public static class ExtraIDEntities {
+
         public static final int PIGEON_ID = 1;
     }
 
     public static class IntakeConstants {
+
         public static final int DEPLOY_MOTOR_ID = 17;
         public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
 
@@ -109,6 +122,7 @@ public final class Constants {
     }
 
     public static class ShooterConstants {
+
         public static final int LAUNCHER_MOTOR_ID = 3;
         public static final boolean LAUNCHER_MOTOR_INVERTED = false;
 
@@ -134,6 +148,7 @@ public final class Constants {
     }
 
     public static class HopperConstants {
+
         public static final int KICKER_MOTOR_ID = 9;
         public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
 
@@ -142,6 +157,7 @@ public final class Constants {
     }
 
     public static class FieldPoses {
+
         private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
 
         public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));

src/main/java/frc/robot/RobotContainer.java

@@ -14,6 +14,7 @@ import frc.robot.commands.AutoShootCommandChassisTurret;
 import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
 import frc.robot.commands.RelayCommand;
 import frc.robot.commands.SetDeflectorCommand;
+import frc.robot.commands.ZoneCheck;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -83,6 +84,7 @@ public class RobotContainer {
         m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
         m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
         m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+        m_driverController.b().toggleOnTrue(new ZoneCheck());
     }
 
     /**

src/main/java/frc/robot/commands/AnyTarget.java

@@ -21,7 +21,7 @@ import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 
-public class ZoneSwap extends Command {
+public class AnyTarget extends Command {
 
     // For Chassis Control
     private DriveSubsystem driveSubsystem;
@@ -40,7 +40,7 @@ public class ZoneSwap extends Command {
 
     private final double chassisRotateSpeed = Math.PI * 0.1;
 
-    public ZoneSwap(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) {
+    public AnyTarget(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) {
         this.driveSubsystem = driveSubsystem;
         this.hopperSubsystem = hopperSubsystem;
         this.turretSubsystem = turretSubsystem;

src/main/java/frc/robot/commands/ZoneCheck.java

@@ -0,0 +1,79 @@
+package frc.robot.commands;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+
+public class ZoneCheck extends Command {
+
+    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+    private Zone zone;
+
+    public enum Zone {
+        Blue,
+        Red,
+        Neutral;
+    }
+
+    public ZoneCheck() {
+        getBotPose();
+    }
+
+    @Override
+    public void initialize() {
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                switch (zone) {
+                    case Blue:
+                        System.out.println("Im Home");
+                    case Neutral:
+                        System.out.println("Uhhhhhhhh not red");
+                        ;
+                    default:
+                        System.out.println("BOOOOO, BOOOO RED");
+                        ;
+                }
+            } else {
+                switch (zone) {
+                    case Blue:
+                        System.out.println("Screw Blue");
+                        ;
+                    case Neutral:
+                        System.out.println("Uhhhhhhhh not red");
+                        ;
+                    default:
+                        System.out.println("Im Home");
+                }
+            }
+        });
+    }
+
+    @Override
+    public void execute() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    private double[] getBotPose() {
+        return LimelightHelpers.getBotPose_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
+    }
+
+    private void getZone() {
+        if (getBotPose()[0] >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
+            zone = Zone.Red;
+        } else if (getBotPose()[0] >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
+            zone = Zone.Neutral;
+        } else {
+            zone = Zone.Blue;
+        }
+    }
+}

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -135,7 +135,6 @@ public class DriveSubsystem extends TestableSubsystem {
                                 .build())
                 .build();
 
-
         // Set vision measurement confidence values
         driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999));