From 6010e39ed403371a813759863957782227d83604 Mon Sep 17 00:00:00 2001 From: tori lee <144298063+tortillini-tokki@users.noreply.github.com> Date: Sun, 29 Mar 2026 15:24:05 -0700 Subject: [PATCH 1/3] add robot speed pose ambiguity scalar - change robot RotationSupplier to SwerveDriveState for access to heading + speed - add isFront boolean, sets true for front cameras - multiply stdDev matrices by dynamic matrixScalar dependent on robot speed :) --- src/main/java/frc/robot/Constants.java | 3 ++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../subsystems/vision/LocalizationCamera.java | 33 +++++++++++++++---- .../subsystems/vision/VisionSubsystem.java | 12 ++++--- 4 files changed, 38 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ac61e873..8f717cc3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -500,6 +500,9 @@ public static class VisionConstants { public static final int NUM_LAST_EST_POSES = 3; public static final double STD_DEV_SCALAR = 30; + public static final double MAX_ROBOT_VISION_VELOCITY = 2.0; // untuned 3/29 + public static final double AMBIGUITY_MATRIX_SCALAR = 4.0; // untuned 3/29 + // --- vision subsystem --- // (camera setup) // DEFAULT CONSTANTS (robot specific constants are below) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af484d65..b4df1510 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -97,7 +97,7 @@ public boolean isMagnitudeGreaterThan(double threshold) { public final CommandSwerveDrivetrain m_drivetrain = TunerConstants.createDrivetrain(); public final RollerSubsystem m_roller = new RollerSubsystem(); public final ShooterSubsystem m_shooter = new ShooterSubsystem(); - public final VisionSubsystem m_vision = new VisionSubsystem(() -> m_drivetrain.getState().Pose.getRotation()); + public final VisionSubsystem m_vision = new VisionSubsystem(() -> m_drivetrain.getState()); public final IndexerSubsystem m_indexer = new IndexerSubsystem(); public final PivotSubsystem m_pivot = new PivotSubsystem(); public final ColumnSubsystem m_column = new ColumnSubsystem(); diff --git a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java index f27b5db0..eab94211 100644 --- a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,6 +26,8 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonTrackedTarget; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + import frc.robot.Constants.VisionConstants; public class LocalizationCamera { @@ -32,11 +35,12 @@ public class LocalizationCamera { private final PhotonCamera m_camera; private final String m_cameraName; private final String m_logString; + private final boolean m_isFront; // boolean that stores camera location (front or back) private AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); private final Field2d m_estPoseField = new Field2d(); // field pose estimator private PhotonPoseEstimator poseEstimator; - private final Supplier m_robotRotationSupplier; + private final Supplier m_robotSwerveStateSupplier; private LocalVisionFilterPipeline m_filterPipeline = new LocalVisionFilterPipeline(); // filtering pipeline for each camera, initalizes as empty pipeline @@ -50,11 +54,19 @@ public class LocalizationCamera { // every camera periodically creates a new CameraReading containing robot pose, std dev, timestamp, and number of targets seen. public static record CameraReading(String cameraName, EstimatedRobotPose robotPose, Matrix stdDevs, double timestampSeconds, int numTargets) {} - public LocalizationCamera(String cameraName, Transform3d robotToCam, Supplier robotHeadingSupplier) { + public LocalizationCamera(String cameraName, Transform3d robotToCam, Supplier robotSwerveStateSupplier) { m_cameraName = cameraName; m_camera = new PhotonCamera(m_cameraName); m_logString = "vision/" + m_cameraName; - m_robotRotationSupplier = robotHeadingSupplier; + m_robotSwerveStateSupplier = robotSwerveStateSupplier; + + // boolean that stores camera location + if (m_cameraName.indexOf("front") == -1) { + m_isFront = false; + } + else { + m_isFront = true; + } poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam); @@ -229,7 +241,7 @@ private Optional resolveHighAmbiguityPose( return Optional.empty(); } - Rotation2d currentHeading = m_robotRotationSupplier.get(); + Rotation2d currentHeading = m_robotSwerveStateSupplier.get().Pose.getRotation(); Pose3d chosenPose = bestPose.get(); // Prefer the candidate whose field-relative heading is closer to the drivetrain heading. if (headingDistance(alternatePose.get().toPose2d().getRotation(), currentHeading) @@ -271,6 +283,8 @@ private Matrix calculateEstimationStdDevs(EstimatedRobotPose estimatedPo // Pose present. Start running Heuristic int numTags = 0; double avgDist = 0; + double matrixScalar = 1.0; + // Precalculation - see how many tags we found, and calculate an // average-distance metric @@ -286,10 +300,17 @@ private Matrix calculateEstimationStdDevs(EstimatedRobotPose estimatedPo .getDistance(estimatedPose.estimatedPose.toPose2d().getTranslation()); } + // if camera is in front and robot is above max velocity, update matrixScalar + double linearRobotSpeed = Math.hypot(m_robotSwerveStateSupplier.get().Speeds.vxMetersPerSecond, + m_robotSwerveStateSupplier.get().Speeds.vyMetersPerSecond); + if (m_isFront && linearRobotSpeed > VisionConstants.MAX_ROBOT_VISION_VELOCITY) { + matrixScalar = VisionConstants.AMBIGUITY_MATRIX_SCALAR; + } + if (numTags == 0) { // No tags visible. Default to single-tag std devs SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviation-state", "no tags visible"); - return VisionConstants.kSingleTagStdDevs; + return VisionConstants.kSingleTagStdDevs.times(matrixScalar); } else if (numTags == 1 && avgDist > VisionConstants.VISION_DISTANCE_DISCARD) { SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviation-state", "target too far"); return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); @@ -298,7 +319,7 @@ private Matrix calculateEstimationStdDevs(EstimatedRobotPose estimatedPo avgDist /= numTags; // increase std devs based on (average) distance SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviation-state", "good :)"); - return unscaledStdDevs.times(1 + (avgDist * avgDist / VisionConstants.STD_DEV_SCALAR)); + return unscaledStdDevs.times(1 + (avgDist * avgDist / VisionConstants.STD_DEV_SCALAR)).times(matrixScalar); } } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 97e76026..b82e87f2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -43,6 +44,7 @@ import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import frc.robot.Constants; import frc.robot.Constants.VisionConstants; @@ -65,11 +67,11 @@ public class VisionSubsystem extends SubsystemBase { private boolean m_rawVideoModeEnabled = false; // true = raw video feed, false = normal AprilTag processing /** Creates a new Vision Subsystem. */ - public VisionSubsystem(Supplier robotHeadingSupplier) { - cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D, robotHeadingSupplier)); - cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D, robotHeadingSupplier)); - cameras.add(new LocalizationCamera(VisionConstants.CAMERA3_NAME, VisionConstants.ROBOT_TO_CAM3_3D, robotHeadingSupplier)); - cameras.add(new LocalizationCamera(VisionConstants.CAMERA4_NAME, VisionConstants.ROBOT_TO_CAM4_3D, robotHeadingSupplier)); + public VisionSubsystem(Supplier robotSwerveStateSupplier) { + cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D, robotSwerveStateSupplier)); + cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D, robotSwerveStateSupplier)); + cameras.add(new LocalizationCamera(VisionConstants.CAMERA3_NAME, VisionConstants.ROBOT_TO_CAM3_3D, robotSwerveStateSupplier)); + cameras.add(new LocalizationCamera(VisionConstants.CAMERA4_NAME, VisionConstants.ROBOT_TO_CAM4_3D, robotSwerveStateSupplier)); } public List getValidCameraReadings(){ From ef83102bd3df198337b490769df949d9701abdee Mon Sep 17 00:00:00 2001 From: tori lee <144298063+tortillini-tokki@users.noreply.github.com> Date: Sun, 29 Mar 2026 15:24:21 -0700 Subject: [PATCH 2/3] auggie fix localizationCameraTest --- .../vision/LocalizationCameraTest.java | 36 +++++++++++++++++-- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java b/src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java index 843c4c20..2a37d0ec 100644 --- a/src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java +++ b/src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java @@ -10,6 +10,9 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; 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.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import org.junit.jupiter.api.AfterEach; @@ -17,6 +20,8 @@ import org.junit.jupiter.api.Test; import org.photonvision.EstimatedRobotPose; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.vision.LocalizationCamera; import frc.robot.subsystems.vision.LocalizationCamera.CameraReading; @@ -26,6 +31,7 @@ import java.lang.reflect.Field; import java.util.LinkedList; import java.util.List; +import java.util.function.Supplier; class LocalizationCameraTest { static final double DELTA = 1e-5; // acceptable deviation range @@ -33,19 +39,43 @@ class LocalizationCameraTest { LocalizationCamera m_camera; Transform3d m_robotToCam; + SwerveDriveState m_mockSwerveDriveState; + + /** + * Creates a mock SwerveDriveState for testing purposes. + * Returns a state with zero pose and zero velocity. + */ + private Supplier createMockSwerveDriveStateSupplier() { + m_mockSwerveDriveState = new SwerveDriveState(); + m_mockSwerveDriveState.Pose = new Pose2d(); + m_mockSwerveDriveState.Speeds = new ChassisSpeeds(); + m_mockSwerveDriveState.ModuleStates = new SwerveModuleState[] { + new SwerveModuleState(), new SwerveModuleState(), + new SwerveModuleState(), new SwerveModuleState() + }; + m_mockSwerveDriveState.ModuleTargets = new SwerveModuleState[] { + new SwerveModuleState(), new SwerveModuleState(), + new SwerveModuleState(), new SwerveModuleState() + }; + m_mockSwerveDriveState.ModulePositions = new SwerveModulePosition[] { + new SwerveModulePosition(), new SwerveModulePosition(), + new SwerveModulePosition(), new SwerveModulePosition() + }; + return () -> m_mockSwerveDriveState; + } @BeforeEach void setup() { // Initialize the HAL for simulation assert HAL.initialize(500, 0); - + // Create a test camera transform m_robotToCam = new Transform3d( new Translation3d(0.2, 0.0, 0.5), new Rotation3d(0, Math.toRadians(-15), 0) ); - - m_camera = new LocalizationCamera("test_camera", m_robotToCam, Rotation2d::new); + + m_camera = new LocalizationCamera("test_camera", m_robotToCam, createMockSwerveDriveStateSupplier()); } @AfterEach From c401a2de88d656539c1f6d0d8cf52dad5553839f Mon Sep 17 00:00:00 2001 From: tori lee <144298063+tortillini-tokki@users.noreply.github.com> Date: Sun, 29 Mar 2026 22:41:26 -0700 Subject: [PATCH 3/3] mitigate possible nullPointer errors! --- .../subsystems/vision/LocalizationCamera.java | 27 ++++++++++++++----- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java index a4d55000..39a55cb4 100644 --- a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java @@ -241,11 +241,18 @@ private Optional resolveHighAmbiguityPose( var bestPose = estimateSingleTagRobotPose(candidatePoseEstimator, target, target.getBestCameraToTarget()); var alternatePose = estimateSingleTagRobotPose(candidatePoseEstimator, target, target.getAlternateCameraToTarget()); + SwerveDriveState robotState = m_robotSwerveStateSupplier.get(); + + // null check for robot swerve state. If it's null, can't resolve heading. + if (robotState == null) { + return Optional.empty(); + } + if (bestPose.isEmpty() || alternatePose.isEmpty()) { return Optional.empty(); } - Rotation2d currentHeading = m_robotSwerveStateSupplier.get().Pose.getRotation(); + Rotation2d currentHeading = robotState.Pose.getRotation(); Pose3d chosenPose = bestPose.get(); // Prefer the candidate whose field-relative heading is closer to the drivetrain heading. if (headingDistance(alternatePose.get().toPose2d().getRotation(), currentHeading) @@ -304,13 +311,19 @@ private Matrix calculateEstimationStdDevs(EstimatedRobotPose estimatedPo .getDistance(estimatedPose.estimatedPose.toPose2d().getTranslation()); } - // if camera is in front and robot is above max velocity, update matrixScalar - double linearRobotSpeed = Math.hypot(m_robotSwerveStateSupplier.get().Speeds.vxMetersPerSecond, - m_robotSwerveStateSupplier.get().Speeds.vyMetersPerSecond); - if (m_isFront && linearRobotSpeed > VisionConstants.MAX_ROBOT_VISION_VELOCITY) { - matrixScalar = VisionConstants.AMBIGUITY_MATRIX_SCALAR; - } + // if robot is moving, want to trust front camera data less. + // NOTE: if state is null, matrixScalar stays at 1.0 + SwerveDriveState robotState = m_robotSwerveStateSupplier.get(); + if (robotState != null && robotState.Speeds != null) { // necessary null check + // if camera is in front and robot is above max velocity, update matrixScalar + double linearRobotSpeed = Math.hypot(robotState.Speeds.vxMetersPerSecond, + robotState.Speeds.vyMetersPerSecond); + if (m_isFront && linearRobotSpeed > VisionConstants.MAX_ROBOT_VISION_VELOCITY) { + matrixScalar = VisionConstants.AMBIGUITY_MATRIX_SCALAR; + } + } + if (numTags == 0) { // No tags visible. Default to single-tag std devs SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviationState", "no tags visible");