From 26f803c7fe99d81faa67902b6b8bd102d5c140a1 Mon Sep 17 00:00:00 2001 From: Jace Rodgers Date: Mon, 5 Oct 2026 10:22:47 -0500 Subject: [PATCH 1/5] refactor and bring back stuff --- src/main/java/frc/robot/FieldConstants.java | 15 + .../robot/localization/CameraProcessor.java | 10 +- .../robot/localization/DrivetrainState.java | 78 ++---- .../localization/TurretCameraAdapter.java | 2 +- .../robot/localization/VisionObservation.java | 13 +- .../frc/robot/subsystems/turret/Turret.java | 1 + .../frc/robot/subsystems/vision/Vision.java | 6 +- .../localization/TurretLocalizationTest.java | 264 ++++++++++++++++++ 8 files changed, 337 insertions(+), 52 deletions(-) create mode 100644 src/test/java/frc/robot/localization/TurretLocalizationTest.java diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index bd300cdf..19197826 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -305,6 +305,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. * diff --git a/src/main/java/frc/robot/localization/CameraProcessor.java b/src/main/java/frc/robot/localization/CameraProcessor.java index 2c966883..d049f8b4 100644 --- a/src/main/java/frc/robot/localization/CameraProcessor.java +++ b/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; /** @@ -114,6 +115,12 @@ public Result process(PhotonPipelineResult r Logger.recordOutput("State/Camera/" + cameraConstants.name + "/estRobotPose", estRobotPose); + var estRobotPose2d = estRobotPose.toPose2d(); + if (!FieldConstants.isInField(estRobotPose2d.getTranslation()) + || FieldConstants.isInsideHub(estRobotPose2d.getTranslation())) { + return Result.err(RejectionReason.OUTSIDE_FIELD); + } + double stdDevMultiplier = stdDevMultiplier(result.targets, cameraPose); double translationStdDev = stdDevMultiplier * velocityTranslationError + cameraConstants.translationError; @@ -132,7 +139,8 @@ public Result process(PhotonPipelineResult r } return Result.ok(new VisionObservation(cameraPose, robotToCamera_, translationStdDev, - rotationStdDev, result.getTimestampSeconds())); + rotationStdDev, result.getTimestampSeconds(), cameraConstants.isTurret, + cameraConstants.name)); } return Result.err(RejectionReason.SINGLE_TAG_ONLY); diff --git a/src/main/java/frc/robot/localization/DrivetrainState.java b/src/main/java/frc/robot/localization/DrivetrainState.java index cc775448..7986fd15 100644 --- a/src/main/java/frc/robot/localization/DrivetrainState.java +++ b/src/main/java/frc/robot/localization/DrivetrainState.java @@ -2,22 +2,15 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; -import java.util.List; -import java.util.Optional; import java.util.function.Consumer; import org.littletonrobotics.junction.Logger; -import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -38,9 +31,6 @@ public class DrivetrainState { private final PoseEstimator visionAdjustedOdometry; - private final TimeInterpolatableBuffer currentTurretAngle = - TimeInterpolatableBuffer.createBuffer(1.5); - private Rotation2d gyroOffset = Rotation2d.kZero; private Rotation2d prevGyroReading = Rotation2d.kZero; @@ -155,24 +145,6 @@ public void overrideInit(Pose2d pose) { initted = true; } - /** Get robot to camera for camera mounted on the turret */ - public Optional getTurretRobotToCamera(Transform3d turretToCamera, - double timestamp) { - var maybeRotation = currentTurretAngle.getSample(timestamp); - if (maybeRotation.isEmpty()) { - return Optional.empty(); - } - var turretRotation = maybeRotation.get(); - Rotation3d rotate = new Rotation3d(0.0, 0.0, turretRotation.getRadians()); - - Transform3d robotToTurret = - new Transform3d(Constants.Vision.turretCenter.getTranslation(), rotate); - - Transform3d robotToCamera = robotToTurret.plus(turretToCamera); - - return Optional.of(robotToCamera); - } - private double prevAngle; /** Set the current turret angle */ @@ -185,7 +157,6 @@ public void setTurretRawAngle(double timestamp, Angle angle) { Logger.recordOutput("State/stationary/turret", false); } prevAngle = angleDeg; - currentTurretAngle.addSample(timestamp, new Rotation2d(angle)); Translation2d[] turretDirection = new Translation2d[2]; turretDirection[0] = getTurretCenterFieldFrame().getTranslation(); turretDirection[1] = @@ -198,32 +169,45 @@ public void setTurretRawAngle(double timestamp, Angle angle) { public void addVisionObservation(VisionObservation observations) { Pose2d robotPose = observations.cameraPose().plus(observations.robotToCamera().inverse()).toPose2d(); + double rotationStdDev = observations.rotationStdDev(); + + if (observations.isTurret()) { + boolean isStationary = this.lastTimeMoved + 0.5 < observations.timestamp(); + String camName = + observations.cameraName().isEmpty() ? "turret" : observations.cameraName(); + Logger.recordOutput("State/Camera/" + camName + "/isStationary", isStationary); + Logger.recordOutput("State/Camera/" + camName + "/stationaryValue", + this.lastTimeMoved - observations.timestamp()); + Logger.recordOutput("State/Camera/" + camName + "/lastMoved", this.lastTimeMoved); + Logger.recordOutput("State/Camera/" + camName + "/timestamp", observations.timestamp()); + + boolean onBump = RobotBase.isReal() + && (FieldConstants.isOnBump(getGlobalPoseEstimate()) + || FieldConstants.isOnBump(robotPose)); + + if (isStationary || onBump) { + if (robotPose.getTranslation() + .getSquaredDistance(getGlobalPoseEstimate().getTranslation()) > Math + .pow(Units.inchesToMeters(3), 2)) { + visionAdjustedOdometry.resetTranslation(robotPose.getTranslation()); + } + } + + if (!isStationary) { + rotationStdDev = 10000.0; + } + } + Pose2d before = visionAdjustedOdometry.getEstimatedPosition(); visionAdjustedOdometry.addVisionMeasurement(robotPose, observations.timestamp(), - observations.getStdDev()); + VecBuilder.fill(observations.translationStdDev(), observations.translationStdDev(), + rotationStdDev)); Pose2d after = visionAdjustedOdometry.getEstimatedPosition(); double correction = after.getTranslation().getDistance(before.getTranslation()); Logger.recordOutput("State/Correction", correction); Logger.recordOutput("State/VisionRobotPose", robotPose); } - - private static double stdDevMultiplier(List targets, Pose3d cameraPose) { - double totalDistance = 0.0; - int count = 0; - for (var tag : targets) { - var maybeTagPose = Constants.Vision.fieldLayout.getTagPose(tag.getFiducialId()); - if (maybeTagPose.isPresent()) { - var tagPose = maybeTagPose.get(); - totalDistance += tagPose.getTranslation().getDistance(cameraPose.getTranslation()); - count++; - } - } - double avgDistance = totalDistance / count; - double stddev = Math.pow(avgDistance, 2.0) / count; - return stddev; - } - /** * Returns the current best estimate of the robot's global field pose. * diff --git a/src/main/java/frc/robot/localization/TurretCameraAdapter.java b/src/main/java/frc/robot/localization/TurretCameraAdapter.java index e839fb2f..c542450b 100644 --- a/src/main/java/frc/robot/localization/TurretCameraAdapter.java +++ b/src/main/java/frc/robot/localization/TurretCameraAdapter.java @@ -23,7 +23,7 @@ public void recordTurretAngle(double timestamp, Rotation2d angle) { angleBuffer.addSample(timestamp, angle); } - Optional getRobotToCameraAt(Transform3d turretToCamera, double timestamp) { + public Optional getRobotToCameraAt(Transform3d turretToCamera, double timestamp) { var maybeTurretRotation = angleBuffer.getSample(timestamp); if (maybeTurretRotation.isEmpty()) { return Optional.empty(); diff --git a/src/main/java/frc/robot/localization/VisionObservation.java b/src/main/java/frc/robot/localization/VisionObservation.java index 8e84e665..b6188fc5 100644 --- a/src/main/java/frc/robot/localization/VisionObservation.java +++ b/src/main/java/frc/robot/localization/VisionObservation.java @@ -8,7 +8,18 @@ /** Vision Observation Record */ public record VisionObservation(Pose3d cameraPose, Transform3d robotToCamera, - double translationStdDev, double rotationStdDev, double timestamp) { + double translationStdDev, double rotationStdDev, double timestamp, boolean isTurret, + String cameraName) { + + public VisionObservation(Pose3d cameraPose, Transform3d robotToCamera, + double translationStdDev, double rotationStdDev, double timestamp) { + this(cameraPose, robotToCamera, translationStdDev, rotationStdDev, timestamp, false, ""); + } + + public VisionObservation(Pose3d cameraPose, Transform3d robotToCamera, + double translationStdDev, double rotationStdDev, double timestamp, boolean isTurret) { + this(cameraPose, robotToCamera, translationStdDev, rotationStdDev, timestamp, isTurret, ""); + } public Vector getStdDev() { return VecBuilder.fill(translationStdDev, translationStdDev, rotationStdDev); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 8bb67d79..f5bd670f 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -53,6 +53,7 @@ public void periodic() { Logger.recordOutput("Turret/currentAngle", inputs.relativeAngle); adapter.recordTurretAngle(MathSharedStore.getTimestamp(), new Rotation2d(Rotations.of(inputs.relativeAngle))); + state.setTurretRawAngle(MathSharedStore.getTimestamp(), Rotations.of(inputs.relativeAngle)); } public Rotation2d getTurretHeading() { diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index b725681d..d7b11036 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -56,6 +56,7 @@ public class Vision extends SubsystemBase { new Trigger(() -> twoAprilTags()).debounce(.3, Debouncer.DebounceType.kBoth); private final CameraProcessor[] cameraProcessors; private final Result[] processorResults; + private final TurretCameraAdapter adapter; /** * Creates the vision subsystem. @@ -67,6 +68,7 @@ public Vision(DrivetrainState state, VisionIO io, TurretCameraAdapter adapter) { super("Vision"); this.io = io; this.state = state; + this.adapter = adapter; this.cameraInputs = IntStream.range(0, Constants.Vision.cameraConstants.length) .mapToObj((_x) -> new VisionIO.CameraInputs()).toArray(VisionIO.CameraInputs[]::new); this.cameraInputKeys = IntStream.range(0, Constants.Vision.cameraConstants.length) @@ -115,7 +117,7 @@ public void periodic() { processorResults[result._0()] = cameraProcessors[result._0()].process(result._1(), state.getFieldRelativeSpeeds()); if (processorResults[result._0()] instanceof CameraProcessor.Err err) { - cameraContributed[result._0()] = err != null; + cameraContributed[result._0()] = false; Logger.recordOutput(cameraContributedKeys[result._0()] + "/rejection", err.toString()); } else if (processorResults[result._0()] instanceof CameraProcessor.Ok ok) { @@ -129,7 +131,7 @@ public void periodic() { for (int i = 0; i < result._1().targets.size(); i++) { var robotToCamera = Constants.Vision.cameraConstants[result._0()].robotToCamera; if (Constants.Vision.cameraConstants[result._0()].isTurret) { - var maybeRobotToCamera = state.getTurretRobotToCamera(robotToCamera, + var maybeRobotToCamera = adapter.getRobotToCameraAt(robotToCamera, result._1().getTimestampSeconds()); if (maybeRobotToCamera.isEmpty()) { continue; diff --git a/src/test/java/frc/robot/localization/TurretLocalizationTest.java b/src/test/java/frc/robot/localization/TurretLocalizationTest.java new file mode 100644 index 00000000..cf9d1047 --- /dev/null +++ b/src/test/java/frc/robot/localization/TurretLocalizationTest.java @@ -0,0 +1,264 @@ +package frc.robot.localization; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.io.File; +import java.util.List; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.FieldConstants; +import frc.robot.subsystems.vision.CameraConstants; +import frc.robot.subsystems.vision.CameraConstantsBuilder; + +public class TurretLocalizationTest { + + private CameraConstants turretCameraConstants; + private CameraConstants backCameraConstants; + private TurretCameraAdapter turretAdapter; + + @BeforeEach + public void setUp() { + turretCameraConstants = new CameraConstantsBuilder() + .coProcessorName("ubuntu") + .name("turret") + .height(800) + .width(1280) + .horizontalFieldOfView(80) + .simFps(20) + .simLatency(0.8) + .simLatencyStdDev(0.02) + .calibrationErrorMean(0.8) + .calibrationErrorStdDev(0.08) + .robotToCamera(new Transform3d(Constants.Vision.turretCenter, Constants.Vision.turretRight)) + .translationError(Units.inchesToMeters(6)) + .rotationError(0.3) + .singleTagError(0) + .isTurret(true) + .finish(); + + backCameraConstants = new CameraConstantsBuilder() + .coProcessorName("skip") + .name("back") + .height(800) + .width(1280) + .horizontalFieldOfView(80) + .simFps(20) + .simLatency(0.3) + .simLatencyStdDev(0.02) + .calibrationErrorMean(0.8) + .calibrationErrorStdDev(0.08) + .robotToCamera(new Transform3d(new Translation3d(-0.335, 0.19, 0.202), + new Rotation3d(Degrees.of(180), Degrees.of(-26), Degrees.of(163)))) + .translationError(0.3) + .rotationError(0.3) + .singleTagError(0) + .isTurret(false) + .finish(); + + turretAdapter = new TurretCameraAdapter(Constants.Vision.turretCenter.getTranslation()); + } + + private SwerveModulePosition[] getZeroModulePositions() { + return new SwerveModulePosition[] { + new SwerveModulePosition(0.0, Rotation2d.kZero), + new SwerveModulePosition(0.0, Rotation2d.kZero), + new SwerveModulePosition(0.0, Rotation2d.kZero), + new SwerveModulePosition(0.0, Rotation2d.kZero) + }; + } + + @Test + public void testPhysicalImpossibilityRejection() { + CameraProcessor processor = new CameraProcessor(turretCameraConstants, turretAdapter); + turretAdapter.recordTurretAngle(1.0, Rotation2d.kZero); + + // Outside field test + assertFalse(FieldConstants.isInField(new Translation2d(-0.5, 4.0))); + assertFalse(FieldConstants.isInField(new Translation2d(FieldConstants.fieldLength + 1.0, 4.0))); + assertFalse(FieldConstants.isInField(new Translation2d(4.0, -0.5))); + assertFalse(FieldConstants.isInField(new Translation2d(4.0, FieldConstants.fieldWidth + 0.5))); + assertTrue(FieldConstants.isInField(new Translation2d(4.0, 4.0))); + + // Inside hub test + assertTrue(FieldConstants.isInsideHub(FieldConstants.Hub.centerHub)); + assertTrue(FieldConstants.isInsideHub(new Translation2d( + FieldConstants.fieldLength - FieldConstants.Hub.centerHub.getX(), + FieldConstants.Hub.centerHub.getY()))); + assertFalse(FieldConstants.isInsideHub(new Translation2d(1.0, 1.0))); + } + + @Test + public void testTurretMotionUpdatesLastTimeMoved() { + DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); + + // Initial angle + state.setTurretRawAngle(1.0, Degrees.of(0)); + + // Motion under threshold (<= 2 degrees) shouldn't count as moving + double movedTimeBefore = 1.0; + state.setTurretRawAngle(1.05, Degrees.of(1.5)); + + // Motion over threshold (> 2 degrees) should count as moving + state.setTurretRawAngle(1.10, Degrees.of(5.0)); + } + + @Test + public void testStationaryAndMovingRotationStdDev() { + DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); + state.resetPose(new Pose2d(5.0, 5.0, Rotation2d.kZero)); + + // Mark turret moving at t = 10.0 + state.setTurretRawAngle(10.0, Degrees.of(0)); + state.setTurretRawAngle(10.02, Degrees.of(10.0)); // moved > 2 deg + + // Turret frame at t = 10.2 (< 0.5s since moved -> NOT stationary) + Pose3d cameraPose = new Pose3d(5.0, 5.0, 0.5, new Rotation3d()); + Transform3d robotToCamera = new Transform3d(); + VisionObservation movingTurretObs = new VisionObservation( + cameraPose, robotToCamera, 0.1, 0.05, 10.2, true, "turret"); + + state.addVisionObservation(movingTurretObs); + + // When moving, rotation std dev is 10000.0, so heading is barely adjusted + // Now test stationary turret frame at t = 11.0 (> 0.5s since moved -> stationary) + VisionObservation stationaryTurretObs = new VisionObservation( + cameraPose, robotToCamera, 0.1, 0.05, 11.0, true, "turret"); + + state.addVisionObservation(stationaryTurretObs); + + // Non-turret camera at t = 10.2 (moving) should NOT have rotation std dev forced to 10000.0 + VisionObservation nonTurretObs = new VisionObservation( + cameraPose, robotToCamera, 0.1, 0.05, 10.2, false, "back"); + state.addVisionObservation(nonTurretObs); + } + + @Test + public void testBumpTranslationSnap() { + DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); + // Place initial estimate on field + state.resetPose(new Pose2d(2.0, 2.0, Rotation2d.kZero)); + + // Mark moving at t = 10.0 + state.updateMeasuredSpeeds(new ChassisSpeeds(1.0, 0.0, 0.0)); + + // Case 1: Moving, not on bump, error > 3 inches -> NO snap (filtered only) + Pose3d visionPoseOffBump = new Pose3d(2.2, 2.0, 0.5, new Rotation3d()); // 0.2m = ~8 inches error + VisionObservation movingOffBump = new VisionObservation( + visionPoseOffBump, new Transform3d(), 0.1, 0.05, 10.1, true, "turret"); + state.addVisionObservation(movingOffBump); + + // Estimate should NOT have snapped all the way to 2.2 + assertTrue(Math.abs(state.getGlobalPoseEstimate().getX() - 2.2) > 0.05); + + // Case 2: Stationary, error > 3 inches -> SNAP occurs + state.updateMeasuredSpeeds(new ChassisSpeeds(0.0, 0.0, 0.0)); + double now = MathSharedStore.getTimestamp(); + // Turret moved at 'now' + state.setTurretRawAngle(now, Degrees.of(10.0)); + + // Observation 1 second later (> 0.5s after last moved -> stationary) + Pose3d visionPoseStationary = new Pose3d(3.0, 3.0, 0.5, new Rotation3d()); + VisionObservation stationaryObs = new VisionObservation( + visionPoseStationary, new Transform3d(), 0.1, 0.05, now + 1.0, true, "turret"); + state.addVisionObservation(stationaryObs); + + // Snapped directly to (3.0, 3.0) + assertEquals(3.0, state.getGlobalPoseEstimate().getX(), 1e-4); + assertEquals(3.0, state.getGlobalPoseEstimate().getY(), 1e-4); + } + + @Test + public void testIsOnBumpGeometry() { + // Center hub X and slightly offset in Y (within LeftBump width) + Pose2d allianceBumpPose = new Pose2d( + FieldConstants.Hub.centerHub.getX(), + FieldConstants.Hub.centerHub.getY() + Units.inchesToMeters(50.0), + Rotation2d.kZero); + assertTrue(FieldConstants.isOnBump(allianceBumpPose)); + + // Opposing bump + Pose2d oppBumpPose = new Pose2d( + FieldConstants.fieldLength - FieldConstants.Hub.centerHub.getX(), + FieldConstants.Hub.centerHub.getY() + Units.inchesToMeters(50.0), + Rotation2d.kZero); + assertTrue(FieldConstants.isOnBump(oppBumpPose)); + + // Off bump + Pose2d offBumpPose = new Pose2d(1.0, 1.0, Rotation2d.kZero); + assertFalse(FieldConstants.isOnBump(offBumpPose)); + } + + @Test + public void testCurieMatchLogIfPresent() { + File logFile = new File("/Users/jacerodgers/Downloads/akit_26-05-01_22-08-48_curie_q119.wpilog"); + if (!logFile.exists()) { + System.out.println("Curie log file not found, skipping log replay test."); + return; + } + + System.out.println("Found Curie Q119 log file! Inspecting vision & bump entries..."); + WPILOGReader reader = new WPILOGReader(logFile.getAbsolutePath()); + reader.start(); + org.littletonrobotics.junction.LogTable table = new org.littletonrobotics.junction.LogTable(0); + int totalFrames = 0; + int onBumpFrames = 0; + int turretMovedFrames = 0; + int speedMovedFrames = 0; + int visionOnBumpFrames = 0; + java.util.Map runningState = + new java.util.HashMap<>(); + while (reader.updateTable(table)) { + totalFrames++; + runningState.putAll(table.getAll(false)); + var onBump = runningState.get("/RealOutputs/State/isOnBump"); + if (onBump != null && onBump.getBoolean()) { + onBumpFrames++; + } + var turretMoved = runningState.get("/RealOutputs/State/stationary/turret"); + if (turretMoved != null && turretMoved.getBoolean()) { + turretMovedFrames++; + } + var speedMoved = runningState.get("/RealOutputs/State/stationary/speeds"); + if (speedMoved != null && speedMoved.getBoolean()) { + speedMovedFrames++; + } + var turretEst = runningState.get("/RealOutputs/Viz/Cameras/turret/Est"); + if (turretEst != null) { + double[] poseArr = turretEst.getDoubleArray(); + if (poseArr != null && poseArr.length >= 2) { + Pose2d vPose = new Pose2d(poseArr[0], poseArr[1], Rotation2d.kZero); + if (FieldConstants.isOnBump(vPose)) { + visionOnBumpFrames++; + } + } + } + } + System.out.println("Curie Q119 Log analysis: " + totalFrames + " total frames | onBump: " + + onBumpFrames + " | turretMoved: " + turretMovedFrames + " | speedMoved: " + speedMovedFrames + + " | visionOnBump: " + visionOnBumpFrames); + assertTrue(totalFrames > 0, "Curie Q119 match log should have frames"); + assertTrue(turretMovedFrames > 0, "Curie Q119 match should have instances where turret moved"); + } +} From 7a52a5e71e1da8ad817698ebc369a3020254053c Mon Sep 17 00:00:00 2001 From: Jace Rodgers Date: Mon, 5 Oct 2026 14:43:03 -0500 Subject: [PATCH 2/5] lots of changes --- .../robot/localization/CameraProcessor.java | 50 ++++++- .../robot/localization/DrivetrainState.java | 6 +- .../robot/localization/RejectionReason.java | 2 +- .../robot/localization/VisionObservation.java | 12 +- .../frc/robot/subsystems/vision/Vision.java | 4 +- .../frc/robot/subsystems/vision/VisionIO.java | 3 + .../robot/subsystems/vision/VisionReal.java | 1 + .../robot/subsystems/vision/VisionSim.java | 7 + .../localization/TurretLocalizationTest.java | 140 +++++++++++++++--- 9 files changed, 182 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/localization/CameraProcessor.java b/src/main/java/frc/robot/localization/CameraProcessor.java index d049f8b4..41bdb57d 100644 --- a/src/main/java/frc/robot/localization/CameraProcessor.java +++ b/src/main/java/frc/robot/localization/CameraProcessor.java @@ -1,16 +1,22 @@ package frc.robot.localization; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Seconds; import java.util.List; import org.jspecify.annotations.Nullable; import org.littletonrobotics.junction.Logger; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import edu.wpi.first.math.geometry.Pose2d; 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.kinematics.ChassisSpeeds; import frc.robot.Constants; import frc.robot.FieldConstants; +import frc.robot.math.geometry.Penetration; +import frc.robot.math.geometry.Rectangle; +import frc.robot.math.geometry.SeparatingAxis; import frc.robot.subsystems.vision.CameraConstants; /** @@ -32,6 +38,17 @@ public class CameraProcessor { private final CameraConstants cameraConstants; + private final Rectangle robotRect = new Rectangle("pose", Pose2d.kZero, + Constants.Swerve.bumperFront.in(Meters) * 2, Constants.Swerve.bumperRight.in(Meters) * 2); + private final Rectangle[] hubRects = new Rectangle[] { + new Rectangle("hub", new Pose2d(FieldConstants.Hub.centerHub, Rotation2d.kZero), + FieldConstants.Hub.width, FieldConstants.Hub.width), + new Rectangle("oppHub", + new Pose2d(FieldConstants.fieldLength - FieldConstants.Hub.centerHub.getX(), + FieldConstants.Hub.centerHub.getY(), Rotation2d.kZero), + FieldConstants.Hub.width, FieldConstants.Hub.width)}; + private final Penetration hubPenetration = new Penetration("HubPen"); + /** * Adapter that resolves the time-varying robot-to-camera transform for a turreted camera. * {@code null} for fixed (non-turret) cameras. @@ -70,6 +87,19 @@ public CameraProcessor(CameraConstants cameraConstants, @Nullable TurretCameraAd */ public Result process(PhotonPipelineResult result, ChassisSpeeds currentSpeeds) { + return process(result, currentSpeeds, true); + } + + /** + * 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 process(PhotonPipelineResult result, + ChassisSpeeds currentSpeeds, boolean isReal) { var multiTag = result.getMultiTagResult(); Transform3d robotToCamera_ = cameraConstants.robotToCamera; @@ -116,9 +146,21 @@ public Result process(PhotonPipelineResult r estRobotPose); var estRobotPose2d = estRobotPose.toPose2d(); - if (!FieldConstants.isInField(estRobotPose2d.getTranslation()) - || FieldConstants.isInsideHub(estRobotPose2d.getTranslation())) { - return Result.err(RejectionReason.OUTSIDE_FIELD); + robotRect.setPose(estRobotPose2d); + + // Reject if whole robot (including bumpers) extends outside field perimeter + for (var corner : robotRect.getCorners()) { + if (corner.getX() < 0.0 || corner.getX() > FieldConstants.fieldLength + || corner.getY() < 0.0 || corner.getY() > FieldConstants.fieldWidth) { + return Result.err(RejectionReason.OUTSIDE_FIELD); + } + } + + // Reject if robot penetrates either hub + for (var hub : hubRects) { + if (SeparatingAxis.solve(robotRect, hub, hubPenetration)) { + return Result.err(RejectionReason.INSIDE_HUB); + } } double stdDevMultiplier = stdDevMultiplier(result.targets, cameraPose); @@ -140,7 +182,7 @@ public Result process(PhotonPipelineResult r return Result.ok(new VisionObservation(cameraPose, robotToCamera_, translationStdDev, rotationStdDev, result.getTimestampSeconds(), cameraConstants.isTurret, - cameraConstants.name)); + cameraConstants.name, isReal)); } return Result.err(RejectionReason.SINGLE_TAG_ONLY); diff --git a/src/main/java/frc/robot/localization/DrivetrainState.java b/src/main/java/frc/robot/localization/DrivetrainState.java index 7986fd15..a868710e 100644 --- a/src/main/java/frc/robot/localization/DrivetrainState.java +++ b/src/main/java/frc/robot/localization/DrivetrainState.java @@ -181,7 +181,7 @@ public void addVisionObservation(VisionObservation observations) { Logger.recordOutput("State/Camera/" + camName + "/lastMoved", this.lastTimeMoved); Logger.recordOutput("State/Camera/" + camName + "/timestamp", observations.timestamp()); - boolean onBump = RobotBase.isReal() + boolean onBump = observations.isReal() && (FieldConstants.isOnBump(getGlobalPoseEstimate()) || FieldConstants.isOnBump(robotPose)); @@ -208,6 +208,10 @@ public void addVisionObservation(VisionObservation observations) { Logger.recordOutput("State/VisionRobotPose", robotPose); } + public double getLastTimeMoved() { + return lastTimeMoved; + } + /** * Returns the current best estimate of the robot's global field pose. * diff --git a/src/main/java/frc/robot/localization/RejectionReason.java b/src/main/java/frc/robot/localization/RejectionReason.java index 4c36f144..87d61d4f 100644 --- a/src/main/java/frc/robot/localization/RejectionReason.java +++ b/src/main/java/frc/robot/localization/RejectionReason.java @@ -2,5 +2,5 @@ /** Rejection Reasons For Vision */ public enum RejectionReason { - NO_TARGETS, SINGLE_TAG_ONLY, HIGH_AMBIGUITY, MISSING_TURRET_ANGLE, OUTSIDE_FIELD + NO_TARGETS, SINGLE_TAG_ONLY, HIGH_AMBIGUITY, MISSING_TURRET_ANGLE, OUTSIDE_FIELD, INSIDE_HUB } diff --git a/src/main/java/frc/robot/localization/VisionObservation.java b/src/main/java/frc/robot/localization/VisionObservation.java index b6188fc5..7632ad63 100644 --- a/src/main/java/frc/robot/localization/VisionObservation.java +++ b/src/main/java/frc/robot/localization/VisionObservation.java @@ -9,17 +9,7 @@ /** Vision Observation Record */ public record VisionObservation(Pose3d cameraPose, Transform3d robotToCamera, double translationStdDev, double rotationStdDev, double timestamp, boolean isTurret, - String cameraName) { - - public VisionObservation(Pose3d cameraPose, Transform3d robotToCamera, - double translationStdDev, double rotationStdDev, double timestamp) { - this(cameraPose, robotToCamera, translationStdDev, rotationStdDev, timestamp, false, ""); - } - - public VisionObservation(Pose3d cameraPose, Transform3d robotToCamera, - double translationStdDev, double rotationStdDev, double timestamp, boolean isTurret) { - this(cameraPose, robotToCamera, translationStdDev, rotationStdDev, timestamp, isTurret, ""); - } + String cameraName, boolean isReal) { public Vector getStdDev() { return VecBuilder.fill(translationStdDev, translationStdDev, rotationStdDev); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index d7b11036..386ef9ef 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -114,8 +114,8 @@ public void periodic() { cameraViz[i] = new Translation3d[0]; } for (var result : results) { - processorResults[result._0()] = - cameraProcessors[result._0()].process(result._1(), state.getFieldRelativeSpeeds()); + processorResults[result._0()] = cameraProcessors[result._0()].process( + result._1(), state.getFieldRelativeSpeeds(), cameraInputs[result._0()].isReal); if (processorResults[result._0()] instanceof CameraProcessor.Err err) { cameraContributed[result._0()] = false; Logger.recordOutput(cameraContributedKeys[result._0()] + "/rejection", diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 6699df10..965d2be7 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -23,11 +23,13 @@ public static class CameraInputs implements LoggableInputs, Cloneable { public PhotonPipelineResult[] results = new PhotonPipelineResult[0]; public Optional> cameraMatrix = Optional.empty(); public Optional> distCoeffs = Optional.empty(); + public boolean isReal = true; private static final PhotonPipelineResultSerde serde = new PhotonPipelineResultSerde(); @Override public void toLog(LogTable table) { + table.put("isReal", isReal); table.put("length", results.length); for (int i = 0; i < results.length; i++) { LogTable subtable = table.getSubtable("[" + i + "]"); @@ -56,6 +58,7 @@ public void toLog(LogTable table) { @Override public void fromLog(LogTable table) { + isReal = table.get("isReal", true); int length = table.get("length", 0); results = new PhotonPipelineResult[length]; for (int i = 0; i < length; i++) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionReal.java b/src/main/java/frc/robot/subsystems/vision/VisionReal.java index 94fc911e..075a9d22 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionReal.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionReal.java @@ -46,6 +46,7 @@ public void updateInputs(CameraInputs[] inputs) { cameras[i].getAllUnreadResults().toArray(PhotonPipelineResult[]::new); inputs[i].cameraMatrix = cameras[i].getCameraMatrix(); inputs[i].distCoeffs = cameras[i].getDistCoeffs(); + inputs[i].isReal = true; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionSim.java index aa450848..298e43c5 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSim.java @@ -62,4 +62,11 @@ public void updateState(Pose3d robotPose, Angle turretAngle) { new Rotation3d(new Rotation2d(turretAngle))))); } + @Override + public void updateInputs(CameraInputs[] inputs) { + super.updateInputs(inputs); + for (int i = 0; i < inputs.length; i++) { + inputs[i].isReal = false; + } + } } diff --git a/src/test/java/frc/robot/localization/TurretLocalizationTest.java b/src/test/java/frc/robot/localization/TurretLocalizationTest.java index cf9d1047..ac4a5030 100644 --- a/src/test/java/frc/robot/localization/TurretLocalizationTest.java +++ b/src/test/java/frc/robot/localization/TurretLocalizationTest.java @@ -9,6 +9,7 @@ import java.io.File; import java.util.List; +import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.littletonrobotics.junction.wpilog.WPILOGReader; @@ -16,6 +17,8 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -29,6 +32,9 @@ import edu.wpi.first.math.util.Units; import frc.robot.Constants; import frc.robot.FieldConstants; +import frc.robot.math.geometry.Penetration; +import frc.robot.math.geometry.Rectangle; +import frc.robot.math.geometry.SeparatingAxis; import frc.robot.subsystems.vision.CameraConstants; import frc.robot.subsystems.vision.CameraConstantsBuilder; @@ -38,6 +44,11 @@ public class TurretLocalizationTest { private CameraConstants backCameraConstants; private TurretCameraAdapter turretAdapter; + @BeforeAll + public static void initializeHal() { + HAL.initialize(500, 0); + } + @BeforeEach public void setUp() { turretCameraConstants = new CameraConstantsBuilder() @@ -91,67 +102,127 @@ private SwerveModulePosition[] getZeroModulePositions() { @Test public void testPhysicalImpossibilityRejection() { - CameraProcessor processor = new CameraProcessor(turretCameraConstants, turretAdapter); - turretAdapter.recordTurretAngle(1.0, Rotation2d.kZero); - - // Outside field test + // Outside field test - point center assertFalse(FieldConstants.isInField(new Translation2d(-0.5, 4.0))); assertFalse(FieldConstants.isInField(new Translation2d(FieldConstants.fieldLength + 1.0, 4.0))); assertFalse(FieldConstants.isInField(new Translation2d(4.0, -0.5))); assertFalse(FieldConstants.isInField(new Translation2d(4.0, FieldConstants.fieldWidth + 0.5))); assertTrue(FieldConstants.isInField(new Translation2d(4.0, 4.0))); - // Inside hub test + // Robot bounding box outside perimeter test + Rectangle robotRect = new Rectangle("pose", Pose2d.kZero, + Constants.Swerve.bumperFront.in(Meters) * 2, Constants.Swerve.bumperRight.in(Meters) * 2); + + // Center is inside (x = 0.1), but corners extend past x = 0.0 + robotRect.setPose(new Pose2d(0.1, 4.0, Rotation2d.kZero)); + boolean anyCornerOutside = false; + for (var corner : robotRect.getCorners()) { + if (corner.getX() < 0.0 || corner.getX() > FieldConstants.fieldLength + || corner.getY() < 0.0 || corner.getY() > FieldConstants.fieldWidth) { + anyCornerOutside = true; + break; + } + } + assertTrue(anyCornerOutside, "Robot bumper extending past perimeter should be detected as outside field"); + + // Inside hub test - point center assertTrue(FieldConstants.isInsideHub(FieldConstants.Hub.centerHub)); assertTrue(FieldConstants.isInsideHub(new Translation2d( FieldConstants.fieldLength - FieldConstants.Hub.centerHub.getX(), FieldConstants.Hub.centerHub.getY()))); assertFalse(FieldConstants.isInsideHub(new Translation2d(1.0, 1.0))); + + // SeparatingAxis hub collision test + Rectangle hubRect = new Rectangle("hub", + new Pose2d(FieldConstants.Hub.centerHub, Rotation2d.kZero), + FieldConstants.Hub.width, FieldConstants.Hub.width); + + Penetration penetration = new Penetration("pen"); + // Robot centered right on hub + robotRect.setPose(new Pose2d(FieldConstants.Hub.centerHub, Rotation2d.kZero)); + assertTrue(SeparatingAxis.solve(robotRect, hubRect, penetration), + "Robot at hub center must penetrate hub"); + + // Robot safely away from hub + robotRect.setPose(new Pose2d(1.0, 1.0, Rotation2d.kZero)); + assertFalse(SeparatingAxis.solve(robotRect, hubRect, penetration), + "Robot at (1, 1) must not penetrate hub"); } @Test public void testTurretMotionUpdatesLastTimeMoved() { DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); - // Initial angle + assertEquals(0.0, state.getLastTimeMoved(), 1e-6); + + // Initial angle with 0 deg difference (prevAngle defaults to 0.0) state.setTurretRawAngle(1.0, Degrees.of(0)); + assertEquals(0.0, state.getLastTimeMoved(), 1e-6, "Initial 0 deg should not trigger movement"); // Motion under threshold (<= 2 degrees) shouldn't count as moving - double movedTimeBefore = 1.0; state.setTurretRawAngle(1.05, Degrees.of(1.5)); + assertEquals(0.0, state.getLastTimeMoved(), 1e-6, "Change of 1.5 deg (<= 2 deg) should not count as moving"); // Motion over threshold (> 2 degrees) should count as moving state.setTurretRawAngle(1.10, Degrees.of(5.0)); + assertTrue(state.getLastTimeMoved() > 0.0, "Change of > 2 deg must update lastTimeMoved"); } @Test public void testStationaryAndMovingRotationStdDev() { + SimHooks.restartTiming(); DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); state.resetPose(new Pose2d(5.0, 5.0, Rotation2d.kZero)); + state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, MathSharedStore.getTimestamp()); + + double t0 = MathSharedStore.getTimestamp(); - // Mark turret moving at t = 10.0 - state.setTurretRawAngle(10.0, Degrees.of(0)); - state.setTurretRawAngle(10.02, Degrees.of(10.0)); // moved > 2 deg + // Mark turret moving at t0 + state.setTurretRawAngle(t0, Degrees.of(0)); + state.setTurretRawAngle(t0, Degrees.of(10.0)); // moved > 2 deg - // Turret frame at t = 10.2 (< 0.5s since moved -> NOT stationary) - Pose3d cameraPose = new Pose3d(5.0, 5.0, 0.5, new Rotation3d()); + // Turret frame at t0 (< 0.5s since moved -> NOT stationary) + // Camera indicates a 45 degree heading offset + Pose3d cameraPose = new Pose3d(5.0, 5.0, 0.5, new Rotation3d(0, 0, Math.toRadians(45))); Transform3d robotToCamera = new Transform3d(); VisionObservation movingTurretObs = new VisionObservation( - cameraPose, robotToCamera, 0.1, 0.05, 10.2, true, "turret"); + cameraPose, robotToCamera, 0.1, 0.05, t0, true, "turret", true); state.addVisionObservation(movingTurretObs); - // When moving, rotation std dev is 10000.0, so heading is barely adjusted - // Now test stationary turret frame at t = 11.0 (> 0.5s since moved -> stationary) + // When moving, turret camera rotation std dev is set to 10000.0, so robot heading is barely touched + double headingAfterMoving = state.getGlobalPoseEstimate().getRotation().getDegrees(); + assertTrue(Math.abs(headingAfterMoving) < 0.1, + "Turret camera should not touch heading while moving (got: " + headingAfterMoving + " deg)"); + + // Step sim timing forward by 1.0s (> 0.5s since moved -> now stationary) + SimHooks.stepTiming(1.0); + double t1 = MathSharedStore.getTimestamp(); + state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, t1); + VisionObservation stationaryTurretObs = new VisionObservation( - cameraPose, robotToCamera, 0.1, 0.05, 11.0, true, "turret"); + cameraPose, robotToCamera, 0.1, 0.05, t1, true, "turret", true); state.addVisionObservation(stationaryTurretObs); - // Non-turret camera at t = 10.2 (moving) should NOT have rotation std dev forced to 10000.0 + // Stationary turret camera fuses heading at normal std dev (0.05 rad), pulling heading toward 45 deg + double headingAfterStationary = state.getGlobalPoseEstimate().getRotation().getDegrees(); + assertTrue(Math.abs(headingAfterStationary) > 1.0, + "Stationary turret camera should adjust heading (got: " + headingAfterStationary + " deg)"); + + // Reset pose back to 0 heading to test non-turret camera while moving + state.resetPose(new Pose2d(5.0, 5.0, Rotation2d.kZero)); + state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, t1); + state.setTurretRawAngle(t1, Degrees.of(30.0)); // mark moving again + + // Non-turret camera (moving) should NOT have rotation std dev forced to 10000.0 VisionObservation nonTurretObs = new VisionObservation( - cameraPose, robotToCamera, 0.1, 0.05, 10.2, false, "back"); + cameraPose, robotToCamera, 0.1, 0.05, t1, false, "back", true); state.addVisionObservation(nonTurretObs); + + double headingAfterNonTurret = state.getGlobalPoseEstimate().getRotation().getDegrees(); + assertTrue(Math.abs(headingAfterNonTurret) > 1.0, + "Non-turret camera should fuse heading even when moving (got: " + headingAfterNonTurret + " deg)"); } @Test @@ -160,13 +231,15 @@ public void testBumpTranslationSnap() { // Place initial estimate on field state.resetPose(new Pose2d(2.0, 2.0, Rotation2d.kZero)); - // Mark moving at t = 10.0 + double now = MathSharedStore.getTimestamp(); + + // Mark moving at 'now' state.updateMeasuredSpeeds(new ChassisSpeeds(1.0, 0.0, 0.0)); // Case 1: Moving, not on bump, error > 3 inches -> NO snap (filtered only) Pose3d visionPoseOffBump = new Pose3d(2.2, 2.0, 0.5, new Rotation3d()); // 0.2m = ~8 inches error VisionObservation movingOffBump = new VisionObservation( - visionPoseOffBump, new Transform3d(), 0.1, 0.05, 10.1, true, "turret"); + visionPoseOffBump, new Transform3d(), 0.1, 0.05, now + 0.1, true, "turret", true); state.addVisionObservation(movingOffBump); // Estimate should NOT have snapped all the way to 2.2 @@ -174,19 +247,35 @@ public void testBumpTranslationSnap() { // Case 2: Stationary, error > 3 inches -> SNAP occurs state.updateMeasuredSpeeds(new ChassisSpeeds(0.0, 0.0, 0.0)); - double now = MathSharedStore.getTimestamp(); - // Turret moved at 'now' state.setTurretRawAngle(now, Degrees.of(10.0)); // Observation 1 second later (> 0.5s after last moved -> stationary) Pose3d visionPoseStationary = new Pose3d(3.0, 3.0, 0.5, new Rotation3d()); VisionObservation stationaryObs = new VisionObservation( - visionPoseStationary, new Transform3d(), 0.1, 0.05, now + 1.0, true, "turret"); + visionPoseStationary, new Transform3d(), 0.1, 0.05, now + 1.0, true, "turret", true); state.addVisionObservation(stationaryObs); // Snapped directly to (3.0, 3.0) assertEquals(3.0, state.getGlobalPoseEstimate().getX(), 1e-4); assertEquals(3.0, state.getGlobalPoseEstimate().getY(), 1e-4); + + // Case 3: On bump (robot on bump or vision on bump), error > 3 inches -> SNAP occurs even if moving + Pose2d bumpCenterPose = new Pose2d( + FieldConstants.Hub.centerHub.getX(), + FieldConstants.Hub.centerHub.getY() + Units.inchesToMeters(50.0), + Rotation2d.kZero); + state.resetPose(bumpCenterPose); + state.updateMeasuredSpeeds(new ChassisSpeeds(1.0, 0.0, 0.0)); // moving! + + Pose3d bumpVisionPose = new Pose3d( + bumpCenterPose.getX() + 0.2, bumpCenterPose.getY() + 0.2, 0.5, new Rotation3d()); + VisionObservation onBumpMovingObs = new VisionObservation( + bumpVisionPose, new Transform3d(), 0.1, 0.05, now + 0.01, true, "turret", true); + state.addVisionObservation(onBumpMovingObs); + + // Should snap because it is on bump! + assertEquals(bumpVisionPose.getX(), state.getGlobalPoseEstimate().getX(), 1e-4); + assertEquals(bumpVisionPose.getY(), state.getGlobalPoseEstimate().getY(), 1e-4); } @Test @@ -212,7 +301,10 @@ public void testIsOnBumpGeometry() { @Test public void testCurieMatchLogIfPresent() { - File logFile = new File("/Users/jacerodgers/Downloads/akit_26-05-01_22-08-48_curie_q119.wpilog"); + File logFile = new File("logs/akit_26-05-01_22-08-48_curie_q119.wpilog"); + if (!logFile.exists()) { + logFile = new File("/Users/jacerodgers/Downloads/akit_26-05-01_22-08-48_curie_q119.wpilog"); + } if (!logFile.exists()) { System.out.println("Curie log file not found, skipping log replay test."); return; From 3990ba059240cbd7059fe65acd7b42672490decb Mon Sep 17 00:00:00 2001 From: Jace Rodgers Date: Tue, 6 Oct 2026 18:30:17 -0500 Subject: [PATCH 3/5] fixed issues --- .../robot/localization/CameraProcessor.java | 10 ++++--- .../robot/localization/DrivetrainState.java | 21 +++++++++++--- .../frc/robot/subsystems/swerve/Swerve.java | 3 +- .../frc/robot/subsystems/swerve/SwerveIO.java | 1 + .../robot/subsystems/swerve/SwerveReal.java | 1 + .../robot/subsystems/swerve/SwerveSim.java | 1 + .../localization/TurretLocalizationTest.java | 29 ++++++++++--------- 7 files changed, 43 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/localization/CameraProcessor.java b/src/main/java/frc/robot/localization/CameraProcessor.java index 41bdb57d..70341f58 100644 --- a/src/main/java/frc/robot/localization/CameraProcessor.java +++ b/src/main/java/frc/robot/localization/CameraProcessor.java @@ -149,10 +149,12 @@ public Result process(PhotonPipelineResult r robotRect.setPose(estRobotPose2d); // Reject if whole robot (including bumpers) extends outside field perimeter - for (var corner : robotRect.getCorners()) { - if (corner.getX() < 0.0 || corner.getX() > FieldConstants.fieldLength - || corner.getY() < 0.0 || corner.getY() > FieldConstants.fieldWidth) { - return Result.err(RejectionReason.OUTSIDE_FIELD); + if (Constants.keepInField) { + for (var corner : robotRect.getCorners()) { + if (corner.getX() < 0.0 || corner.getX() > FieldConstants.fieldLength + || corner.getY() < 0.0 || corner.getY() > FieldConstants.fieldWidth) { + return Result.err(RejectionReason.OUTSIDE_FIELD); + } } } diff --git a/src/main/java/frc/robot/localization/DrivetrainState.java b/src/main/java/frc/robot/localization/DrivetrainState.java index a868710e..a62453f5 100644 --- a/src/main/java/frc/robot/localization/DrivetrainState.java +++ b/src/main/java/frc/robot/localization/DrivetrainState.java @@ -16,7 +16,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.RobotBase; import frc.robot.Constants; import frc.robot.FieldConstants; import frc.robot.math.geometry.Rectangle; @@ -95,16 +94,30 @@ public void resetInit() { */ public void addOdometryObservation(SwerveModulePosition[] wheelPositions, Rotation2d gyroYaw, double timestamp) { + addOdometryObservation(wheelPositions, gyroYaw, timestamp, true); + } + + /** + * Updates odometry and pose estimates using swerve module encoders and an optional gyro + * measurement. + * + * @param wheelPositions current swerve module positions + * @param gyroYaw current robot yaw, if available + * @param timestamp measurement timestamp in seconds + * @param isReal whether this observation comes from real hardware (or replay) vs sim + */ + public void addOdometryObservation(SwerveModulePosition[] wheelPositions, Rotation2d gyroYaw, + double timestamp, boolean isReal) { prevGyroReading = gyroYaw; Logger.recordOutput("State/prevRot", getGlobalPoseEstimate().getRotation()); var before = getGlobalPoseEstimate(); - visionAdjustedOdometry.update(gyroYaw.minus(gyroOffset), wheelPositions); + visionAdjustedOdometry.updateWithTime(timestamp, gyroYaw.minus(gyroOffset), wheelPositions); var after = getGlobalPoseEstimate(); if (FieldConstants.isOnBump(before)) { Logger.recordOutput("State/isOnBump", true); var diff = after.minus(before); diff = new Transform2d(diff.getX() * 0.6, diff.getY(), diff.getRotation()); - if (RobotBase.isReal()) { + if (isReal) { visionAdjustedOdometry.resetPose(before.plus(diff)); } } else { @@ -152,7 +165,7 @@ public void setTurretRawAngle(double timestamp, Angle angle) { var angleDeg = angle.in(Degrees); if (Math.abs(angleDeg - prevAngle) > 2) { Logger.recordOutput("State/stationary/turret", true); - this.lastTimeMoved = MathSharedStore.getTimestamp(); + this.lastTimeMoved = timestamp; } else { Logger.recordOutput("State/stationary/turret", false); } diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index e1bbccdc..b06e1c13 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -231,7 +231,8 @@ public void periodic() { wheelPositions[j] = modules[j].getOdometryPosition(i); } state.addOdometryObservation(wheelPositions, - Rotation2d.fromRadians(gyroInputs.yawRads[i]), sampleTimestamps[i]); + Rotation2d.fromRadians(gyroInputs.yawRads[i]), sampleTimestamps[i], + this.inputs.isReal); } SwerveModuleState[] wheelStates = new SwerveModuleState[modules.length]; for (int j = 0; j < modules.length; j++) { diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java index 18853dda..845c7744 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java @@ -15,6 +15,7 @@ public interface SwerveIO { @AutoLog public static class SwerveInputs { public double[] timestamps = new double[0]; + public boolean isReal = true; } /** Update inputs */ diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java index a97c4812..228af10b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -20,6 +20,7 @@ public SwerveReal(PhoenixOdometryThread odometryThread) { public void updateInputs(SwerveInputs inputs) { inputs.timestamps = this.timestampQueue.stream().mapToDouble(x -> x).toArray(); this.timestampQueue.clear(); + inputs.isReal = true; } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java index 652e12fa..91ffd618 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java @@ -61,6 +61,7 @@ public SwerveModuleIO moduleProvider(int index, PhoenixOdometryThread odometryTh @Override public void updateInputs(SwerveInputs inputs) { inputs.timestamps = new double[] {Timer.getTimestamp()}; + inputs.isReal = false; } diff --git a/src/test/java/frc/robot/localization/TurretLocalizationTest.java b/src/test/java/frc/robot/localization/TurretLocalizationTest.java index ac4a5030..9f0bf0dc 100644 --- a/src/test/java/frc/robot/localization/TurretLocalizationTest.java +++ b/src/test/java/frc/robot/localization/TurretLocalizationTest.java @@ -17,7 +17,6 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; -import edu.wpi.first.wpilibj.simulation.SimHooks; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.geometry.Pose2d; @@ -32,6 +31,7 @@ import edu.wpi.first.math.util.Units; import frc.robot.Constants; import frc.robot.FieldConstants; +import frc.robot.subsystems.swerve.util.SwerveArcOdometry; import frc.robot.math.geometry.Penetration; import frc.robot.math.geometry.Rectangle; import frc.robot.math.geometry.SeparatingAxis; @@ -165,17 +165,17 @@ public void testTurretMotionUpdatesLastTimeMoved() { // Motion over threshold (> 2 degrees) should count as moving state.setTurretRawAngle(1.10, Degrees.of(5.0)); - assertTrue(state.getLastTimeMoved() > 0.0, "Change of > 2 deg must update lastTimeMoved"); + assertEquals(1.10, state.getLastTimeMoved(), 1e-6, + "Change of > 2 deg must update lastTimeMoved to the provided timestamp"); } @Test public void testStationaryAndMovingRotationStdDev() { - SimHooks.restartTiming(); DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); - state.resetPose(new Pose2d(5.0, 5.0, Rotation2d.kZero)); - state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, MathSharedStore.getTimestamp()); + state.resetPose(new Pose2d(2.0, 2.0, Rotation2d.kZero)); - double t0 = MathSharedStore.getTimestamp(); + double t0 = 1.0; + state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, t0); // Mark turret moving at t0 state.setTurretRawAngle(t0, Degrees.of(0)); @@ -183,7 +183,7 @@ public void testStationaryAndMovingRotationStdDev() { // Turret frame at t0 (< 0.5s since moved -> NOT stationary) // Camera indicates a 45 degree heading offset - Pose3d cameraPose = new Pose3d(5.0, 5.0, 0.5, new Rotation3d(0, 0, Math.toRadians(45))); + Pose3d cameraPose = new Pose3d(2.0, 2.0, 0.5, new Rotation3d(0, 0, Math.toRadians(45))); Transform3d robotToCamera = new Transform3d(); VisionObservation movingTurretObs = new VisionObservation( cameraPose, robotToCamera, 0.1, 0.05, t0, true, "turret", true); @@ -195,9 +195,8 @@ public void testStationaryAndMovingRotationStdDev() { assertTrue(Math.abs(headingAfterMoving) < 0.1, "Turret camera should not touch heading while moving (got: " + headingAfterMoving + " deg)"); - // Step sim timing forward by 1.0s (> 0.5s since moved -> now stationary) - SimHooks.stepTiming(1.0); - double t1 = MathSharedStore.getTimestamp(); + // 1.0s later (> 0.5s since moved -> now stationary) + double t1 = 2.0; state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, t1); VisionObservation stationaryTurretObs = new VisionObservation( @@ -211,13 +210,14 @@ public void testStationaryAndMovingRotationStdDev() { "Stationary turret camera should adjust heading (got: " + headingAfterStationary + " deg)"); // Reset pose back to 0 heading to test non-turret camera while moving - state.resetPose(new Pose2d(5.0, 5.0, Rotation2d.kZero)); - state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, t1); - state.setTurretRawAngle(t1, Degrees.of(30.0)); // mark moving again + state.resetPose(new Pose2d(2.0, 2.0, Rotation2d.kZero)); + double t2 = 3.0; + state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, t2); + state.setTurretRawAngle(t2, Degrees.of(30.0)); // mark moving again // Non-turret camera (moving) should NOT have rotation std dev forced to 10000.0 VisionObservation nonTurretObs = new VisionObservation( - cameraPose, robotToCamera, 0.1, 0.05, t1, false, "back", true); + cameraPose, robotToCamera, 0.1, 0.05, t2, false, "back", true); state.addVisionObservation(nonTurretObs); double headingAfterNonTurret = state.getGlobalPoseEstimate().getRotation().getDegrees(); @@ -225,6 +225,7 @@ public void testStationaryAndMovingRotationStdDev() { "Non-turret camera should fuse heading even when moving (got: " + headingAfterNonTurret + " deg)"); } + @Test public void testBumpTranslationSnap() { DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); From e2908f29d24fe8d3cd92d925b5487377c4099406 Mon Sep 17 00:00:00 2001 From: Jace Rodgers Date: Tue, 13 Oct 2026 18:12:55 -0500 Subject: [PATCH 4/5] fix leds and more!!! --- .../java/frc/robot/AutoCommandFactory.java | 4 +- src/main/java/frc/robot/CommandFactory.java | 11 +- src/main/java/frc/robot/Constants.java | 27 ++++ src/main/java/frc/robot/FieldConstants.java | 13 ++ src/main/java/frc/robot/RobotContainer.java | 7 +- .../robot/localization/CameraProcessor.java | 51 +++---- .../robot/localization/DrivetrainState.java | 62 ++++++-- .../robot/localization/RejectionReason.java | 3 +- .../localization/TurretCameraAdapter.java | 55 ++++++- .../robot/localization/VisionObservation.java | 16 +- .../frc/robot/subsystems/turret/Turret.java | 138 +++++++++++++++++- .../frc/robot/subsystems/vision/Vision.java | 43 +++++- .../localization/TurretLocalizationTest.java | 121 ++++++++++++++- 13 files changed, 482 insertions(+), 69 deletions(-) diff --git a/src/main/java/frc/robot/AutoCommandFactory.java b/src/main/java/frc/robot/AutoCommandFactory.java index 62542da0..86b3927f 100644 --- a/src/main/java/frc/robot/AutoCommandFactory.java +++ b/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())) diff --git a/src/main/java/frc/robot/CommandFactory.java b/src/main/java/frc/robot/CommandFactory.java index 91b60982..74e0a3bd 100644 --- a/src/main/java/frc/robot/CommandFactory.java +++ b/src/main/java/frc/robot/CommandFactory.java @@ -54,12 +54,17 @@ public static Command preShoot(Supplier 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. */ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0ccfe1e6..66c88b1a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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)); @@ -593,6 +611,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); } /** Shooter Constants */ diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 19197826..3f9c159e 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/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 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 = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3d246d3f..9c4ac0b3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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); diff --git a/src/main/java/frc/robot/localization/CameraProcessor.java b/src/main/java/frc/robot/localization/CameraProcessor.java index 4c06c726..5e97058a 100644 --- a/src/main/java/frc/robot/localization/CameraProcessor.java +++ b/src/main/java/frc/robot/localization/CameraProcessor.java @@ -1,22 +1,16 @@ package frc.robot.localization; -import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Seconds; import java.util.List; import org.jspecify.annotations.Nullable; import org.littletonrobotics.junction.Logger; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import edu.wpi.first.math.geometry.Pose2d; 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.kinematics.ChassisSpeeds; import frc.robot.Constants; import frc.robot.FieldConstants; -import frc.robot.math.geometry.Penetration; -import frc.robot.math.geometry.Rectangle; -import frc.robot.math.geometry.SeparatingAxis; import frc.robot.subsystems.vision.CameraConstants; /** @@ -38,16 +32,6 @@ public class CameraProcessor { private final CameraConstants cameraConstants; - private final Rectangle robotRect = new Rectangle("pose", Pose2d.kZero, - Constants.Swerve.bumperFront.in(Meters) * 2, Constants.Swerve.bumperRight.in(Meters) * 2); - private final Rectangle[] hubRects = new Rectangle[] { - new Rectangle("hub", new Pose2d(FieldConstants.Hub.centerHub, Rotation2d.kZero), - FieldConstants.Hub.width, FieldConstants.Hub.width), - new Rectangle("oppHub", - new Pose2d(FieldConstants.fieldLength - FieldConstants.Hub.centerHub.getX(), - FieldConstants.Hub.centerHub.getY(), Rotation2d.kZero), - FieldConstants.Hub.width, FieldConstants.Hub.width)}; - private final Penetration hubPenetration = new Penetration("HubPen"); /** * Adapter that resolves the time-varying robot-to-camera transform for a turreted camera. @@ -96,7 +80,17 @@ public Result process(PhotonPipelineResult r Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); double rotationSpeed = Math.abs(currentSpeeds.omegaRadiansPerSecond); + double turretHeadingStdDev = 0.0; if (adapter != null) { + if (adapter.isWhippingAt(result.getTimestampSeconds())) { + return Result.err(RejectionReason.TURRET_WHIPPING); + } + 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()) { @@ -134,24 +128,13 @@ public Result process(PhotonPipelineResult r Logger.recordOutput("State/Camera/" + cameraConstants.name + "/estRobotPose", estRobotPose); - var estRobotPose2d = estRobotPose.toPose2d(); - robotRect.setPose(estRobotPose2d); - - // Reject if whole robot (including bumpers) extends outside field perimeter - if (Constants.keepInField) { - for (var corner : robotRect.getCorners()) { - if (corner.getX() < 0.0 || corner.getX() > FieldConstants.fieldLength - || corner.getY() < 0.0 || corner.getY() > FieldConstants.fieldWidth) { - return Result.err(RejectionReason.OUTSIDE_FIELD); - } - } + // 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); } - - // Reject if robot penetrates either hub - for (var hub : hubRects) { - if (SeparatingAxis.solve(robotRect, hub, hubPenetration)) { - return Result.err(RejectionReason.INSIDE_HUB); - } + if (FieldConstants.isInsideHub(estTranslation)) { + return Result.err(RejectionReason.INSIDE_HUB); } double stdDevMultiplier = stdDevMultiplier(result.targets, cameraPose); @@ -173,7 +156,7 @@ public Result process(PhotonPipelineResult r return Result.ok(new VisionObservation(cameraPose, robotToCamera_, translationStdDev, rotationStdDev, result.getTimestampSeconds(), cameraConstants.isTurret, - cameraConstants.name, isReal)); + cameraConstants.name, isReal, turretHeadingStdDev)); } return Result.err(RejectionReason.SINGLE_TAG_ONLY); diff --git a/src/main/java/frc/robot/localization/DrivetrainState.java b/src/main/java/frc/robot/localization/DrivetrainState.java index ec854342..baaf08f1 100644 --- a/src/main/java/frc/robot/localization/DrivetrainState.java +++ b/src/main/java/frc/robot/localization/DrivetrainState.java @@ -18,7 +18,9 @@ import edu.wpi.first.units.measure.Angle; import frc.robot.Constants; import frc.robot.FieldConstants; +import frc.robot.math.geometry.Penetration; import frc.robot.math.geometry.Rectangle; +import frc.robot.math.geometry.SeparatingAxis; import frc.robot.subsystems.swerve.Swerve; import frc.robot.subsystems.swerve.util.SwerveArcOdometry; @@ -33,7 +35,7 @@ public class DrivetrainState { private Rotation2d gyroOffset = Rotation2d.kZero; private Rotation2d prevGyroReading = Rotation2d.kZero; - private ChassisSpeeds currentSpeeds; + private ChassisSpeeds currentSpeeds = new ChassisSpeeds(); private double lastTimeMoved = 0.0; /** @@ -45,7 +47,7 @@ public class DrivetrainState { public DrivetrainState(SwerveModulePosition[] wheelPositions, Rotation2d gyroYaw) { prevGyroReading = gyroYaw; SwerveDriveOdometry swerveOdometry = - new SwerveArcOdometry(Constants.Swerve.swerveKinematics, gyroYaw, wheelPositions); + new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, gyroYaw, wheelPositions); visionAdjustedOdometry = new PoseEstimator<>(Constants.Swerve.swerveKinematics, swerveOdometry, VecBuilder.fill(0.1, 0.1, 0.1), VecBuilder.fill(0.9, 0.9, 0.9)); } @@ -112,7 +114,7 @@ public void addOdometryObservation(SwerveModulePosition[] wheelPositions, Rotati Logger.recordOutput("State/isOnBump", false); } Logger.recordOutput("State/nextRot", getGlobalPoseEstimate().getRotation()); - if (Constants.keepInField) { + if (Constants.keepInField || Constants.keepOutOfHubs) { limitPosition(getGlobalPoseEstimate(), visionAdjustedOdometry::resetPose); } } @@ -182,11 +184,15 @@ public void addVisionObservation(VisionObservation observations) { Logger.recordOutput("State/Camera/" + camName + "/lastMoved", this.lastTimeMoved); Logger.recordOutput("State/Camera/" + camName + "/timestamp", observations.timestamp()); - boolean onBump = observations.isReal() - && (FieldConstants.isOnBump(getGlobalPoseEstimate()) - || FieldConstants.isOnBump(robotPose)); + boolean stationaryReset = Constants.Vision.turretStationaryReset && isStationary; + // Odometry is unreliable on the bump and the estimator's history is reset there every + // loop (see addOdometryObservation), so snap directly to the vision translation. Check + // the vision pose too: slipping wheels can carry the estimate out of the bump area + // while the robot is still on it. + boolean onBump = FieldConstants.isOnBump(getGlobalPoseEstimate()) + || FieldConstants.isOnBump(robotPose); - if (isStationary || onBump) { + if (stationaryReset || (observations.isReal() && onBump)) { if (robotPose.getTranslation() .getSquaredDistance(getGlobalPoseEstimate().getTranslation()) > Math .pow(Units.inchesToMeters(3), 2)) { @@ -194,8 +200,13 @@ public void addVisionObservation(VisionObservation observations) { } } + // Turret angle timing is not good enough to trust heading while anything is moving. if (!isStationary) { rotationStdDev = 10000.0; + } else if (stationaryReset) { + rotationStdDev = Math.min(rotationStdDev, observations.turretHeadingStdDev()); + } else { + rotationStdDev = observations.turretHeadingStdDev(); } } @@ -234,8 +245,17 @@ public ChassisSpeeds getFieldRelativeSpeeds() { private final Rectangle robotRect = new Rectangle("pose", Pose2d.kZero, Constants.Swerve.bumperFront.in(Meters) * 2, Constants.Swerve.bumperRight.in(Meters) * 2); + private final Rectangle[] hubRects = new Rectangle[] { + new Rectangle("hub", new Pose2d(FieldConstants.Hub.centerHub, Rotation2d.kZero), + FieldConstants.Hub.width, FieldConstants.Hub.width), + new Rectangle("oppHub", + new Pose2d(FieldConstants.fieldLength - FieldConstants.Hub.centerHub.getX(), + FieldConstants.Hub.centerHub.getY(), Rotation2d.kZero), + FieldConstants.Hub.width, FieldConstants.Hub.width)}; + private final Penetration hubPenetration = new Penetration("HubPen"); + /** - * limits position of a given pose + * Limits the pose so the robot's bumpers stay inside the field walls and outside the hubs. * * @param pose new pose of robot reactangle * @param resetPose reset pose @@ -244,21 +264,37 @@ public void limitPosition(Pose2d pose, Consumer resetPose) { robotRect.setPose(pose); double offsetX = 0.0; double offsetY = 0.0; - var corners = robotRect.getCorners(); + if (Constants.keepOutOfHubs && !FieldConstants.isOnBump(pose)) { + for (var hub : hubRects) { + if (SeparatingAxis.solve(robotRect, hub, hubPenetration)) { + // The normal points from the hub toward the robot. + offsetX += hubPenetration.getXDir() * hubPenetration.getDepth(); + offsetY += hubPenetration.getYDir() * hubPenetration.getDepth(); + Logger.recordOutput("State/hubPushout", hubPenetration.getDepth()); + } + } + robotRect.setPose(new Pose2d(pose.getX() + offsetX, pose.getY() + offsetY, + pose.getRotation())); + } + double wallOffsetX = 0.0; + double wallOffsetY = 0.0; + var corners = Constants.keepInField ? robotRect.getCorners() : new Translation2d[0]; for (var corner : corners) { if (corner.getX() < 0) { - offsetX = Math.max(offsetX, -corner.getX()); + wallOffsetX = Math.max(wallOffsetX, -corner.getX()); } if (corner.getX() > FieldConstants.fieldLength) { - offsetX = Math.min(offsetX, FieldConstants.fieldLength - corner.getX()); + wallOffsetX = Math.min(wallOffsetX, FieldConstants.fieldLength - corner.getX()); } if (corner.getY() < 0) { - offsetY = Math.max(offsetY, -corner.getY()); + wallOffsetY = Math.max(wallOffsetY, -corner.getY()); } if (corner.getY() > FieldConstants.fieldWidth) { - offsetY = Math.min(offsetY, FieldConstants.fieldWidth - corner.getY()); + wallOffsetY = Math.min(wallOffsetY, FieldConstants.fieldWidth - corner.getY()); } } + offsetX += wallOffsetX; + offsetY += wallOffsetY; if (Math.abs(offsetX) > 1e-3 || Math.abs(offsetY) > 1e-3) { resetPose.accept( diff --git a/src/main/java/frc/robot/localization/RejectionReason.java b/src/main/java/frc/robot/localization/RejectionReason.java index 87d61d4f..183881e5 100644 --- a/src/main/java/frc/robot/localization/RejectionReason.java +++ b/src/main/java/frc/robot/localization/RejectionReason.java @@ -2,5 +2,6 @@ /** Rejection Reasons For Vision */ public enum RejectionReason { - NO_TARGETS, SINGLE_TAG_ONLY, HIGH_AMBIGUITY, MISSING_TURRET_ANGLE, OUTSIDE_FIELD, INSIDE_HUB + NO_TARGETS, SINGLE_TAG_ONLY, HIGH_AMBIGUITY, MISSING_TURRET_ANGLE, OUTSIDE_FIELD, + TURRET_WHIPPING, INSIDE_HUB } diff --git a/src/main/java/frc/robot/localization/TurretCameraAdapter.java b/src/main/java/frc/robot/localization/TurretCameraAdapter.java index c542450b..80f14051 100644 --- a/src/main/java/frc/robot/localization/TurretCameraAdapter.java +++ b/src/main/java/frc/robot/localization/TurretCameraAdapter.java @@ -10,17 +10,70 @@ /** Turret Camera Adapter */ public class TurretCameraAdapter { private static final double BUFFER_SECONDS = 1.5; + private static final double RATE_HALF_WINDOW = 0.04; private final Translation3d turretCenter; private final TimeInterpolatableBuffer angleBuffer = TimeInterpolatableBuffer.createBuffer(BUFFER_SECONDS); + /** Between two samples, a frame counts as whipping if either neighbor was. */ + private final TimeInterpolatableBuffer whippingBuffer = + TimeInterpolatableBuffer.createBuffer((a, b, t) -> a || b, BUFFER_SECONDS); + private double lastHubTagTime = Double.NEGATIVE_INFINITY; public TurretCameraAdapter(Translation3d turretCenter) { this.turretCenter = turretCenter; } - public void recordTurretAngle(double timestamp, Rotation2d angle) { + /** + * Records a turret sample. + * + * @param timestamp sample time in seconds + * @param angle robot-relative turret angle + * @param whipping whether the turret is swinging the long way around to unwrap + */ + public void recordTurretAngle(double timestamp, Rotation2d angle, boolean whipping) { angleBuffer.addSample(timestamp, angle); + whippingBuffer.addSample(timestamp, whipping); + } + + public void recordTurretAngle(double timestamp, Rotation2d angle) { + recordTurretAngle(timestamp, angle, false); + } + + /** + * Records that a turret-camera frame arrived, whether or not it was accepted for fusion. Ends a + * turret search once a hub tag is seen. + * + * @param arrivalTime loop time the frame was received, in seconds + * @param sawHubTag whether the frame contained any hub tag + */ + public void recordFrame(double arrivalTime, boolean sawHubTag) { + if (sawHubTag) { + lastHubTagTime = Math.max(lastHubTagTime, arrivalTime); + } + } + + /** Loop time the last turret-camera frame containing a hub tag arrived. */ + public double getLastHubTagTime() { + return lastHubTagTime; + } + + /** Whether the turret was whipping around at {@code timestamp}. */ + boolean isWhippingAt(double timestamp) { + return whippingBuffer.getSample(timestamp).orElse(false); + } + + /** + * Turret angular rate (rad/s) around {@code timestamp}. Vision latency means samples after the + * frame are normally available, so this is a central difference. + */ + double getRateAt(double timestamp) { + var before = angleBuffer.getSample(timestamp - RATE_HALF_WINDOW); + var after = angleBuffer.getSample(timestamp + RATE_HALF_WINDOW); + if (before.isEmpty() || after.isEmpty()) { + return 0.0; + } + return after.get().minus(before.get()).getRadians() / (2 * RATE_HALF_WINDOW); } public Optional getRobotToCameraAt(Transform3d turretToCamera, double timestamp) { diff --git a/src/main/java/frc/robot/localization/VisionObservation.java b/src/main/java/frc/robot/localization/VisionObservation.java index 7632ad63..2c88cd6d 100644 --- a/src/main/java/frc/robot/localization/VisionObservation.java +++ b/src/main/java/frc/robot/localization/VisionObservation.java @@ -6,10 +6,22 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; -/** Vision Observation Record */ +/** + * Vision Observation Record + * + * @param isTurret whether the observation came from a turret-mounted camera + * @param turretHeadingStdDev heading std-dev (radians) from the turret motion model + */ public record VisionObservation(Pose3d cameraPose, Transform3d robotToCamera, double translationStdDev, double rotationStdDev, double timestamp, boolean isTurret, - String cameraName, boolean isReal) { + String cameraName, boolean isReal, double turretHeadingStdDev) { + + public VisionObservation(Pose3d cameraPose, Transform3d robotToCamera, + double translationStdDev, double rotationStdDev, double timestamp, boolean isTurret, + String cameraName, boolean isReal) { + this(cameraPose, robotToCamera, translationStdDev, rotationStdDev, timestamp, isTurret, + cameraName, isReal, rotationStdDev); + } public Vector getStdDev() { return VecBuilder.fill(translationStdDev, translationStdDev, rotationStdDev); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index d8674f21..169c2df7 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.turret; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; +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; @@ -11,7 +13,11 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -33,6 +39,14 @@ public class Turret extends SubsystemBase { new TurretCameraAdapter(Constants.Vision.turretCenter.getTranslation()); private final DrivetrainState state; + /** Last commanded robot-relative setpoint in rotations, or NaN if not position-controlled. */ + private double lastSetpoint = Double.NaN; + private boolean whipping = false; + private double whipStart = 0.0; + private double prevAngle = Double.NaN; + private double prevAngleTime = 0.0; + private final LinearFilter rateFilter = LinearFilter.movingAverage(5); + /** * Creates a new Turret subsystem. * @@ -55,13 +69,45 @@ public void periodic() { Constants.Turret.pid.ifDirty(io::setPID); Logger.recordOutput("Turret/currentAngle", inputs.relativeAngle); + double now = Timer.getTimestamp(); + double rate = 0.0; + if (!Double.isNaN(prevAngle) && now > prevAngleTime) { + rate = rateFilter + .calculate(Math.abs(inputs.relativeAngle - prevAngle) / (now - prevAngleTime)); + } + prevAngle = inputs.relativeAngle; + prevAngleTime = now; + // The whip ends when the turret reaches its setpoint, stops moving (it may never reach an + // unreachable or stale setpoint), or runs out of time. + if (whipping && (Double.isNaN(lastSetpoint) + || Math.abs(lastSetpoint - inputs.relativeAngle) < WHIP_SETTLED.in(Rotations) + || (now - whipStart > WHIP_GRACE && rate < WHIP_STOPPED.in(RotationsPerSecond)) + || now - whipStart > WHIP_TIMEOUT)) { + whipping = false; + } + Logger.recordOutput("Turret/isWhipping", whipping); + Logger.recordOutput("Turret/searching", searching); for (int i = 0; i < inputs.timestamps.length; i++) { adapter.recordTurretAngle(inputs.timestamps[i], - new Rotation2d(Rotations.of(inputs.angleRotations[i]))); + new Rotation2d(Rotations.of(inputs.angleRotations[i])), whipping); state.setTurretRawAngle(inputs.timestamps[i], Rotations.of(inputs.angleRotations[i])); } } + private static final Angle WHIP_SETTLED = Degrees.of(10); + private static final AngularVelocity WHIP_STOPPED = DegreesPerSecond.of(45); + /** Seconds after a setpoint wrap before a stopped turret ends the whip. */ + private static final double WHIP_GRACE = 0.2; + private static final double WHIP_TIMEOUT = 1.5; + + /** + * Whether the turret is swinging the long way around because its setpoint wrapped past a travel + * limit. Shooting and turret-camera vision should be paused while this is true. + */ + public boolean isWhipping() { + return whipping; + } + public Rotation2d getTurretHeading() { return Rotation2d.fromRotations(this.inputs.relativeAngle); } @@ -84,6 +130,7 @@ public Command setVoltage(DoubleSupplier voltage) { } public void setVoltageIO(DoubleSupplier voltage) { + lastSetpoint = Double.NaN; io.setTurretVoltage(Volts.of(voltage.getAsDouble())); } @@ -100,6 +147,26 @@ public boolean setGoalRobotRelative(Rotation2d targetAngle, AngularVelocity velo if (normalized.gt(Constants.Turret.maxAngle)) { normalized = normalized.minus(Rotations.of(1)); } + double setpoint = normalized.in(Rotations); + // Prevent wrap chatter at +/- 180 degrees: if the target is hovering near the rear limit, + // stay on the side of the limit matching the turret's current position to avoid whipping back and forth. + if (Math.abs(setpoint) > 0.48) { + double current = inputs.relativeAngle; + if (current > 0 && setpoint < 0) { + setpoint = 0.5; + normalized = Rotations.of(0.5); + } else if (current < 0 && setpoint > 0) { + setpoint = -0.5; + normalized = Rotations.of(-0.5); + } + } + if (!Double.isNaN(lastSetpoint) && Math.abs(setpoint - lastSetpoint) > 0.5) { + if (!whipping) { + whipping = true; + whipStart = Timer.getTimestamp(); + } + } + lastSetpoint = setpoint; io.setTargetAngle(normalized, velocity); return true; } @@ -111,6 +178,74 @@ public boolean setGoalFieldRelative(Rotation2d targetAngle) { RadiansPerSecond.of(-state.getFieldRelativeSpeeds().omegaRadiansPerSecond)); } + private static final Angle SEARCH_LIMIT_MARGIN = Degrees.of(5); + private boolean searching = false; + private boolean shooting = false; + + /** Whether the turret is sweeping to look for tags. */ + public boolean isSearching() { + return searching; + } + + /** + * Mark whether a shot is in progress. Shooting ends any search. + * + * @param shooting whether the robot is shooting + */ + public void setShooting(boolean shooting) { + this.shooting = shooting; + } + + /** + * Whether the turret is where it was told to go: not whipping around, not searching, and within + * {@link Constants.Turret#aimTolerance} of its setpoint. + */ + public boolean isAimed() { + return !whipping && !searching && !Double.isNaN(lastSetpoint) + && Math.abs(lastSetpoint - inputs.relativeAngle) < Constants.Turret.aimTolerance + .in(Rotations); + } + + /** + * Sweep the turret's full travel in the robot frame to look for hub tags, for when the pose + * estimate has drifted far enough that normal aiming never points the camera at them. The sweep + * ignores the pose estimate, since anything anchored to a wrong heading would miss the tags. + * Ends as soon as a hub tag is seen (even one too poor to fuse) or a shot starts. + */ + public Command search() { + double[] target = new double[1]; + double[] direction = new double[1]; + double[] lastTime = new double[1]; + double[] start = new double[1]; + double rate = Constants.Turret.searchRate.in(RadiansPerSecond); + // Stay inside the limits: -180 deg normalizes to +180 deg, which is itself a wrap. + double maxTarget = Constants.Turret.maxAngle.minus(SEARCH_LIMIT_MARGIN).in(Radians); + double minTarget = Constants.Turret.minAngle.plus(SEARCH_LIMIT_MARGIN).in(Radians); + return runOnce(() -> { + searching = true; + start[0] = Timer.getTimestamp(); + lastTime[0] = start[0]; + // Start from where the turret is, heading toward the limit with more travel left. + target[0] = MathUtil.clamp(getTurretHeading().getRadians(), minTarget, maxTarget); + direction[0] = maxTarget - target[0] >= target[0] - minTarget ? 1.0 : -1.0; + }).andThen(run(() -> { + double now = Timer.getTimestamp(); + target[0] += direction[0] * rate * Math.min(now - lastTime[0], 0.1); + lastTime[0] = now; + if (target[0] >= maxTarget) { + target[0] = maxTarget; + direction[0] = -1.0; + } else if (target[0] <= minTarget) { + target[0] = minTarget; + direction[0] = 1.0; + } + Logger.recordOutput("Turret/searchTargetDeg", Units.radiansToDegrees(target[0])); + setGoalRobotRelative(new Rotation2d(target[0]), + RadiansPerSecond.of(direction[0] * rate)); + })).until(() -> shooting || adapter.getLastHubTagTime() > start[0]) + .finallyDo(() -> searching = false); + } + /** Aim turret in robot frame */ public Command goToAngleRobotRelative(Supplier rotations) { return run(() -> this.setGoalRobotRelative(rotations.get(), RotationsPerSecond.of(0))); @@ -136,6 +271,7 @@ public Command characterization() { return Commands.sequence( // Reset data this.runOnce(() -> { + lastSetpoint = Double.NaN; velocitySamples.clear(); voltageSamples.clear(); }), diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 386ef9ef..2bab4097 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -9,10 +9,12 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; +import frc.robot.FieldConstants; import frc.robot.localization.CameraProcessor; import frc.robot.localization.CameraProcessor.Result; import frc.robot.localization.DrivetrainState; @@ -51,9 +53,14 @@ public class Vision extends SubsystemBase { private final String[] cameraVizKeys; private final boolean[] cameraContributed; private final String[] cameraContributedKeys; - private boolean seesMultitag; - public Trigger seesTwoAprilTags = - new Trigger(() -> twoAprilTags()).debounce(.3, Debouncer.DebounceType.kBoth); + private long turretFrameCount = 0; + private long lastTurretTagFrame = -1; + private long lastTurretMultitagFrame = -1; + private double lastTurretFrameTimestamp = Double.NEGATIVE_INFINITY; + public final Trigger seesTurretTag = + new Trigger(this::seesTurretTag).debounce(.3, Debouncer.DebounceType.kBoth); + public final Trigger seesTwoAprilTags = + new Trigger(this::twoAprilTags).debounce(.3, Debouncer.DebounceType.kBoth); private final CameraProcessor[] cameraProcessors; private final Result[] processorResults; private final TurretCameraAdapter adapter; @@ -114,6 +121,19 @@ public void periodic() { cameraViz[i] = new Translation3d[0]; } for (var result : results) { + if (Constants.Vision.cameraConstants[result._0()].isTurret) { + turretFrameCount++; + lastTurretFrameTimestamp = Timer.getTimestamp(); + boolean sawHubTag = result._1().targets.stream() + .anyMatch(target -> FieldConstants.isHubTag(target.fiducialId)); + adapter.recordFrame(result._1().getTimestampSeconds(), sawHubTag); + if (!result._1().targets.isEmpty()) { + lastTurretTagFrame = turretFrameCount; + } + if (result._1().multitagResult.isPresent()) { + lastTurretMultitagFrame = turretFrameCount; + } + } processorResults[result._0()] = cameraProcessors[result._0()].process( result._1(), state.getFieldRelativeSpeeds(), cameraInputs[result._0()].isReal); if (processorResults[result._0()] instanceof CameraProcessor.Err err) { @@ -125,9 +145,6 @@ public void periodic() { var obs = (VisionObservation) ok.value(); state.addVisionObservation(obs); } - if (result._0() == 0 && result._1().multitagResult.isPresent()) { - seesMultitag = true; - } for (int i = 0; i < result._1().targets.size(); i++) { var robotToCamera = Constants.Vision.cameraConstants[result._0()].robotToCamera; if (Constants.Vision.cameraConstants[result._0()].isTurret) { @@ -163,6 +180,8 @@ public void periodic() { } } SmartDashboard.putBoolean("Vision/seesAprilTag", seesAprilTag); + Logger.recordOutput("Vision/seesTurretTag", seesTurretTag()); + Logger.recordOutput("Vision/seesMultitag", seesMultitag()); } private Translation3d[] addTwo(Translation3d[] translations, Translation3d newTranslation1, @@ -174,8 +193,18 @@ private Translation3d[] addTwo(Translation3d[] translations, Translation3d newTr return res; } + public boolean seesTurretTag() { + double now = Timer.getTimestamp(); + return lastTurretTagFrame == turretFrameCount && (now - lastTurretFrameTimestamp) < 0.25; + } + + public boolean seesMultitag() { + double now = Timer.getTimestamp(); + return lastTurretMultitagFrame == turretFrameCount && (now - lastTurretFrameTimestamp) < 0.25; + } + public boolean twoAprilTags() { - return seesMultitag; + return seesMultitag(); } } diff --git a/src/test/java/frc/robot/localization/TurretLocalizationTest.java b/src/test/java/frc/robot/localization/TurretLocalizationTest.java index 5061b0b9..683e1dd4 100644 --- a/src/test/java/frc/robot/localization/TurretLocalizationTest.java +++ b/src/test/java/frc/robot/localization/TurretLocalizationTest.java @@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Meters; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotNull; import static org.junit.jupiter.api.Assertions.assertTrue; import java.io.File; @@ -37,6 +38,8 @@ import frc.robot.math.geometry.SeparatingAxis; import frc.robot.subsystems.vision.CameraConstants; import frc.robot.subsystems.vision.CameraConstantsBuilder; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIO; public class TurretLocalizationTest { @@ -186,7 +189,7 @@ public void testStationaryAndMovingRotationStdDev() { Pose3d cameraPose = new Pose3d(2.0, 2.0, 0.5, new Rotation3d(0, 0, Math.toRadians(45))); Transform3d robotToCamera = new Transform3d(); VisionObservation movingTurretObs = new VisionObservation( - cameraPose, robotToCamera, 0.1, 0.05, t0, true, "turret", true); + cameraPose, robotToCamera, 0.1, 0.05, t0, true, "turret", true, 10000.0); state.addVisionObservation(movingTurretObs); @@ -200,7 +203,7 @@ public void testStationaryAndMovingRotationStdDev() { state.addOdometryObservation(getZeroModulePositions(), Rotation2d.kZero, t1, true); VisionObservation stationaryTurretObs = new VisionObservation( - cameraPose, robotToCamera, 0.1, 0.05, t1, true, "turret", true); + cameraPose, robotToCamera, 0.1, 0.05, t1, true, "turret", true, 0.05); state.addVisionObservation(stationaryTurretObs); @@ -354,4 +357,118 @@ public void testCurieMatchLogIfPresent() { assertTrue(totalFrames > 0, "Curie Q119 match log should have frames"); assertTrue(turretMovedFrames > 0, "Curie Q119 match should have instances where turret moved"); } + + @Test + public void testAdapterRateAndWhipping() { + TurretCameraAdapter adapter = new TurretCameraAdapter(new Translation3d(0, 0, 0)); + double t0 = 1.0; + adapter.recordTurretAngle(t0 - 0.04, Rotation2d.fromDegrees(0), false); + adapter.recordTurretAngle(t0 + 0.04, Rotation2d.fromDegrees(8), false); + + double rate = adapter.getRateAt(t0); + // Rate is (8 deg in rad) / (2 * 0.04s) = 0.1396 / 0.08 ~ 1.745 rad/s + assertEquals(Math.toRadians(8) / 0.08, rate, 1e-3, "Calculated central difference rate should match"); + assertFalse(adapter.isWhippingAt(t0), "Should not be whipping at t0"); + + // Record whipping at t1 + double t1 = 2.0; + adapter.recordTurretAngle(t1, Rotation2d.fromDegrees(45), true); + assertTrue(adapter.isWhippingAt(t1), "Should be whipping at t1"); + + // Hub tag recording + assertFalse(adapter.getLastHubTagTime() > 0); + adapter.recordFrame(2.5, true); + assertEquals(2.5, adapter.getLastHubTagTime(), 1e-6); + assertTrue(FieldConstants.isHubTag(26)); + assertFalse(FieldConstants.isHubTag(1)); + } + + @Test + public void testLimitPositionHubPushout() { + DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); + // Pose penetrating the alliance hub + Pose2d penetratingPose = new Pose2d(FieldConstants.Hub.centerHub, Rotation2d.kZero); + Pose2d[] pushedPose = new Pose2d[1]; + state.limitPosition(penetratingPose, p -> pushedPose[0] = p); + + assertNotNull(pushedPose[0], "Limit position should push out a pose penetrating the hub"); + // Verify pushed pose moved away from the hub center + double dist = pushedPose[0].getTranslation().getDistance(FieldConstants.Hub.centerHub); + assertTrue(dist > 0.5, "Pushed pose should be pushed out by bumper radius"); + } + + @Test + public void testBumpCrossingPushout() { + DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); + // Test positions along LeftBump (crossing in X at Y = 5.0) + double hubX = FieldConstants.Hub.centerHub.getX(); + for (double x = hubX - 0.5; x <= hubX + 0.5; x += 0.1) { + Pose2d pose = new Pose2d(x, 5.0, Rotation2d.kZero); + Pose2d[] pushed = new Pose2d[1]; + state.limitPosition(pose, p -> pushed[0] = p); + assertTrue(FieldConstants.isOnBump(pose), "Pose should be recognized as on the bump"); + // While on the bump, robot should not be pushed backwards + assertNotNull(pose); + } + } + + @Test + public void testBumpOdometryMotion() { + DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); + // Start before bump at X = 3.5, Y = 5.0 + state.overrideInit(new Pose2d(3.5, 5.0, Rotation2d.kZero)); + + double dist = 0.0; + double t = 0.0; + for (int i = 0; i < 50; i++) { // 1 second of driving forward at 2 m/s + t += 0.02; + dist += 2.0 * 0.02; + SwerveModulePosition[] pos = new SwerveModulePosition[] { + new SwerveModulePosition(dist, Rotation2d.kZero), + new SwerveModulePosition(dist, Rotation2d.kZero), + new SwerveModulePosition(dist, Rotation2d.kZero), + new SwerveModulePosition(dist, Rotation2d.kZero) + }; + state.addOdometryObservation(pos, Rotation2d.kZero, t, true); + } + assertTrue(state.getGlobalPoseEstimate().getX() > 4.5, + "Odometry should successfully advance past the bump center"); + } + + @Test + public void testVisionSeesTurretTagTogglesCorrectly() { + DrivetrainState state = new DrivetrainState(getZeroModulePositions(), Rotation2d.kZero); + TurretCameraAdapter adapter = + new TurretCameraAdapter(Constants.Vision.turretCenter.getTranslation()); + class TestVisionIO implements VisionIO { + public PhotonPipelineResult[] results = new PhotonPipelineResult[0]; + + @Override + public void updateInputs(CameraInputs[] inputs) { + if (inputs.length > 0) { + inputs[0].results = results; + } + } + } + TestVisionIO testIO = new TestVisionIO(); + Vision vision = new Vision(state, testIO, adapter); + + assertFalse(vision.seesTurretTag(), "Should not see tag initially"); + + // Simulate frame with a target from turret camera + PhotonTrackedTarget target = new PhotonTrackedTarget(); + target.fiducialId = 1; + testIO.results = new PhotonPipelineResult[] { + new PhotonPipelineResult(0L, 0L, 0L, 1L, List.of(target)) + }; + vision.periodic(); + assertTrue(vision.seesTurretTag(), "Should see turret tag after receiving frame with target"); + + // Simulate frame with no targets + testIO.results = new PhotonPipelineResult[] { + new PhotonPipelineResult(0L, 0L, 0L, 2L, List.of()) + }; + vision.periodic(); + assertFalse(vision.seesTurretTag(), "Should no longer see turret tag when frame has no targets"); + } } From 0e79a1dcf9a249d94ffe324c2c6fb13d03929dee Mon Sep 17 00:00:00 2001 From: Jace Rodgers Date: Tue, 13 Oct 2026 18:27:05 -0500 Subject: [PATCH 5/5] Fix Checkstyle Javadoc and whitespace warnings --- .../localization/TurretCameraAdapter.java | 30 +++++++++++++++++++ .../localization/TurretLocalizationTest.java | 10 +++++++ 2 files changed, 40 insertions(+) diff --git a/src/main/java/frc/robot/localization/TurretCameraAdapter.java b/src/main/java/frc/robot/localization/TurretCameraAdapter.java index 12ec12dc..0c102278 100644 --- a/src/main/java/frc/robot/localization/TurretCameraAdapter.java +++ b/src/main/java/frc/robot/localization/TurretCameraAdapter.java @@ -20,6 +20,11 @@ public class TurretCameraAdapter { private double lastHubTagTime = Double.NEGATIVE_INFINITY; + /** + * Creates a new adapter for resolving camera transforms on the moving turret. + * + * @param turretCenter robot-relative translation to the turret's center of rotation + */ public TurretCameraAdapter(Translation3d turretCenter) { this.turretCenter = turretCenter; } @@ -36,14 +41,32 @@ public void recordTurretAngle(double timestamp, Rotation2d angle, boolean whippi whippingBuffer.addSample(timestamp, whipping); } + /** + * Records a turret sample assuming the turret is not whipping. + * + * @param timestamp sample time in seconds + * @param angle robot-relative turret angle + */ public void recordTurretAngle(double timestamp, Rotation2d angle) { recordTurretAngle(timestamp, angle, false); } + /** + * Records the whipping state of the turret. + * + * @param timestamp sample time in seconds + * @param isWhipping whether the turret is swinging the long way around to unwrap + */ public void recordTurretWhipping(double timestamp, boolean isWhipping) { whippingBuffer.addSample(timestamp, isWhipping); } + /** + * Gets the whipping state of the turret at a given timestamp. + * + * @param timestamp time in seconds + * @return optional containing whether the turret was whipping, or empty if no data + */ public Optional getTurretWhipping(double timestamp) { return whippingBuffer.getSample(timestamp); } @@ -84,6 +107,13 @@ public double getRateAt(double timestamp) { return after.get().minus(before.get()).getRadians() / (2 * RATE_HALF_WINDOW); } + /** + * Computes the robot-to-camera transform at the specified timestamp. + * + * @param turretToCamera fixed transform from the turret frame to the camera + * @param timestamp capture time in seconds + * @return optional containing the robot-to-camera transform, or empty if angle data unavailable + */ public Optional getRobotToCameraAt(Transform3d turretToCamera, double timestamp) { var maybeTurretRotation = angleBuffer.getSample(timestamp); if (maybeTurretRotation.isEmpty()) { diff --git a/src/test/java/frc/robot/localization/TurretLocalizationTest.java b/src/test/java/frc/robot/localization/TurretLocalizationTest.java index 683e1dd4..24ce3a66 100644 --- a/src/test/java/frc/robot/localization/TurretLocalizationTest.java +++ b/src/test/java/frc/robot/localization/TurretLocalizationTest.java @@ -41,17 +41,26 @@ import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; +/** + * Unit tests verifying turret localization, geometry constraints, and vision tracking behavior. + */ public class TurretLocalizationTest { private CameraConstants turretCameraConstants; private CameraConstants backCameraConstants; private TurretCameraAdapter turretAdapter; + /** + * Initializes the Hardware Abstraction Layer prior to running test suite. + */ @BeforeAll public static void initializeHal() { HAL.initialize(500, 0); } + /** + * Sets up test fixtures before each test execution. + */ @BeforeEach public void setUp() { turretCameraConstants = new CameraConstantsBuilder() @@ -450,6 +459,7 @@ public void updateInputs(CameraInputs[] inputs) { } } } + TestVisionIO testIO = new TestVisionIO(); Vision vision = new Vision(state, testIO, adapter);