Fix Turret Regressions

src/main/java/frc/robot/AutoCommandFactory.java

@@ -201,7 +201,7 @@ public AutoRoutine rampAuto() {
                             .deadlineFor(intake.extendHopper(1.0).andThen(
                                 intake.intakeBalls().alongWith(indexer.spinWhileIntake()))),
                         crossRampIntoZone(routine), swerve.emergencyStop(),
-                        CommandFactory.shoot(targetingState, shooter, indexer, adjustableHood)
+                        CommandFactory.shoot(targetingState, shooter, indexer, adjustableHood, turret)
                             .alongWith(
                                 Commands.waitSeconds(2.0).andThen(intake.retractHopper(1.0)))));
         return routine;
@@ -430,7 +430,7 @@ public Command halfSweepTrenchRampPath(AutoRoutine routine, boolean left, Double
      */
     Command autoShooting(double shootingTime, double delayTime) {
         return Commands.sequence(Commands.waitSeconds(delayTime),
-            CommandFactory.shoot(targetingState, shooter, indexer, adjustableHood)
+            CommandFactory.shoot(targetingState, shooter, indexer, adjustableHood, turret)
                 .alongWith(intake.jerkIntake(),
                     turret.goToAngleFieldRelative(
                         () -> targetingState.getDesiredTurretHeadingFieldRelative()))

src/main/java/frc/robot/CommandFactory.java

@@ -54,12 +54,17 @@ public static Command preShoot(Supplier<Pose2d> robotPoseSupplier,
         }, shooter, turret);
     }
 
-    /** Shoot at a given target. */
+    /**
+     * Shoot at a given target. Does not command the turret, but stops any turret search and only
+     * feeds while the turret is on its setpoint.
+     */
     public static Command shoot(TargetingState state, Shooter shooter, Indexer indexer,
-        AdjustableHood hood) {
+        AdjustableHood hood, Turret turret) {
         return Commands.parallel(shooter.shoot(() -> state.getDesiredFlywheelSpeed()),
             hood.setGoal(() -> Degrees.of(state.getDesiredHoodAngle())),
-            indexer.runSpindexer(() -> state.isOkayToShoot()));
+            indexer.runSpindexer(() -> state.isOkayToShoot() && turret.isAimed()))
+            .beforeStarting(() -> turret.setShooting(true))
+            .finallyDo(() -> turret.setShooting(false));
     }
 
     /** Point turret at hub. */

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

@@ -4,6 +4,7 @@
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 import java.util.List;
@@ -50,6 +51,9 @@ public final class Constants {
 
     public static final boolean keepInField = false;
 
+    /** Push the pose estimate out of the hubs, which the robot cannot drive through. */
+    public static final boolean keepOutOfHubs = true;
+
     public static final int odometryQueueSize = 100;
 
     /** Constants for driver controls */
@@ -325,6 +329,20 @@ public static final class Vision {
         public static final Pose3d turretCenter =
             new Pose3d(new Translation3d(-0.155575, -0.13335, 0), Rotation3d.kZero);
 
+        /**
+         * While the robot and turret are stationary, fuse turret-camera heading at the camera's
+         * configured {@code rotationError} and snap translation to vision, instead of using the
+         * motion-dependent heading model below.
+         */
+        public static final boolean turretStationaryReset = true;
+        /** Turret-camera heading std-dev (radians) with the turret still. */
+        public static final double turretHeadingStdDevFloor = Units.degreesToRadians(1.0);
+        /**
+         * Effective timing uncertainty (seconds) of the turret angle at a frame's capture time.
+         * Heading std-dev grows by this times the turret's angular rate.
+         */
+        public static final double turretHeadingTimingStdDev = 0.010;
+
         public static final Pose3d turretRight = new Pose3d(-0.18097, -0.27012, 0.51406,
             new Rotation3d(0, Units.degreesToRadians(-22.115), 0.0))
                 .rotateAround(turretCenter.getTranslation(), new Rotation3d(Rotation2d.kZero));
@@ -594,6 +612,15 @@ public static final class Turret {
         public static final Angle maxAngle = Degrees.of(180);
         public static final Angle minAngle = Degrees.of(-180);
 
+        /** The indexer only feeds when the turret is within this of its setpoint. */
+        public static final Angle aimTolerance = Degrees.of(5);
+        /**
+         * Search sweep rate. Slow enough that turret-camera heading stays trustworthy (see
+         * {@code Vision.turretHeadingTimingStdDev}) so the first frames found can pull a drifted
+         * estimate back.
+         */
+        public static final AngularVelocity searchRate = RadiansPerSecond.of(1.0);
+
         public static final double maxWhipTime = 2.0;
         public static final double degreeTolerance = 5.0;
     }

src/main/java/frc/robot/FieldConstants.java

@@ -2,6 +2,7 @@
 
 import java.io.IOException;
 import java.io.UncheckedIOException;
+import java.util.Set;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -293,8 +294,20 @@ public static class Hub {
             AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d();
     }
 
+    /** AprilTag IDs mounted on the blue and red hubs. */
+    private static final Set<Integer> hubTagIds =
+        Set.of(18, 19, 20, 21, 24, 25, 26, 27, 2, 3, 4, 5, 8, 9, 10, 11);
+
+    /** Return true if the AprilTag is mounted on either hub. */
+    public static boolean isHubTag(int id) {
+        return hubTagIds.contains(id);
+    }
+
     /** Return true if pose is in the area of the bump. */
     public static boolean isOnBump(Pose2d pose) {
+        if (isInsideHub(pose.getTranslation())) {
+            return false;
+        }
         AABB bumpArea = new AABB(Hub.centerHub, LeftBump.depth,
             LeftBump.width * 2 + Units.inchesToMeters(47.0));
         AABB oppBumpArea =
@@ -305,6 +318,21 @@ public static boolean isOnBump(Pose2d pose) {
             || oppBumpArea.contains(pose.getTranslation());
     }
 
+    /** Return true if translation is within the field boundaries. */
+    public static boolean isInField(Translation2d point) {
+        return point.getX() >= 0.0 && point.getX() <= fieldLength
+            && point.getY() >= 0.0 && point.getY() <= fieldWidth;
+    }
+
+    /** Return true if translation is inside either the blue or red hub. */
+    public static boolean isInsideHub(Translation2d point) {
+        AABB hubArea = new AABB(Hub.centerHub, Hub.width, Hub.width);
+        AABB oppHubArea =
+            new AABB(new Translation2d(fieldLength - Hub.centerHub.getX(), Hub.centerHub.getY()),
+                Hub.width, Hub.width);
+        return hubArea.contains(point) || oppHubArea.contains(point);
+    }
+
     /**
      * Geometry for the left bump and common reference points.
      *

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

@@ -259,13 +259,13 @@ public RobotContainer(RobotRunType runtimeType) {
         shooter.setDefaultCommand(shooter.shoot(0.0));
 
         // TRIGGERS
-        RobotModeTriggers.disabled().and(vision.seesTwoAprilTags.negate())
+        RobotModeTriggers.disabled().and(vision.seesTurretTag.negate())
             .whileTrue(leds.setLEDsBreathe(Color.kBlue));
         RobotModeTriggers.teleop().onTrue(swerve.resetFieldRelativeOffsetBasedOnPose());
         // RobotModeTriggers.teleop().onTrue(Commands.runOnce(() -> {
         // swerve.state.setTrims(0.0, swerve.state.getTrimLeft());
         // }));
-        vision.seesTwoAprilTags.whileTrue(leds.setRainbow());
+        vision.seesTurretTag.whileTrue(leds.setRainbow());
 
         // BUTTON BINDINGS
         maybeController("Driver", driver, this::setupDriver);
@@ -292,7 +292,7 @@ private void setupDriver() {
 
         driver.rightTrigger()
             .whileTrue(Commands.parallel(
-                CommandFactory.shoot(targetingState, shooter, indexer, adjustableHood),
+                CommandFactory.shoot(targetingState, shooter, indexer, adjustableHood, turret),
                 swerve.driveUserRelative(TeleopControls.teleopControls(
                     () -> -combineControllers(CommandXboxController::getLeftY, driver, tuner),
                     () -> -combineControllers(CommandXboxController::getLeftX, driver, tuner),
@@ -334,6 +334,7 @@ private void setupOperator() {
             .onTrue(CommandFactory.resetInit(swerve, turret));
         operator.b().whileTrue(turret.setVoltage(() -> 0));
         operator.x().whileTrue(turret.setVoltage(() -> operator.getLeftY() * 3.0));
+        operator.rightBumper().whileTrue(turret.search());
         operator.y().onTrue(Commands.runOnce(() -> targetingState.setTrims(0.0, 0.0)));
         operator.povUp().onTrue(Commands.runOnce(() -> {
             targetingState.incTrims(0.5, 0);

src/main/java/frc/robot/localization/CameraProcessor.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc.robot.Constants;
+import frc.robot.FieldConstants;
 import frc.robot.subsystems.vision.CameraConstants;
 
 /**
@@ -31,6 +32,7 @@ public class CameraProcessor {
 
     private final CameraConstants cameraConstants;
 
+
     /**
      * Adapter that resolves the time-varying robot-to-camera transform for a turreted camera.
      * {@code null} for fixed (non-turret) cameras.
@@ -60,23 +62,32 @@ public CameraProcessor(CameraConstants cameraConstants, @Nullable TurretCameraAd
     }
 
 
+
     /**
      * processes photon results to get robots pose
      *
      * @param result photon results
      * @param currentSpeeds robot speeds
+     * @param isReal whether the vision observation is from real hardware or replay
      * @return position data
      */
     public Result<VisionObservation, RejectionReason> process(PhotonPipelineResult result,
-        ChassisSpeeds currentSpeeds) {
+        ChassisSpeeds currentSpeeds, boolean isReal) {
         var multiTag = result.getMultiTagResult();
         Transform3d robotToCamera_ = cameraConstants.robotToCamera;
 
         double translationSpeed =
             Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond);
         double rotationSpeed = Math.abs(currentSpeeds.omegaRadiansPerSecond);
 
+        double turretHeadingStdDev = 0.0;
         if (adapter != null) {
+            double turretRate = adapter.getRateAt(result.getTimestampSeconds());
+            turretHeadingStdDev = Math.hypot(Constants.Vision.turretHeadingStdDevFloor,
+                Constants.Vision.turretHeadingTimingStdDev * turretRate);
+            Logger.recordOutput("State/Camera/" + cameraConstants.name + "/turretRate", turretRate);
+            Logger.recordOutput("State/Camera/" + cameraConstants.name + "/turretHeadingStdDev",
+                turretHeadingStdDev);
             var maybeRobotToCamera =
                 adapter.getRobotToCameraAt(robotToCamera_, result.getTimestampSeconds());
             if (maybeRobotToCamera.isEmpty()) {
@@ -120,6 +131,15 @@ public Result<VisionObservation, RejectionReason> process(PhotonPipelineResult r
             Logger.recordOutput("State/Camera/" + cameraConstants.name + "/estRobotPose",
                 estRobotPose);
 
+            // Physically impossible poses; these show up when the robot is tilted on the bump.
+            var estTranslation = estRobotPose.toPose2d().getTranslation();
+            if (Constants.keepInField && !FieldConstants.isInField(estTranslation)) {
+                return Result.err(RejectionReason.OUTSIDE_FIELD);
+            }
+            if (FieldConstants.isInsideHub(estTranslation)) {
+                return Result.err(RejectionReason.INSIDE_HUB);
+            }
+
             double stdDevMultiplier = stdDevMultiplier(result.targets, cameraPose);
             double translationStdDev =
                 stdDevMultiplier * velocityTranslationError + cameraConstants.translationError;
@@ -138,7 +158,8 @@ public Result<VisionObservation, RejectionReason> process(PhotonPipelineResult r
             }
 
             return Result.ok(new VisionObservation(cameraPose, robotToCamera_, translationStdDev,
-                rotationStdDev, result.getTimestampSeconds()));
+                rotationStdDev, result.getTimestampSeconds(), cameraConstants.isTurret,
+                cameraConstants.name, isReal, turretHeadingStdDev));
         }
 
         return Result.err(RejectionReason.SINGLE_TAG_ONLY);