worked on some vision stuff

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

@@ -156,7 +156,9 @@ public static class FieldConstants {
     public static final double SPEAKER_HEIGHT = 2.13;
 
     public static final double SHOOTER_SLOW_THRESHOLD = 2;
+
     public static final double TAG_DISTANCE_THRESHOLD = 2;
+    public static final double TAG_DISTANCE_RESET_THRESHOLD = 1;
 
     public static final int SPEAKER_TAG_BLUE = 7;
     public static final int SPEAKER_TAG_RED = 4;

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

@@ -86,9 +86,7 @@ public class RobotContainer {
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
-  public RobotContainer() {
-    NamedCommands.registerCommand("brake", new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
-
+  public RobotContainer() {    
     NamedCommands.registerCommand("actuateOut", new SetShooter(_shooterSubsystem, () -> Presets.ACTUATE_SHOOTER_ANGLE).andThen(
       new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE))
     );
@@ -180,7 +178,7 @@ private void configureBindings() {
       )
     );
 
-    _operatorController.R2().whileTrue(
+    _driveController.R2().whileTrue(
       new AutoAim(
         _shooterSubsystem,
         _elevatorSubsystem,

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

@@ -76,7 +76,7 @@ public class SwerveDriveSubsystem extends SubsystemBase {
   private final Orchestra _orchestra = new Orchestra();
   String song = "output.chrp";
 
-  private DrivingSpeeds _drivingState = DrivingSpeeds.SLOW;
+  private DrivingSpeeds _drivingState = DrivingSpeeds.FAST;
 
   private Field2d _field = new Field2d();
 

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

@@ -51,17 +51,42 @@ public double getLatency() {
     return Timer.getFPGATimestamp() - (tl / 1000.0) - (cl / 1000.0);
   }
 
+  /** Return a boolean for whether a tag is seen. */
+  public boolean isApriltagVisible() {
+    double tv = _limelight.getEntry("tv").getDouble(0);
+
+    if (tv == 0) {
+      return false;
+    }
+
+    if (tv == 1) {
+      return true;
+    }
+
+    return false;
+  }
+
   /**
-   * Returns the "wpiblue" botpose of the robot from the limelight.
+   * Return a boolean for whether the desired tag is seen.
    *
-   * @return An Optional of Pose2d which is necessary if no valid (at least 2 tags, good distance) data is found from
-   * the limelight.
+   * @param ID
+   *            The id of the desired tag.
+   */
+  public boolean isApriltagVisible(int ID) {
+    if (!isApriltagVisible())
+      return false;
+
+    return _limelight.getTag(ID) != null;
+  }
+
+  /**
+   * Returns the botpose as an array from the limelight if it is valid (at least 2 tags, good distance). 
+   * 
+   * @return NT "botpose_wpiblue" limelight topic.
    * 
    * @see Optional
    */
-  public Optional<Pose2d> getBotpose() {
-    // return Optional.empty();
-
+  public Optional<double[]> getValidBotpose() {
     if (!isApriltagVisible()) return Optional.empty();
 
     NetworkTableEntry botpose_entry = _limelight.getEntry("botpose_wpiblue");
@@ -76,61 +101,42 @@ public Optional<Pose2d> getBotpose() {
     // if (tags < 2) return Optional.empty();
     if (distance > FieldConstants.TAG_DISTANCE_THRESHOLD) return Optional.empty();
 
-    double botposeX = _xFilter.calculate(botpose_array[0]); // to get rid of the weird origin outlier
-    double botposeY = _yFilter.calculate(botpose_array[1]); // to get rid of the weird origin outlier
-    double botposeYaw = _yawFilter.calculate(botpose_array[5]); // to get rid of weird 0 heading outlier
-    Rotation2d botposeRotation = Rotation2d.fromDegrees(botposeYaw);
-
-    Pose2d botpose2D = new Pose2d(botposeX, botposeY, botposeRotation);
-
-    return Optional.of(botpose2D);
+    return Optional.of(botpose_array);
   }
 
   /**
-   * Tells whether limelight data is valid or not. For the data to be valid, the limelight 
-   * must be looking at at least one apriltag, and the apriltag must be in the specific distance range.
+   * If the limelight is in perfect condition with the apriltags to reset the robot's pose.
    */
-  public boolean isValid() {
-    if (!isApriltagVisible()) return false;
-
-    JsonNode tags = _limelight.getTags();
-
-    for (JsonNode tag : tags) {
-      double distance = ((ArrayNode) tag.get("t6t_cs")).get(2).asDouble();
-      if (distance <= FieldConstants.TAG_DISTANCE_THRESHOLD) {
-        return true;
-      }
-    }
-
-    return false;
-  }
-
-  /** Return a boolean for whether a tag is seen. */
-  public boolean isApriltagVisible() {
-    double tv = _limelight.getEntry("tv").getDouble(0);
-
-    if (tv == 0) {
-      return false;
-    }
-
-    if (tv == 1) {
-      return true;
-    }
+  public boolean canResetPose() {
+    Optional<double[]> botpose = getValidBotpose();
+    if (botpose.isEmpty()) return false;
+    if (botpose.get()[9] <= FieldConstants.TAG_DISTANCE_RESET_THRESHOLD) return true;
 
     return false;
   }
 
   /**
-   * Return a boolean for whether the desired tag is seen.
+   * Returns the "wpiblue" Pose2d of the robot from the limelight.
    *
-   * @param ID
-   *            The id of the desired tag.
+   * @return An Optional of Pose2d which is necessary if no valid (at least 2 tags, good distance) data is found from
+   * the limelight.
+   * 
+   * @see Optional
    */
-  public boolean isApriltagVisible(int ID) {
-    if (!isApriltagVisible())
-      return false;
+  public Optional<Pose2d> getBotpose() {
+    Optional<double[]> botpose = getValidBotpose();
+    if (botpose.isEmpty()) return Optional.empty();
 
-    return _limelight.getTag(ID) != null;
+    double[] botpose_array = botpose.get();
+
+    double botposeX = _xFilter.calculate(botpose_array[0]); // to get rid of the weird origin outlier
+    double botposeY = _yFilter.calculate(botpose_array[1]); // to get rid of the weird origin outlier
+    double botposeYaw = _yawFilter.calculate(botpose_array[5]); // to get rid of weird 0 heading outlier
+    Rotation2d botposeRotation = Rotation2d.fromDegrees(botposeYaw);
+
+    Pose2d botpose2D = new Pose2d(botposeX, botposeY, botposeRotation);
+
+    return Optional.of(botpose2D);
   }
 
   /**

src/main/java/frc/robot/utils/UtilFuncs.java

@@ -5,6 +5,7 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -20,19 +21,28 @@ public final class UtilFuncs {
   private static DoubleSupplier _distanceSupplier;
   private static AprilTagFieldLayout _field;
 
+  /**
+   * Loads the AprilTag field.
+   */
   public static void LoadField() {
     _field = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo);
+    _field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
   }
 
+  /**
+   * Gets the necessary speaker pose for auto-aim.
+   * 
+   * @return 3d point to aim at.
+   */
   public static Pose3d GetSpeakerPose() {
     Pose3d pose;
 
     if (GetAlliance() == Alliance.Red) {
       pose =  _field.getTagPose(FieldConstants.SPEAKER_TAG_RED).get();
+    } else {
+      pose = _field.getTagPose(FieldConstants.SPEAKER_TAG_BLUE).get();
     }
 
-    pose = _field.getTagPose(FieldConstants.SPEAKER_TAG_BLUE).get();
-
     return new Pose3d(
       new Translation3d(pose.getX(), pose.getY(), pose.getZ() + FieldConstants.SPEAKER_TAG_OFFSET),
       new Rotation3d()