[FEATURE]: Add 6328 single-tag pose estimation

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

@@ -55,15 +55,13 @@ public class RobotContainer {
   private AlgaeSuperstructure algaeSuperstructure =
       new AlgaeSuperstructure(algaePivot, algaeRollers);
 
-  private AutomaticAutonomousMaker3000 automaker =
-      new AutomaticAutonomousMaker3000(drivetrain, coralSuperstructure);
-
   private Vision vision =
       Vision.create(
           // Java 21 pattern matching switch would be nice
           (drivetrain instanceof DrivetrainSim)
               ? ((DrivetrainSim) drivetrain)::getActualPose
               : drivetrain::getPose,
+          () -> drivetrain.getPose().getRotation(),
           visionEst ->
               drivetrain.addVisionMeasurement(
                   visionEst.estimate().estimatedPose.toPose2d(),
@@ -74,6 +72,8 @@ public class RobotContainer {
                   reefVisionEst.estimate().estimatedPose.toPose2d(),
                   reefVisionEst.estimate().timestampSeconds,
                   reefVisionEst.stdDevs()));
+  private AutomaticAutonomousMaker3000 automaker =
+      new AutomaticAutonomousMaker3000(drivetrain, coralSuperstructure, vision::canSeeReefTag);
 
   private CommandXboxController driver = new CommandXboxController(0);
   private XboxController manipulator = new XboxController(1);
@@ -252,7 +252,7 @@ private void configureTuningBindings() {
     //               System.out.println("Changing volts to: " + volts);
     //             }));
 
-    driver.y().whileTrue(ReefAlign.tuneAlignment(drivetrain));
+    driver.y().whileTrue(ReefAlign.tuneAlignment(drivetrain, vision::canSeeReefTag));
 
     // driver.b().whileTrue(coralSuperstructure.feedCoral());
 
@@ -410,7 +410,10 @@ private void configureBindings() {
                                     //                     .getTargetAngle()
                                     //                     .isEquivalent(
                                     //                         queuedSetpoint.getArmAngle())),
-                                    ReefAlign.alignToReef(drivetrain, () -> queuedReefPosition))
+                                    ReefAlign.alignToReef(
+                                        drivetrain,
+                                        () -> queuedReefPosition,
+                                        vision::canSeeReefTag))
                                 .onlyWhile(
                                     () ->
                                         ReefAlign.isWithinReefRange(

src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java

@@ -26,6 +26,7 @@
 import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.function.IntPredicate;
 import org.json.simple.parser.ParseException;
 
 @Logged
@@ -103,7 +104,10 @@ public class AutomaticAutonomousMaker3000 {
 
   private Command storedAuto;
 
-  public AutomaticAutonomousMaker3000(SwerveDrive drive, CoralSuperstructure coralSuperstructure) {
+  public AutomaticAutonomousMaker3000(
+      SwerveDrive drive,
+      CoralSuperstructure coralSuperstructure,
+      IntPredicate useReefPoseEstimate) {
     this.drive = drive;
     this.coralSuperstructure = coralSuperstructure;
 
@@ -131,14 +135,17 @@ public AutomaticAutonomousMaker3000(SwerveDrive drive, CoralSuperstructure coral
                       switch (preBuiltAuto.getSelected()) {
                         case TAXI ->
                             runPath(autoChooser.build().startingPosition.pathID + " to Brake");
-                        case TOPAUTO -> buildAuto(kTopLaneAuto);
-                        case MIDTOPAUTO -> buildAuto(kMidLaneTopAuto);
-                        case MIDBOTAUTO -> buildAuto(kMidLaneBotAuto);
-                        case BOTAUTO -> buildAuto(kBotLaneAuto);
-                        case MIDPRELOADAUTO -> buildAuto(kMidLaneBotPreloadAuto); // test auto again
+                        case TOPAUTO -> buildAuto(kTopLaneAuto, useReefPoseEstimate);
+                        case MIDTOPAUTO -> buildAuto(kMidLaneTopAuto, useReefPoseEstimate);
+                        case MIDBOTAUTO -> buildAuto(kMidLaneBotAuto, useReefPoseEstimate);
+                        case BOTAUTO -> buildAuto(kBotLaneAuto, useReefPoseEstimate);
+                        case MIDPRELOADAUTO ->
+                            buildAuto(
+                                kMidLaneBotPreloadAuto, useReefPoseEstimate); // test auto again
                         case MIDOPPOSITESIDEAUTO ->
-                            buildAuto(kMidLaneOppositeSideAuto); // test auto x2
-                        case CUSTOM -> buildAuto(autoChooser.build());
+                            buildAuto(
+                                kMidLaneOppositeSideAuto, useReefPoseEstimate); // test auto x2
+                        case CUSTOM -> buildAuto(autoChooser.build(), useReefPoseEstimate);
                         default -> new PathsAndAuto(Commands.none(), new ArrayList<>());
                       };
 
@@ -192,7 +199,7 @@ private void visualizeAuto(List<PathPlannerPath> paths) {
   }
 
   // Returns the path list for visualization and autonomous command
-  public PathsAndAuto buildAuto(CycleAutoConfig config) {
+  public PathsAndAuto buildAuto(CycleAutoConfig config, IntPredicate useReefPoseEstimate) {
     pathError = "";
     try {
       Command auto =
@@ -235,7 +242,8 @@ public PathsAndAuto buildAuto(CycleAutoConfig config) {
                   withScoring(
                       toPathCommand(pathNewGoalEndState, true),
                       config.scoringGroup.get(i).pole,
-                      config.scoringGroup.get(i).level));
+                      config.scoringGroup.get(i).level,
+                      useReefPoseEstimate));
           paths.add(path);
         } else {
 
@@ -269,7 +277,8 @@ public PathsAndAuto buildAuto(CycleAutoConfig config) {
                       withScoring(
                           toPathCommand(scorePathNewGoalEndState),
                           config.scoringGroup.get(i).pole,
-                          config.scoringGroup.get(i).level));
+                          config.scoringGroup.get(i).level,
+                          useReefPoseEstimate));
           lastReefSide = config.scoringGroup.get(i).reefSide;
 
           paths.add(intakePath);
@@ -320,7 +329,8 @@ public Command withIntaking(Command path, FeedLocation location) {
                     coralSuperstructure.feedCoral().until(() -> coralSuperstructure.hasCoral())));
   }
 
-  public Command withScoring(Command path, Pole pole, Level level) {
+  public Command withScoring(
+      Command path, Pole pole, Level level, IntPredicate useReefPoseEstimate) {
     CoralScorerSetpoint setpoint =
         switch (level) {
           default -> CoralScorerSetpoint.L1;
@@ -342,25 +352,30 @@ public Command withScoring(Command path, Pole pole, Level level) {
                 .alongWith(coralSuperstructure.getEndEffector().stallCoralIfDetected()))
         .andThen(
             ReefAlign.alignToReef(
-                    drive, () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT)
-                .withDeadline(
+                drive,
+                () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT,
+                useReefPoseEstimate))
+        .asProxy()
+        .alongWith(
+            coralSuperstructure
+                .goToSetpointPID(() -> preAlignElevatorHeight, () -> setpoint.getArmAngle())
+                .asProxy())
+        .asProxy()
+        .withDeadline(
+            coralSuperstructure
+                .goToSetpointPID(() -> preAlignElevatorHeight, () -> setpoint.getArmAngle())
+                .alongWith(coralSuperstructure.getEndEffector().stallCoralIfDetected())
+                .until(() -> drive.atPoseSetpoint())
+                .withTimeout(2.5)
+                .andThen(
                     coralSuperstructure
-                        .goToSetpointPID(() -> preAlignElevatorHeight, () -> setpoint.getArmAngle())
+                        .goToSetpointProfiled(() -> setpoint)
                         .alongWith(coralSuperstructure.getEndEffector().stallCoralIfDetected())
-                        .until(() -> drive.atPoseSetpoint())
-                        .withTimeout(2.5)
-                        .andThen(
-                            coralSuperstructure
-                                .goToSetpointProfiled(() -> setpoint)
-                                .alongWith(
-                                    coralSuperstructure.getEndEffector().stallCoralIfDetected())
-                                .until(() -> coralSuperstructure.atTargetState(setpoint)))
+                        .until(() -> coralSuperstructure.atTargetState(setpoint)))
+                .andThen(
+                    Commands.waitSeconds(0.5)
                         .andThen(
-                            Commands.waitSeconds(0.5)
-                                .andThen(
-                                    coralSuperstructure
-                                        .outtakeCoral(() -> setpoint)
-                                        .withTimeout(0.5)))));
+                            coralSuperstructure.outtakeCoral(() -> setpoint).withTimeout(0.5))));
   }
 
   private Command toPathCommand(PathPlannerPath path, boolean zero) {

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

@@ -30,6 +30,7 @@
 import java.util.Map;
 import java.util.Optional;
 import java.util.function.DoubleSupplier;
+import java.util.function.IntPredicate;
 import java.util.function.Supplier;
 
 public class ReefAlign {
@@ -197,7 +198,9 @@ private static Pose2d getNearestRightAlign(int reefTagID) {
   }
 
   public static Command alignToReef(
-      SwerveDrive swerveDrive, Supplier<ReefPosition> targetReefPosition) {
+      SwerveDrive swerveDrive,
+      Supplier<ReefPosition> targetReefPosition,
+      IntPredicate useReefPoseEstimate) {
     return Commands.runOnce(() -> Leds.getInstance().isReefAligning = true)
         .andThen(
             swerveDrive.driveToFieldPose(
@@ -211,12 +214,22 @@ public static Command alignToReef(
                       };
 
                   return new AlignmentSetpoint(target, true);
+                },
+                () -> {
+                  final int nearestReefID = getNearestReefID(swerveDrive.getPose());
+                  if (useReefPoseEstimate.test(nearestReefID)) {
+                    return swerveDrive.getReefVisionPose();
+                  } else {
+                    return swerveDrive.getPose();
+                  }
                 }))
         .finallyDo(() -> Leds.getInstance().isReefAligning = false);
   }
 
   public static Command alignToPrealignReef(
-      SwerveDrive swerveDrive, Supplier<ReefPosition> targetReefPosition) {
+      SwerveDrive swerveDrive,
+      Supplier<ReefPosition> targetReefPosition,
+      IntPredicate useReefPoseEstimate) {
     return Commands.runOnce(() -> Leds.getInstance().isReefAligning = true)
         .andThen(
             swerveDrive.driveToFieldPose(
@@ -235,12 +248,22 @@ public static Command alignToPrealignReef(
                               new Translation2d(kIntermediateDistance, Meters.zero()),
                               Rotation2d.kZero));
                   return new AlignmentSetpoint(target, false);
+                },
+                () -> {
+                  final int nearestReefID = getNearestReefID(swerveDrive.getPose());
+                  if (useReefPoseEstimate.test(nearestReefID)) {
+                    return swerveDrive.getReefVisionPose();
+                  } else {
+                    return swerveDrive.getPose();
+                  }
                 }))
         .finallyDo(() -> Leds.getInstance().isReefAligning = false);
   }
 
   public static Command alignToTag(
-      SwerveDrive swerveDrive, Supplier<ReefPosition> targetReefPosition) {
+      SwerveDrive swerveDrive,
+      Supplier<ReefPosition> targetReefPosition,
+      IntPredicate useReefPoseEstimate) {
     return swerveDrive.driveToFieldPose(
         () -> {
           Pose2d target =
@@ -258,10 +281,18 @@ public static Command alignToTag(
               target.plus(new Transform2d(translationError.getX(), 0, Rotation2d.kZero));
 
           return new AlignmentSetpoint(newTarget, false);
+        },
+        () -> {
+          final int nearestReefID = getNearestReefID(swerveDrive.getPose());
+          if (useReefPoseEstimate.test(nearestReefID)) {
+            return swerveDrive.getReefVisionPose();
+          } else {
+            return swerveDrive.getPose();
+          }
         });
   }
 
-  public static Command tuneAlignment(SwerveDrive swerveDrive) {
+  public static Command tuneAlignment(SwerveDrive swerveDrive, IntPredicate useReefPoseEstimate) {
     TunableConstant depth = new TunableConstant("/ReefAlign/Depth", kReefDistance.in(Inch));
     TunableConstant side = new TunableConstant("/ReefAlign/Side", kLeftAlignDistance.in(Inch));
 
@@ -273,6 +304,14 @@ public static Command tuneAlignment(SwerveDrive swerveDrive) {
                       new Transform2d(
                           Inches.of(depth.get()), Inches.of(side.get()), kReefAlignmentRotation));
           return new AlignmentSetpoint(pose, true);
+        },
+        () -> {
+          final int nearestReefID = getNearestReefID(swerveDrive.getPose());
+          if (useReefPoseEstimate.test(nearestReefID)) {
+            return swerveDrive.getReefVisionPose();
+          } else {
+            return swerveDrive.getPose();
+          }
         });
   }
 

src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java

@@ -198,12 +198,9 @@ public void driveRobotCentric(
   }
 
   @Override
-  public void driveToFieldPose(Pose2d pose) {
-
-    final var currentPose = getPose();
-
+  public void driveToFieldPose(Pose2d target, Pose2d current) {
     double distance =
-        currentPose.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation());
+        current.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation());
 
     // increase weighting of velocity from PID radius (weight = 0) to velocity radius (weight = 1)
     double autoFfFactor =
@@ -219,21 +216,20 @@ public void driveToFieldPose(Pose2d pose) {
 
     ChassisSpeeds targetSpeeds =
         ChassisSpeeds.discretize(
-            xPoseController.calculate(getPose().getX(), pose.getX())
+            xPoseController.calculate(current.getX(), target.getX())
                 + xPoseController.getSetpoint().velocity * ffFactor,
-            yPoseController.calculate(getPose().getY(), pose.getY())
+            yPoseController.calculate(current.getY(), target.getY())
                 + yPoseController.getSetpoint().velocity * ffFactor,
             thetaController.calculate(
-                    getPose().getRotation().getRadians(), pose.getRotation().getRadians())
+                    current.getRotation().getRadians(), target.getRotation().getRadians())
                 + thetaController.getSetpoint().velocity * ffFactor,
             RobotConstants.kRobotLoopPeriod.in(Seconds));
 
-    if (currentPose.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation())
+    if (current.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation())
         < DrivetrainConstants.kAlignmentSetpointTranslationTolerance.in(Meters))
       targetSpeeds = new ChassisSpeeds(0, 0, targetSpeeds.omegaRadiansPerSecond);
 
-    if (Math.abs(
-            currentPose.getRotation().minus(alignmentSetpoint.pose().getRotation()).getDegrees())
+    if (Math.abs(current.getRotation().minus(alignmentSetpoint.pose().getRotation()).getDegrees())
         < DrivetrainConstants.kAlignmentSetpointRotationTolerance.in(Degrees))
       targetSpeeds =
           new ChassisSpeeds(targetSpeeds.vxMetersPerSecond, targetSpeeds.vyMetersPerSecond, 0);

src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java

@@ -172,13 +172,11 @@ public void driveRobotCentric(
   }
 
   @Override
-  public void driveToFieldPose(Pose2d pose) {
+  public void driveToFieldPose(Pose2d pose, Pose2d current) {
     if (pose == null) return;
 
-    final Pose2d currentPose = getPose();
-
     double distance =
-        currentPose.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation());
+        current.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation());
 
     // increase weighting of velocity from PID radius (weight = 0) to velocity radius (weight = 1)
     double autoFfFactor =
@@ -194,21 +192,20 @@ public void driveToFieldPose(Pose2d pose) {
 
     ChassisSpeeds targetSpeeds =
         ChassisSpeeds.discretize(
-            xPoseController.calculate(getPose().getX(), pose.getX())
+            xPoseController.calculate(current.getX(), pose.getX())
                 + xPoseController.getSetpoint().velocity * ffFactor,
-            yPoseController.calculate(getPose().getY(), pose.getY())
+            yPoseController.calculate(current.getY(), pose.getY())
                 + yPoseController.getSetpoint().velocity * ffFactor,
             thetaController.calculate(
-                    getPose().getRotation().getRadians(), pose.getRotation().getRadians())
+                    current.getRotation().getRadians(), pose.getRotation().getRadians())
                 + thetaController.getSetpoint().velocity * ffFactor,
             RobotConstants.kRobotLoopPeriod.in(Seconds));
 
-    if (currentPose.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation())
+    if (current.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation())
         < DrivetrainConstants.kAlignmentSetpointTranslationTolerance.in(Meters))
       targetSpeeds = new ChassisSpeeds(0, 0, targetSpeeds.omegaRadiansPerSecond);
 
-    if (Math.abs(
-            currentPose.getRotation().minus(alignmentSetpoint.pose().getRotation()).getDegrees())
+    if (Math.abs(current.getRotation().minus(alignmentSetpoint.pose().getRotation()).getDegrees())
         < DrivetrainConstants.kAlignmentSetpointRotationTolerance.in(Degrees))
       targetSpeeds =
           new ChassisSpeeds(targetSpeeds.vxMetersPerSecond, targetSpeeds.vyMetersPerSecond, 0);

src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java

@@ -166,9 +166,13 @@ default Command driveFixedHeading(
   }
 
   // field relative auto drive w/ external pid controllers
-  void driveToFieldPose(Pose2d pose);
+  void driveToFieldPose(Pose2d target, Pose2d current);
 
-  default Command driveToFieldPose(Supplier<AlignmentSetpoint> pose) {
+  default Command driveToFieldPose(Supplier<Pose2d> pose) {
+    return driveToFieldPose(() -> new AlignmentSetpoint(pose.get(), true), this::getPose);
+  }
+
+  default Command driveToFieldPose(Supplier<AlignmentSetpoint> pose, Supplier<Pose2d> robotPose) {
     return runOnce(
             () -> {
               ChassisSpeeds speeds =
@@ -186,7 +190,7 @@ default Command driveToFieldPose(Supplier<AlignmentSetpoint> pose) {
             run(
                 () -> {
                   setAlignmentSetpoint(pose.get());
-                  driveToFieldPose(pose.get().pose);
+                  driveToFieldPose(pose.get().pose, robotPose.get());
                 }));
   }