From aa4e46b900f9a70bba2f8fc8ac38750321a2123a Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 7 Feb 2026 12:38:27 -0500 Subject: [PATCH 01/67] Added vision truster from java2025 --- .../frc/robot/constants/GameConstants.java | 1 + .../vision/truster/BasicVisionFilter.java | 85 +++++++++++++++++++ .../vision/truster/ConstantVisionTruster.java | 17 ++++ .../vision/truster/DistanceVisionTruster.java | 16 ++++ .../vision/truster/FilterResult.java | 7 ++ .../vision/truster/LinearVisionTruster.java | 21 +++++ .../vision/truster/PoseDeviation.java | 23 +++++ .../vision/truster/SquareVisionTruster.java | 22 +++++ .../vision/truster/VisionFilter.java | 8 ++ .../vision/truster/VisionMeasurement.java | 13 +++ .../vision/truster/VisionTransformer.java | 8 ++ .../vision/truster/VisionTruster.java | 8 ++ 12 files changed, 229 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/DistanceVisionTruster.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/FilterResult.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/PoseDeviation.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTransformer.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index d431969f..a180580b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -86,4 +86,5 @@ public enum Mode { public static final int SHIFT_4_START = 55; public static final int ENDGAME_START = 30; + public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java new file mode 100644 index 00000000..b7ac8ff9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -0,0 +1,85 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import frc.robot.constants.Constants; +import java.util.LinkedHashMap; +import java.util.Optional; +import java.util.Queue; + +/** + * Vision filter implementation
+ * Keeps track of robot position over time
+ * Processes Vision measurements in batches of two
+ * Only allows vision measurements to be processed if their delta change in position is close to the + * delta change in position of the robot odometry + */ +public abstract class BasicVisionFilter implements VisionFilter, VisionTransformer { + + private final TimeInterpolatableBuffer poseBuffer; + + public BasicVisionFilter(TimeInterpolatableBuffer poseBuffer) { + this.poseBuffer = poseBuffer; + } + + @Override + public LinkedHashMap filter( + Queue measurements) { + LinkedHashMap resultMap = new LinkedHashMap<>(); + VisionMeasurement m1 = measurements.poll(); + VisionMeasurement m2 = measurements.peek(); + boolean processing = true; + do { + /* + ------------------------------------------------------- + Handle Null Case + ------------------------------------------------------- + */ + if (m1 == null) { + processing = false; + if (m2 != null) { + resultMap.put(m2, FilterResult.NOT_PROCESSED); + } + continue; + } else if (m2 == null) { + resultMap.put(m1, FilterResult.NOT_PROCESSED); + processing = false; + continue; + } + /* + ------------------------------------------------------- + Filter poses + ------------------------------------------------------- + */ + Pose2d vision1Pose = getVisionPose(m1); + Pose2d vision2Pose = getVisionPose(m2); + boolean valid1 = + filterVision(vision1Pose, vision2Pose, m1.timeOfMeasurement(), m2.timeOfMeasurement()); + resultMap.put(m1, valid1 ? FilterResult.ACCEPTED : FilterResult.REJECTED); + m1 = measurements.poll(); + m2 = measurements.peek(); + + } while (processing); + return resultMap; + } + + private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double m2Time) { + Optional odomPoseAtVis1 = poseBuffer.getSample(m1Time); + Optional odomPoseAtVis2 = poseBuffer.getSample(m2Time); + if (odomPoseAtVis1.isEmpty() || odomPoseAtVis2.isEmpty()) { + return false; + } + if (!inBounds(m1Pose) || !inBounds(m2Pose)) { + return false; + } + double odomDiff1To2 = + odomPoseAtVis1.get().getTranslation().getDistance(odomPoseAtVis2.get().getTranslation()); + double visionDiff1To2 = m1Pose.getTranslation().getDistance(m2Pose.getTranslation()); + double diff = Math.abs(odomDiff1To2 - visionDiff1To2); + return Math.abs(diff) <= Constants.VISION_CONSISTANCY_THRESHOLD; + } + + private boolean inBounds(Pose2d pose2d) { + return pose2d.getX() > 0 && pose2d.getX() < 20 && pose2d.getY() > 0 && pose2d.getY() < 20; + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java new file mode 100644 index 00000000..3017ccb1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; + + +public class ConstantVisionTruster extends DistanceVisionTruster { + + public ConstantVisionTruster(Vector initialSTD) { + super(initialSTD); + } + + @Override + public Vector calculateTrust(VisionMeasurement measurement) { + return initialSTD; + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/DistanceVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/DistanceVisionTruster.java new file mode 100644 index 00000000..7e254133 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/DistanceVisionTruster.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; + +public abstract class DistanceVisionTruster implements VisionTruster { + protected Vector initialSTD; + + public DistanceVisionTruster(Vector initialSTD) { + this.initialSTD = initialSTD; + } + + public void setInitialSTD(Vector initialSTD) { + this.initialSTD = initialSTD; + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/FilterResult.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/FilterResult.java new file mode 100644 index 00000000..23edac54 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/FilterResult.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +public enum FilterResult { + ACCEPTED, + REJECTED, + NOT_PROCESSED +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java new file mode 100644 index 00000000..e7f75ea6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; + +public class LinearVisionTruster extends DistanceVisionTruster { + + private final double slopeSTD; + + public LinearVisionTruster(Vector initialSTD, double slopeSTD) { + super(initialSTD); + this.slopeSTD = slopeSTD; + } + + @Override + public Vector calculateTrust(VisionMeasurement measurement) { + double std = measurement.distanceFromTag() * slopeSTD; + return initialSTD.plus(VecBuilder.fill(std, std, std)); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/PoseDeviation.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/PoseDeviation.java new file mode 100644 index 00000000..5ece4074 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/PoseDeviation.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; + +/** POJO containing wheel encoder standard deviation and vision standard deviation */ +public class PoseDeviation { + private final Vector wheelStd; + private final Vector visionStd; + + public PoseDeviation(Vector wheelStd, Vector visionStd) { + this.wheelStd = wheelStd; + this.visionStd = visionStd; + } + + public Vector getWheelStd() { + return wheelStd; + } + + public Vector getVisionStd() { + return visionStd; + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java new file mode 100644 index 00000000..7a925fc8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; + +public class SquareVisionTruster extends DistanceVisionTruster { + + private final double constant; + + public SquareVisionTruster(Vector initialSTD, double constant) { + super(initialSTD); + this.initialSTD = initialSTD; + this.constant = constant; + } + + @Override + public Vector calculateTrust(VisionMeasurement measurement) { + double std = Math.pow(measurement.distanceFromTag() - 0.4572, 2) * constant; + return initialSTD.plus(VecBuilder.fill(std, std, std)); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java new file mode 100644 index 00000000..e1b94831 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import java.util.LinkedHashMap; +import java.util.Queue; + +public interface VisionFilter { + LinkedHashMap filter(Queue measurements); +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java new file mode 100644 index 00000000..b981708a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.geometry.Pose2d; + +/** + * @param measurement estimated robot position calculated from apriltag tag what tag produced the + * position + * @param distanceFromTag distance estimated robot pose was from the tag + * @param timeOfMeasurement difference between the time the measurement was received by the robot + * program and when it was sent over the network + */ +public record VisionMeasurement( + Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTransformer.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTransformer.java new file mode 100644 index 00000000..9f92ed1c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTransformer.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.geometry.Pose2d; + + +public interface VisionTransformer { + Pose2d getVisionPose(VisionMeasurement measurement); +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java new file mode 100644 index 00000000..7ce7c3d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.swervedrive.vision.truster; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; + +public interface VisionTruster { + Vector calculateTrust(VisionMeasurement measurement); +} From 85f02d8a8f4324c7e46c3f0dddfe6b2982a6bcd9 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 7 Feb 2026 15:58:26 -0500 Subject: [PATCH 02/67] added pose estimator and amamger classes(broken without levs april tag code) --- .../java/frc/robot/constants/Constants.java | 5 +- .../swervedrive/SwerveSubsystem.java | 8 +- .../estimation/FilterablePoseManager.java | 86 ++++++++ .../vision/estimation/PoseEstimator.java | 202 ++++++++++++++++++ .../vision/estimation/PoseManager.java | 106 +++++++++ .../vision/truster/PoseDeviation.java | 10 +- .../java/frc/robot/utils/math/ArrayUtils.java | 79 +++++++ 7 files changed, 484 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java create mode 100644 src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java create mode 100644 src/main/java/frc/robot/utils/math/ArrayUtils.java diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 40168f6a..87edb1e2 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -13,5 +13,8 @@ * constants are needed, to reduce verbosity. */ -public class Constants extends Constants2026 {} +public class Constants extends Constants2026 { + + public static final boolean ENABLE_VISION = false; + public static final double POSE_BUFFER_STORAGE_TIME = 0;} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 5b9f2304..54b8ea00 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -47,7 +47,7 @@ public class SwerveSubsystem extends SubsystemBase { * Swerve drive object. */ private final SwerveDrive swerveDrive; - + private Vector variance; /** * Initialize {@link SwerveDrive} with the directory provided. * The SwerveIMU (which can be null) is the instance of the SwerveIMU to use. If non-null, @@ -465,8 +465,10 @@ public Rotation2d getPitch() { public SwerveDrive getSwerveDrive() { return swerveDrive; } + public void setVariance(Vector variance){ + this.variance = variance; + } public void addVisionMeasurement(Pose2d pose){ - double variance = 10; - swerveDrive.addVisionMeasurement(pose, Timer.getFPGATimestamp(), new Vector(N3.instance).plus(VecBuilder.fill(variance, variance, variance))); + swerveDrive.addVisionMeasurement(pose, Timer.getFPGATimestamp(), variance); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java new file mode 100644 index 00000000..3409fa1d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java @@ -0,0 +1,86 @@ +package frc.robot.subsystems.swervedrive.vision.estimation; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.numbers.N3; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.vision.truster.FilterResult; +import frc.robot.subsystems.swervedrive.vision.truster.PoseDeviation; +import frc.robot.subsystems.swervedrive.vision.truster.VisionFilter; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; + +import java.util.ArrayList; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; +import org.littletonrobotics.junction.Logger; + +/** + * A subclass of PoseManager that filters vision measurements before they are fed into the kalman + * filter. + */ +public class FilterablePoseManager extends PoseManager { + private final VisionFilter filter; + private final VisionTruster visionTruster; + + public FilterablePoseManager( + PoseDeviation PoseDeviation, + SwerveDriveKinematics kinematics, + SwerveSubsystem drivebase, + TimeInterpolatableBuffer estimatedPoseBuffer, + VisionFilter filter, + VisionTruster visionTruster) { + super(PoseDeviation, kinematics, drivebase, estimatedPoseBuffer); + this.filter = filter; + this.visionTruster = visionTruster; + } + + public FilterablePoseManager( + Vector visionStd, + SwerveDriveKinematics kinematics, + SwerveSubsystem drivebase, + TimeInterpolatableBuffer estimatedPoseBuffer, + VisionFilter filter, + VisionTruster visionTruster) { + this( + new PoseDeviation(visionStd), + kinematics, drivebase, + estimatedPoseBuffer, + filter, + visionTruster); + } + + @Override + public void processQueue() { + LinkedHashMap filteredData = + filter.filter(visionMeasurementQueue); + visionMeasurementQueue.clear(); + List validMeasurements = new ArrayList<>(); + List invalidMeasurements = new ArrayList<>(); + for (Map.Entry entry : filteredData.entrySet()) { + VisionMeasurement v = entry.getKey(); + FilterResult r = entry.getValue(); + switch (r) { + case ACCEPTED -> { + setVisionSTD(visionTruster.calculateTrust(v)); + validMeasurements.add(v.measurement()); + addVisionMeasurement(v); + } + case NOT_PROCESSED -> visionMeasurementQueue.add(v); + case REJECTED -> { + invalidMeasurements.add(v.measurement()); + } + } + } + Logger.recordOutput("Apriltag/acceptedMeasurements", validMeasurements.toArray(Pose2d[]::new)); + Logger.recordOutput( + "Apriltag/rejectedMeasurements", invalidMeasurements.toArray(Pose2d[]::new)); + } + + public VisionTruster getVisionTruster() { + return visionTruster; + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java new file mode 100644 index 00000000..bb2e25f2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -0,0 +1,202 @@ +package frc.robot.subsystems.swervedrive.vision.estimation; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; +import frc.robot.RobotMode; +import frc.robot.constants.Constants; +import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; +import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.utils.Apriltag; +import frc.robot.utils.math.ArrayUtils; + +import java.util.Arrays; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +/** + * Class in charge of feeding odometry and apriltag measurements from their respective IOs into a + * {@link PoseManager} which outputs a robot position + */ +public class PoseEstimator { + private final Field2d field = new Field2d(); + /* private final SwerveModule frontLeft; + private final SwerveModule frontRight; + private final SwerveModule backLeft; + private final SwerveModule backRight;*/ + // private final LoggableSystem, ApriltagInputs> apriltagSystem; + private int invalidCounter = 0; + + /* standard deviation of robot states, the lower the numbers arm, the more we trust odometry */ + public static final Vector stateStdDevs1 = VecBuilder.fill(0.075, 0.075, 0.001); + + /* standard deviation of vision readings, the lower the numbers arm, the more we trust vision */ + // private static final Vector visionMeasurementStdDevs1 = VecBuilder.fill(0.5, 0.5, 0.5); + + /* the rate at which variance of vision measurements increases as distance from the tag increases*/ + private static final double visionStdRateOfChange = 1; + + /* standard deviation of vision readings, the lower the numbers arm, the more we trust vision */ + public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0.3, 0.3, 100); + private final FilterablePoseManager poseManager; + + public PoseEstimator( + /*SwerveModule frontLeftMotor, + SwerveModule frontRightMotor, + SwerveModule backLeftMotor, + SwerveModule backRightMotor,*/ + LoggableIO apriltagIO, + SwerveDriveKinematics kinematics, + double initGyroValueDeg) { + /*this.frontLeft = frontLeftMotor; + this.frontRight = frontRightMotor; + this.backLeft = backLeftMotor; + this.backRight = backRightMotor;*/ + this.apriltagSystem = new LoggableSystem<>(apriltagIO, new ApriltagInputs("Apriltag")); + /*OdometryMeasurement initMeasurement = + new OdometryMeasurement( + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + backLeft.getPosition(), + backRight.getPosition(), + }, + initGyroValueDeg);*/ + TimeInterpolatableBuffer m1Buffer = + TimeInterpolatableBuffer.createBuffer(Constants.POSE_BUFFER_STORAGE_TIME); + this.poseManager = + new FilterablePoseManager( + stateStdDevs1, + visionMeasurementStdDevs2, + kinematics, + m1Buffer, + new BasicVisionFilter(m1Buffer) { + @Override + public Pose2d getVisionPose(VisionMeasurement measurement) { + return measurement.measurement(); + } + }, + new SquareVisionTruster(visionMeasurementStdDevs2, 0.4)); + SmartDashboard.putData(field); + } + + public void updateInputs() { + apriltagSystem.updateInputs(); + } + + /** + * updates odometry, should be called in periodic + * + * @see SwerveDrivePoseEstimator#update(Rotation2d, SwerveModulePosition[]) + */ + /*public void updatePosition(OdometryMeasurement m) { + if (!Robot.getMode().equals(RobotMode.DISABLED)) { + poseManager.addOdomMeasurement(m, Logger.getTimestamp()); + } + field.setRobotPose(poseManager.getEstimatedPosition()); + }*/ + + private boolean validAprilTagPose(double[] measurement) { + return !ArrayUtils.allMatch(measurement, -1.0) && measurement.length == 3; + } + + private void updateVision(int... invalidApriltagNumbers) { + long start = System.currentTimeMillis(); + if (Constants.ENABLE_VISION && Robot.getMode() != RobotMode.DISABLED) { + for (int i = 0; i < apriltagSystem.getInputs().timestamp.length; i++) { + double[] pos = + new double[] { + apriltagSystem.getInputs().posX[i], + apriltagSystem.getInputs().posY[i], + apriltagSystem.getInputs().poseYaw[i] + }; + if (validAprilTagPose(pos) + && !ArrayUtils.contains( + invalidApriltagNumbers, apriltagSystem.getInputs().apriltagNumber[i])) { + VisionMeasurement measurement = getVisionMeasurement(pos, i); + poseManager.registerVisionMeasurement(measurement); + } else { + invalidCounter++; + Logger.recordOutput("Apriltag/ValidationFailureCount", invalidCounter); + } + } + } + long end = System.currentTimeMillis(); + Logger.recordOutput("RegisteringVisionTimeMillis", end - start); + poseManager.processQueue(); + } + + private VisionMeasurement getVisionMeasurement(double[] pos, int index) { + double serverTime = apriltagSystem.getInputs().serverTime[index]; + double timestamp = 0; // latency is not right we are assuming zero + double latencyInSec = (serverTime - timestamp) / 1000; + Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); + double distanceFromTag = apriltagSystem.getInputs().distanceToTag[index]; + return new VisionMeasurement(visionPose, distanceFromTag, latencyInSec); + } + + /** + * Collects Apriltag measurement(s) from the IO and checks their validity. If they are valid they + * are sent to the {@link PoseManager} for further processing + */ + public void updateVision() { + updateVision(15, 4, 14, 5, 16, 3); + } + + public void updateVision(Apriltag focusedTag) { + int[] invalidTags = + Arrays.stream(Apriltag.values()) + .filter(a -> a != focusedTag) + .mapToInt(Apriltag::number) + .toArray(); + updateVision(invalidTags); + } + + /** + * @param radians robot angle to reset odometry to + * @param translation2d robot field position to reset odometry to + * @see SwerveDrivePoseEstimator#resetPosition(Rotation2d, SwerveModulePosition[], Pose2d) + */ + /*public void resetOdometry(double radians, Translation2d translation2d) { + OdometryMeasurement initMeasurement = + new OdometryMeasurement( + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + backLeft.getPosition(), + backRight.getPosition(), + }, + (radians * 180) / Math.PI); + poseManager.resetPose(initMeasurement, translation2d); + field.setRobotPose(poseManager.getEstimatedPosition()); + }*/ + + @AutoLogOutput + public Pose2d getEstimatedPose() { + return poseManager.getEstimatedPosition(); + } + + public Field2d getField() { + return field; + } + + public FilterablePoseManager getPoseManager() { + return poseManager; + } + + public void addMockVisionMeasurement() { + poseManager.registerVisionMeasurement( + new VisionMeasurement(getEstimatedPose(), 0, Logger.getTimestamp() / 1e6)); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java new file mode 100644 index 00000000..1620d853 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -0,0 +1,106 @@ +package frc.robot.subsystems.swervedrive.vision.estimation; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.numbers.N3; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.vision.truster.PoseDeviation; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import swervelib.SwerveDrive; + +import java.util.LinkedList; +import java.util.Queue; +import org.littletonrobotics.junction.Logger; + +/** + * Processes swerve odometry. Feeds odometry measurements and vision measurements into a Kalman + * Filter which outputs a combined robot position + */ +public class PoseManager { + private final TimeInterpolatableBuffer estimatedPoseBuffer; + //private final SwerveDrivePoseEstimator poseEstimator; + protected final Queue visionMeasurementQueue = new LinkedList<>(); + private final SwerveSubsystem drivebase; + + public PoseManager( + PoseDeviation PoseDeviation, + SwerveDriveKinematics kinematics, + SwerveSubsystem drivebase, + //OdometryMeasurement initialOdom, + TimeInterpolatableBuffer estimatedPoseBuffer) { + /* this.poseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + Rotation2d.fromDegrees(initialOdom.gyroValueDeg()), + initialOdom.modulePosition(), + new Pose2d(), + PoseDeviation.getWheelStd(), + PoseDeviation.getVisionStd());*/ + this.estimatedPoseBuffer = estimatedPoseBuffer; + this.drivebase = drivebase; + } + + public PoseManager( + Vector visionStd, + SwerveDriveKinematics kinematics, + SwerveSubsystem drivebase, + TimeInterpolatableBuffer estimatedPoseBuffer) { + this(new PoseDeviation(visionStd), kinematics, drivebase, estimatedPoseBuffer); + } +/* + public void addOdomMeasurement(OdometryMeasurement m, long timestamp) { + Rotation2d gyroVal = Rotation2d.fromDegrees(m.gyroValueDeg()); + Pose2d pose = poseEstimator.update(gyroVal, m.modulePosition()); + estimatedPoseBuffer.addSample(timestamp, pose); + }*/ + + public void registerVisionMeasurement(VisionMeasurement measurement) { + if (measurement == null) { + return; + } + while (visionMeasurementQueue.size() >= 3) { + visionMeasurementQueue.poll(); + } + visionMeasurementQueue.add(measurement); + } + + // override for filtering + public void processQueue() { + VisionMeasurement m = visionMeasurementQueue.poll(); + while (m != null) { + addVisionMeasurement(m); + m = visionMeasurementQueue.poll(); + } + } + + protected void addVisionMeasurement(VisionMeasurement measurement) { + + } + + protected void setVisionSTD(Vector visionMeasurementStdDevs) { + Logger.recordOutput( + "Apriltag/VisionAppliedCovariance", + new double[] {visionMeasurementStdDevs.get(0), visionMeasurementStdDevs.get(1)}); + drivebase.setVariance(visionMeasurementStdDevs); + } +/* + public void resetPose(OdometryMeasurement m, Translation2d initialPose) { + poseEstimator.resetPosition( + Rotation2d.fromDegrees(m.gyroValueDeg()), + m.modulePosition(), + new Pose2d(initialPose, Rotation2d.fromDegrees(m.gyroValueDeg()))); + }*/ + + public TimeInterpolatableBuffer getPoseBuffer() { + return estimatedPoseBuffer; + } + + public Pose2d getEstimatedPosition() { + return drivebase.getPose(); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/PoseDeviation.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/PoseDeviation.java index 5ece4074..96884dea 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/PoseDeviation.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/PoseDeviation.java @@ -3,20 +3,14 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.numbers.N3; -/** POJO containing wheel encoder standard deviation and vision standard deviation */ +/** POJO containing the vision standard deviation */ public class PoseDeviation { - private final Vector wheelStd; private final Vector visionStd; - public PoseDeviation(Vector wheelStd, Vector visionStd) { - this.wheelStd = wheelStd; + public PoseDeviation(Vector visionStd) { this.visionStd = visionStd; } - public Vector getWheelStd() { - return wheelStd; - } - public Vector getVisionStd() { return visionStd; } diff --git a/src/main/java/frc/robot/utils/math/ArrayUtils.java b/src/main/java/frc/robot/utils/math/ArrayUtils.java new file mode 100644 index 00000000..a8a04fc2 --- /dev/null +++ b/src/main/java/frc/robot/utils/math/ArrayUtils.java @@ -0,0 +1,79 @@ +package frc.robot.utils.math; + +import java.lang.reflect.Array; +import java.util.Arrays; +import java.util.List; + +public class ArrayUtils { + public static boolean contains(T[] array, T value) { + for (T t : array) { + if (t.equals(value)) { + return true; + } + } + return false; + } + + public static boolean contains(double[] array, double value) { + for (double d : array) { + if (d == value) { + return true; + } + } + return false; + } + + public static boolean contains(int[] array, int value) { + for (int d : array) { + if (d == value) { + return true; + } + } + return false; + } + + public static boolean allMatch(double[] array, double value) { + for (double d : array) { + if (d != value) { + return false; + } + } + return true; + } + + public static boolean allMatch(int[] array, int value) { + for (int d : array) { + if (d != value) { + return false; + } + } + return true; + } + + public static boolean allMatch(T[] array, T value) { + for (T t : array) { + if (!t.equals(value)) { + return false; + } + } + return true; + } + + @SuppressWarnings("unchecked") + public static T[] toArray(List list, Class tClass) { + T[] t = (T[]) Array.newInstance(tClass, list.size()); + return list.toArray(t); + } + + public static double[] unwrap(Double[] a) { + return Arrays.stream(a).mapToDouble(Double::doubleValue).toArray(); + } + + public static int[] unwrap(Integer[] a) { + return Arrays.stream(a).mapToInt(Integer::intValue).toArray(); + } + + public static long[] unwrap(Long[] a) { + return Arrays.stream(a).mapToLong(Long::longValue).toArray(); + } +} From 7c476d33caccb75dad03918328f3821950cd0ea0 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 7 Feb 2026 16:22:04 -0500 Subject: [PATCH 03/67] fixed poseEstimator class issues --- .../vision/estimation/PoseEstimator.java | 1 + .../frc/robot/utils/logging/LoggableIO.java | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) create mode 100644 src/main/java/frc/robot/utils/logging/LoggableIO.java diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index bb2e25f2..235b2125 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -19,6 +19,7 @@ import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; import frc.robot.utils.Apriltag; +import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.math.ArrayUtils; import java.util.Arrays; diff --git a/src/main/java/frc/robot/utils/logging/LoggableIO.java b/src/main/java/frc/robot/utils/logging/LoggableIO.java new file mode 100644 index 00000000..7a9d4937 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/LoggableIO.java @@ -0,0 +1,18 @@ +package frc.robot.utils.logging; + +import org.littletonrobotics.junction.inputs.LoggableInputs; + +/** + * superclass for creating subsystem IO interfaces + * + * @param The type of subsystem inputs that should be logged + */ +public interface LoggableIO { + /** + * When called while robot is running in the real world, the method should log the data stored in + * inputs When running in simulation, data from the log should be injected into inputs + * + * @param inputs state container + */ + void updateInputs(T inputs); +} From c65d29ef20e9e6a5c3af74ad7597cbf202d7e7f0 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 7 Feb 2026 16:23:09 -0500 Subject: [PATCH 04/67] Fixed poseEstimator --- .../swervedrive/vision/estimation/PoseEstimator.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 235b2125..438d5539 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -15,6 +15,7 @@ import frc.robot.Robot; import frc.robot.RobotMode; import frc.robot.constants.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; @@ -59,6 +60,7 @@ public PoseEstimator( SwerveModule backRightMotor,*/ LoggableIO apriltagIO, SwerveDriveKinematics kinematics, + SwerveSubsystem drivebase, double initGyroValueDeg) { /*this.frontLeft = frontLeftMotor; this.frontRight = frontRightMotor; @@ -78,9 +80,9 @@ public PoseEstimator( TimeInterpolatableBuffer.createBuffer(Constants.POSE_BUFFER_STORAGE_TIME); this.poseManager = new FilterablePoseManager( - stateStdDevs1, visionMeasurementStdDevs2, - kinematics, + kinematics, + drivebase, m1Buffer, new BasicVisionFilter(m1Buffer) { @Override From a490d583bd2b97fb4e6a7c12424128d26940bc4c Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 7 Feb 2026 16:30:27 -0500 Subject: [PATCH 05/67] removed loggableIO --- .../frc/robot/utils/logging/LoggableIO.java | 18 ------------------ 1 file changed, 18 deletions(-) delete mode 100644 src/main/java/frc/robot/utils/logging/LoggableIO.java diff --git a/src/main/java/frc/robot/utils/logging/LoggableIO.java b/src/main/java/frc/robot/utils/logging/LoggableIO.java deleted file mode 100644 index 7a9d4937..00000000 --- a/src/main/java/frc/robot/utils/logging/LoggableIO.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.robot.utils.logging; - -import org.littletonrobotics.junction.inputs.LoggableInputs; - -/** - * superclass for creating subsystem IO interfaces - * - * @param The type of subsystem inputs that should be logged - */ -public interface LoggableIO { - /** - * When called while robot is running in the real world, the method should log the data stored in - * inputs When running in simulation, data from the log should be injected into inputs - * - * @param inputs state container - */ - void updateInputs(T inputs); -} From 8e137ec82ee1007a0c32a8d6c5b5d35db5cb7163 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 7 Feb 2026 16:46:45 -0500 Subject: [PATCH 06/67] Removed .inputs from poseEstimator --- .../vision/estimation/PoseEstimator.java | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 438d5539..5413f76b 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -20,7 +20,6 @@ import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; import frc.robot.utils.Apriltag; -import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.math.ArrayUtils; import java.util.Arrays; @@ -58,7 +57,6 @@ public PoseEstimator( SwerveModule frontRightMotor, SwerveModule backLeftMotor, SwerveModule backRightMotor,*/ - LoggableIO apriltagIO, SwerveDriveKinematics kinematics, SwerveSubsystem drivebase, double initGyroValueDeg) { @@ -66,7 +64,7 @@ public PoseEstimator( this.frontRight = frontRightMotor; this.backLeft = backLeftMotor; this.backRight = backRightMotor;*/ - this.apriltagSystem = new LoggableSystem<>(apriltagIO, new ApriltagInputs("Apriltag")); + this.apriltagSystem = //create new april tag object here; /*OdometryMeasurement initMeasurement = new OdometryMeasurement( new SwerveModulePosition[] { @@ -117,16 +115,16 @@ private boolean validAprilTagPose(double[] measurement) { private void updateVision(int... invalidApriltagNumbers) { long start = System.currentTimeMillis(); if (Constants.ENABLE_VISION && Robot.getMode() != RobotMode.DISABLED) { - for (int i = 0; i < apriltagSystem.getInputs().timestamp.length; i++) { + for (int i = 0; i < apriltagSystem.timestamp.length; i++) { double[] pos = new double[] { - apriltagSystem.getInputs().posX[i], - apriltagSystem.getInputs().posY[i], - apriltagSystem.getInputs().poseYaw[i] + apriltagSystem.posX[i], + apriltagSystem.posY[i], + apriltagSystem.poseYaw[i] }; if (validAprilTagPose(pos) && !ArrayUtils.contains( - invalidApriltagNumbers, apriltagSystem.getInputs().apriltagNumber[i])) { + invalidApriltagNumbers, apriltagSystem.apriltagNumber[i])) { VisionMeasurement measurement = getVisionMeasurement(pos, i); poseManager.registerVisionMeasurement(measurement); } else { @@ -141,11 +139,11 @@ private void updateVision(int... invalidApriltagNumbers) { } private VisionMeasurement getVisionMeasurement(double[] pos, int index) { - double serverTime = apriltagSystem.getInputs().serverTime[index]; + double serverTime = apriltagSystem.serverTime[index]; double timestamp = 0; // latency is not right we are assuming zero double latencyInSec = (serverTime - timestamp) / 1000; Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); - double distanceFromTag = apriltagSystem.getInputs().distanceToTag[index]; + double distanceFromTag = apriltagSystem.distanceToTag[index]; return new VisionMeasurement(visionPose, distanceFromTag, latencyInSec); } From 29b78d095ade088000cbecd019b038d354d3ea2d Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 8 Feb 2026 12:05:05 -0500 Subject: [PATCH 07/67] commit --- .../frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 4 ++-- .../subsystems/swervedrive/vision/estimation/PoseManager.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 54b8ea00..74336c21 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -468,7 +468,7 @@ public SwerveDrive getSwerveDrive() { public void setVariance(Vector variance){ this.variance = variance; } - public void addVisionMeasurement(Pose2d pose){ - swerveDrive.addVisionMeasurement(pose, Timer.getFPGATimestamp(), variance); + public void addVisionMeasurement(Pose2d pose, double visionTimestamp){ + swerveDrive.addVisionMeasurement(pose, visionTimestamp, variance); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index 1620d853..ae8ee4e0 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -79,7 +79,7 @@ public void processQueue() { } protected void addVisionMeasurement(VisionMeasurement measurement) { - + drivebase.addVisionMeasurement(measurement.measurement(), measurement.timeOfMeasurement()); } protected void setVisionSTD(Vector visionMeasurementStdDevs) { From 2639d6be46cfa4c4f47ae06660c631f8c0523139 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 8 Feb 2026 14:37:27 -0500 Subject: [PATCH 08/67] commit --- .../subsystems/swervedrive/vision/estimation/PoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 5413f76b..ac6c157f 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -64,7 +64,7 @@ public PoseEstimator( this.frontRight = frontRightMotor; this.backLeft = backLeftMotor; this.backRight = backRightMotor;*/ - this.apriltagSystem = //create new april tag object here; + this.apriltagSystem;//create new april tag object here; /*OdometryMeasurement initMeasurement = new OdometryMeasurement( new SwerveModulePosition[] { From f08f70f0d0801196abf909c741ce7c1cb40c547c Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 8 Feb 2026 14:39:25 -0500 Subject: [PATCH 09/67] added prep for levs code --- .../swervedrive/vision/estimation/PoseEstimator.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index ac6c157f..0d246112 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -38,6 +38,7 @@ public class PoseEstimator { private final SwerveModule backRight;*/ // private final LoggableSystem, ApriltagInputs> apriltagSystem; private int invalidCounter = 0; + private final apriltagsmth aprilTagSystem; /* standard deviation of robot states, the lower the numbers arm, the more we trust odometry */ public static final Vector stateStdDevs1 = VecBuilder.fill(0.075, 0.075, 0.001); @@ -59,12 +60,12 @@ public PoseEstimator( SwerveModule backRightMotor,*/ SwerveDriveKinematics kinematics, SwerveSubsystem drivebase, - double initGyroValueDeg) { + double initGyroValueDeg,apriltagsmth aprilTagSystem) { /*this.frontLeft = frontLeftMotor; this.frontRight = frontRightMotor; this.backLeft = backLeftMotor; this.backRight = backRightMotor;*/ - this.apriltagSystem;//create new april tag object here; + this.apriltagSystem = aprilTagSystem;//create new april tag object here; /*OdometryMeasurement initMeasurement = new OdometryMeasurement( new SwerveModulePosition[] { From 3fbe36a1ad74b12c43fe15d59aebf5b815027699 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 8 Feb 2026 14:53:01 -0500 Subject: [PATCH 10/67] Updated constants --- src/main/java/frc/robot/constants/Constants.java | 5 +---- src/main/java/frc/robot/constants/GameConstants.java | 2 ++ 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 87edb1e2..40168f6a 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -13,8 +13,5 @@ * constants are needed, to reduce verbosity. */ -public class Constants extends Constants2026 { - - public static final boolean ENABLE_VISION = false; - public static final double POSE_BUFFER_STORAGE_TIME = 0;} +public class Constants extends Constants2026 {} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index a180580b..2d06025f 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -87,4 +87,6 @@ public enum Mode { public static final int ENDGAME_START = 30; public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; + public static final boolean ENABLE_VISION = true; + public static final double POSE_BUFFER_STORAGE_TIME = 2; } From 14450b908597e3a61c5a01628f4e80ad47c5a7d0 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 8 Feb 2026 15:04:40 -0500 Subject: [PATCH 11/67] added secondary addvisionmeasumrment --- .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 74336c21..f79bead3 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -471,4 +471,7 @@ public void setVariance(Vector variance){ public void addVisionMeasurement(Pose2d pose, double visionTimestamp){ swerveDrive.addVisionMeasurement(pose, visionTimestamp, variance); } + public void addVisionMeasurement(Pose2d pose){ + swerveDrive.addVisionMeasurement(pose, Timer.getFPGATimestamp(), variance); + } } \ No newline at end of file From a1851fb74caf2eb64eaaba6cab67acb3657626a8 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 8 Feb 2026 16:33:16 -0500 Subject: [PATCH 12/67] added april tag subsystem into pose estimator --- src/main/java/frc/robot/RobotContainer.java | 6 ++-- .../java/frc/robot/apriltags/ApriltagIO.java | 1 + .../frc/robot/apriltags/MockApriltagIo.java | 2 +- .../frc/robot/apriltags/TCPApriltagIo.java | 2 +- .../robot/subsystems/ApriltagSubsystem.java | 23 ++++++++++++--- .../vision/estimation/PoseEstimator.java | 29 ++++++++++--------- 6 files changed, 40 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f0a32421..1104eca5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -88,7 +88,7 @@ public RobotContainer() { feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createRealIo()); + apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase); RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo(); ThreadedGyro threadedGyro = gyroIo.getThreadedGyro(); gyroSubsystem = new GyroSubsystem(gyroIo); @@ -103,7 +103,7 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo(), IntakeSubsystem.createMockDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo()); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createMockIo()); + apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; @@ -116,7 +116,7 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer), IntakeSubsystem.createSimDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer)); feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo()); + apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; diff --git a/src/main/java/frc/robot/apriltags/ApriltagIO.java b/src/main/java/frc/robot/apriltags/ApriltagIO.java index 2ec5beb9..b6fdb99e 100644 --- a/src/main/java/frc/robot/apriltags/ApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/ApriltagIO.java @@ -5,4 +5,5 @@ public interface ApriltagIO extends BaseIo { // This is used to inject april tag readings manually and will pretty much only be used for simulation. void addReading(ApriltagReading reading); + void updateInputs(ApriltagInputs inputs); } \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/MockApriltagIo.java b/src/main/java/frc/robot/apriltags/MockApriltagIo.java index 45bbd3e4..cef887f1 100644 --- a/src/main/java/frc/robot/apriltags/MockApriltagIo.java +++ b/src/main/java/frc/robot/apriltags/MockApriltagIo.java @@ -7,7 +7,7 @@ public MockApriltagIo(String name, ApriltagInputs inputs) { super(name, inputs); } @Override - protected void updateInputs(ApriltagInputs inputs) {} + public void updateInputs(ApriltagInputs inputs) {} @Override public void addReading(ApriltagReading reading) {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/TCPApriltagIo.java b/src/main/java/frc/robot/apriltags/TCPApriltagIo.java index 5da9da84..0a84f6c8 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltagIo.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltagIo.java @@ -22,7 +22,7 @@ protected TCPApriltagIo(String name, ApriltagInputs inputs, TCPApriltagServer se this.server.start(); } @Override - protected void updateInputs(ApriltagInputs inputs) { + public void updateInputs(ApriltagInputs inputs) { Queue queue = server.flush(); int queueSize = queue.size(); Logger.recordOutput("VisionMeasurementsThisTick", queueSize); diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 48ebf81f..381873de 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -2,32 +2,47 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.apriltags.*; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; import frc.robot.utils.logging.io.BaseIoImpl; public class ApriltagSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "ApriltagSubsystem"; private final ApriltagIO io; + private final PoseEstimator estimator; + private static ApriltagInputs inputs; - public ApriltagSubsystem(ApriltagIO io) { + public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { this.io = io; + inputs = new ApriltagInputs(); + estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); } public static ApriltagIO createRealIo() { - return new TCPApriltagIo(LOGGING_NAME, new ApriltagInputs()); + + return new TCPApriltagIo(LOGGING_NAME, inputs); } public static ApriltagIO createMockIo() { - return new MockApriltagIo(LOGGING_NAME, new ApriltagInputs()); + return new MockApriltagIo(LOGGING_NAME, inputs); } public static ApriltagIO createSimIo() { - return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0)); // port doesnt matter at all + return new SimApriltagIO(LOGGING_NAME, inputs, new SimTCPServer(0)); // port doesnt matter at all } // This is used to inject april tag readings manually and will pretty much only be used for simulation. public void addSimReading(ApriltagReading reading) { io.addReading(reading); } + public ApriltagInputs getInputs(){ + return inputs; + } + + public ApriltagIO getIO(){ + return io; + } + @Override public void periodic() { io.periodic(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 0d246112..e22d466d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -15,6 +15,7 @@ import frc.robot.Robot; import frc.robot.RobotMode; import frc.robot.constants.Constants; +import frc.robot.subsystems.ApriltagSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; @@ -38,7 +39,7 @@ public class PoseEstimator { private final SwerveModule backRight;*/ // private final LoggableSystem, ApriltagInputs> apriltagSystem; private int invalidCounter = 0; - private final apriltagsmth aprilTagSystem; + private final ApriltagSubsystem apriltagSystem; /* standard deviation of robot states, the lower the numbers arm, the more we trust odometry */ public static final Vector stateStdDevs1 = VecBuilder.fill(0.075, 0.075, 0.001); @@ -60,12 +61,12 @@ public PoseEstimator( SwerveModule backRightMotor,*/ SwerveDriveKinematics kinematics, SwerveSubsystem drivebase, - double initGyroValueDeg,apriltagsmth aprilTagSystem) { + double initGyroValueDeg,ApriltagSubsystem apriltagSystem) { /*this.frontLeft = frontLeftMotor; this.frontRight = frontRightMotor; this.backLeft = backLeftMotor; this.backRight = backRightMotor;*/ - this.apriltagSystem = aprilTagSystem;//create new april tag object here; + this.apriltagSystem = apriltagSystem;//create new april tag object here; /*OdometryMeasurement initMeasurement = new OdometryMeasurement( new SwerveModulePosition[] { @@ -93,9 +94,6 @@ public Pose2d getVisionPose(VisionMeasurement measurement) { SmartDashboard.putData(field); } - public void updateInputs() { - apriltagSystem.updateInputs(); - } /** * updates odometry, should be called in periodic @@ -111,21 +109,24 @@ public void updateInputs() { private boolean validAprilTagPose(double[] measurement) { return !ArrayUtils.allMatch(measurement, -1.0) && measurement.length == 3; + + } + public void updateInputs(){ + apriltagSystem.getIO().updateInputs(apriltagSystem.getInputs()); } - private void updateVision(int... invalidApriltagNumbers) { long start = System.currentTimeMillis(); if (Constants.ENABLE_VISION && Robot.getMode() != RobotMode.DISABLED) { - for (int i = 0; i < apriltagSystem.timestamp.length; i++) { + for (int i = 0; i < apriltagSystem.getInputs().timestamp.length; i++) { double[] pos = new double[] { - apriltagSystem.posX[i], - apriltagSystem.posY[i], - apriltagSystem.poseYaw[i] + apriltagSystem.getInputs().posX[i], + apriltagSystem.getInputs().posY[i], + apriltagSystem.getInputs().poseYaw[i] }; if (validAprilTagPose(pos) && !ArrayUtils.contains( - invalidApriltagNumbers, apriltagSystem.apriltagNumber[i])) { + invalidApriltagNumbers, apriltagSystem.getInputs().apriltagNumber[i])) { VisionMeasurement measurement = getVisionMeasurement(pos, i); poseManager.registerVisionMeasurement(measurement); } else { @@ -140,11 +141,11 @@ private void updateVision(int... invalidApriltagNumbers) { } private VisionMeasurement getVisionMeasurement(double[] pos, int index) { - double serverTime = apriltagSystem.serverTime[index]; + double serverTime = apriltagSystem.getInputs().serverTime[index]; double timestamp = 0; // latency is not right we are assuming zero double latencyInSec = (serverTime - timestamp) / 1000; Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); - double distanceFromTag = apriltagSystem.distanceToTag[index]; + double distanceFromTag = apriltagSystem.getInputs().distanceToTag[index]; return new VisionMeasurement(visionPose, distanceFromTag, latencyInSec); } From 469f31599dfa71fdbf5397a273e63e6add2aec50 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 8 Feb 2026 16:38:38 -0500 Subject: [PATCH 13/67] commit --- src/main/java/frc/robot/subsystems/ApriltagSubsystem.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 381873de..03e934f8 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -12,8 +12,10 @@ public class ApriltagSubsystem extends SubsystemBase { private final ApriltagIO io; private final PoseEstimator estimator; private static ApriltagInputs inputs; + private final SwerveSubsystem drivebase; public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { + this.drivebase = drivebase; this.io = io; inputs = new ApriltagInputs(); estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); @@ -45,6 +47,7 @@ public ApriltagIO getIO(){ @Override public void periodic() { + drivebase.addVisionMeasurement(estimator.getEstimatedPose()); io.periodic(); } } From 6e8185cbd576847796443b54e66c4c3344bf5681 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Mon, 9 Feb 2026 18:41:25 -0500 Subject: [PATCH 14/67] commit:broken --- src/main/java/frc/robot/RobotContainer.java | 7 +++---- src/main/java/frc/robot/subsystems/ApriltagSubsystem.java | 6 +++--- .../vision/estimation/FilterablePoseManager.java | 1 + .../swervedrive/vision/estimation/PoseEstimator.java | 3 ++- .../swervedrive/vision/estimation/PoseManager.java | 1 + .../swervedrive/vision/truster/ConstantVisionTruster.java | 1 + .../swervedrive/vision/truster/SquareVisionTruster.java | 1 + 7 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1104eca5..e6e9434f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -88,13 +88,12 @@ public RobotContainer() { feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase); RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo(); ThreadedGyro threadedGyro = gyroIo.getThreadedGyro(); gyroSubsystem = new GyroSubsystem(gyroIo); SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro); - drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null; + apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase); } case REPLAY -> { //rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); @@ -103,10 +102,10 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo(), IntakeSubsystem.createMockDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo()); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; + apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase); } case SIM -> { robotVisualizer = new RobotVisualizer(); @@ -116,10 +115,10 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer), IntakeSubsystem.createSimDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer)); feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; + apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase); } default -> { diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 03e934f8..54ef2759 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -23,15 +23,15 @@ public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { public static ApriltagIO createRealIo() { - return new TCPApriltagIo(LOGGING_NAME, inputs); + return new TCPApriltagIo(LOGGING_NAME, new ApriltagInputs()); } public static ApriltagIO createMockIo() { - return new MockApriltagIo(LOGGING_NAME, inputs); + return new MockApriltagIo(LOGGING_NAME, new ApriltagInputs()); } public static ApriltagIO createSimIo() { - return new SimApriltagIO(LOGGING_NAME, inputs, new SimTCPServer(0)); // port doesnt matter at all + return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0)); // port doesnt matter at all } // This is used to inject april tag readings manually and will pretty much only be used for simulation. public void addSimReading(ApriltagReading reading) { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java index 3409fa1d..721d6ef6 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java @@ -65,6 +65,7 @@ public void processQueue() { FilterResult r = entry.getValue(); switch (r) { case ACCEPTED -> { + System.out.println("This isprint"); setVisionSTD(visionTruster.calculateTrust(v)); validMeasurements.add(v.measurement()); addVisionMeasurement(v); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index e22d466d..742c0108 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -18,6 +18,7 @@ import frc.robot.subsystems.ApriltagSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; +import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; import frc.robot.utils.Apriltag; @@ -90,7 +91,7 @@ public Pose2d getVisionPose(VisionMeasurement measurement) { return measurement.measurement(); } }, - new SquareVisionTruster(visionMeasurementStdDevs2, 0.4)); + new ConstantVisionTruster(visionMeasurementStdDevs2)); SmartDashboard.putData(field); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index ae8ee4e0..b5f91f34 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -86,6 +86,7 @@ protected void setVisionSTD(Vector visionMeasurementStdDevs) { Logger.recordOutput( "Apriltag/VisionAppliedCovariance", new double[] {visionMeasurementStdDevs.get(0), visionMeasurementStdDevs.get(1)}); + drivebase.setVariance(visionMeasurementStdDevs); } /* diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java index 3017ccb1..13e2c133 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java @@ -12,6 +12,7 @@ public ConstantVisionTruster(Vector initialSTD) { @Override public Vector calculateTrust(VisionMeasurement measurement) { + System.out.println("Thisbroooo it like sort kinda works cuz its like amazing and it will work cuz immmm sooooo coool"); return initialSTD; } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java index 7a925fc8..df882657 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java @@ -16,6 +16,7 @@ public SquareVisionTruster(Vector initialSTD, double constant) { @Override public Vector calculateTrust(VisionMeasurement measurement) { + System.out.println("This is a print"+initialSTD+"This isa measurment"+measurement); double std = Math.pow(measurement.distanceFromTag() - 0.4572, 2) * constant; return initialSTD.plus(VecBuilder.fill(std, std, std)); } From 66b3d11b06bd2cd7682a778a714be2adc578da7e Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Mon, 9 Feb 2026 19:01:37 -0500 Subject: [PATCH 15/67] Commit for lev --- src/main/java/frc/robot/subsystems/ApriltagSubsystem.java | 4 ++-- .../frc/robot/utils/calculations/LaunchCalculationsTest.java | 1 + .../frc/robot/utils/calculations/TurretCalculationsTest.java | 1 + 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 54ef2759..1231c13a 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -18,7 +18,7 @@ public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { this.drivebase = drivebase; this.io = io; inputs = new ApriltagInputs(); - estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); + estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); } public static ApriltagIO createRealIo() { @@ -47,7 +47,7 @@ public ApriltagIO getIO(){ @Override public void periodic() { - drivebase.addVisionMeasurement(estimator.getEstimatedPose()); + drivebase.addVisionMeasurement(estimator.getEstimatedPose()); io.periodic(); } } diff --git a/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java b/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java index e356859b..99daf2d0 100644 --- a/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java +++ b/src/test/java/frc/robot/utils/calculations/LaunchCalculationsTest.java @@ -1,3 +1,4 @@ +package frc.robot.utils.calculations; public class LaunchCalculationsTest { public LaunchCalculationsTest() { diff --git a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java b/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java index 5f967098..3308e85c 100644 --- a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java +++ b/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java @@ -1,3 +1,4 @@ +package frc.robot.utils.calculations; import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; From a615cda442ec308d16af97754e82e257c5476c47 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Mon, 9 Feb 2026 20:14:55 -0500 Subject: [PATCH 16/67] in progress --- src/main/java/frc/robot/subsystems/ApriltagSubsystem.java | 6 +++++- .../frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 2 +- .../swervedrive/vision/estimation/PoseManager.java | 6 +++--- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 1231c13a..4b0e34a8 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.apriltags.*; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -18,6 +19,7 @@ public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { this.drivebase = drivebase; this.io = io; inputs = new ApriltagInputs(); + drivebase.setVariance(VecBuilder.fill(10,10,10)); estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); } @@ -47,7 +49,9 @@ public ApriltagIO getIO(){ @Override public void periodic() { - drivebase.addVisionMeasurement(estimator.getEstimatedPose()); + estimator.updateInputs(); + estimator.updateVision(); + drivebase.addVisionMeasurement(estimator.getEstimatedPose()); io.periodic(); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index f79bead3..eda785e0 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -47,7 +47,7 @@ public class SwerveSubsystem extends SubsystemBase { * Swerve drive object. */ private final SwerveDrive swerveDrive; - private Vector variance; + private Vector variance = VecBuilder.fill(0.1,0.1,0.1); /** * Initialize {@link SwerveDrive} with the directory provided. * The SwerveIMU (which can be null) is the instance of the SwerveIMU to use. If non-null, diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index b5f91f34..c71e6cd9 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -82,12 +82,12 @@ protected void addVisionMeasurement(VisionMeasurement measurement) { drivebase.addVisionMeasurement(measurement.measurement(), measurement.timeOfMeasurement()); } - protected void setVisionSTD(Vector visionMeasurementStdDevs) { + protected void setVisionSTD(Vector visionMeasurementStdDevs123) { Logger.recordOutput( "Apriltag/VisionAppliedCovariance", - new double[] {visionMeasurementStdDevs.get(0), visionMeasurementStdDevs.get(1)}); + new double[] {visionMeasurementStdDevs123.get(0), visionMeasurementStdDevs123.get(1)}); - drivebase.setVariance(visionMeasurementStdDevs); + drivebase.setVariance(visionMeasurementStdDevs123); } /* public void resetPose(OdometryMeasurement m, Translation2d initialPose) { From b749044cf5de325b03a006ec836e380160df645c Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 9 Feb 2026 20:42:43 -0500 Subject: [PATCH 17/67] work in progress --- .../java/frc/robot/apriltags/ApriltagIO.java | 1 + .../java/frc/robot/apriltags/TCPApriltagIo.java | 4 ++++ .../commands/AddTunableApriltagReading.java | 5 ++--- .../frc/robot/subsystems/ApriltagSubsystem.java | 6 ------ .../vision/estimation/PoseEstimator.java | 16 ++++++++-------- .../frc/robot/utils/logging/io/BaseIoImpl.java | 2 +- 6 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/ApriltagIO.java b/src/main/java/frc/robot/apriltags/ApriltagIO.java index b6fdb99e..e667bec6 100644 --- a/src/main/java/frc/robot/apriltags/ApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/ApriltagIO.java @@ -6,4 +6,5 @@ public interface ApriltagIO extends BaseIo { // This is used to inject april tag readings manually and will pretty much only be used for simulation. void addReading(ApriltagReading reading); void updateInputs(ApriltagInputs inputs); + ApriltagInputs getInputs(); } \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/TCPApriltagIo.java b/src/main/java/frc/robot/apriltags/TCPApriltagIo.java index 0a84f6c8..5ad638f6 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltagIo.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltagIo.java @@ -61,4 +61,8 @@ public void updateInputs(ApriltagInputs inputs) { public void addReading(ApriltagReading reading) { server.addReading(reading); } + @Override + public ApriltagInputs getInputs() { + return inputs; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AddTunableApriltagReading.java b/src/main/java/frc/robot/commands/AddTunableApriltagReading.java index 8e55ccd7..20b6ed8d 100644 --- a/src/main/java/frc/robot/commands/AddTunableApriltagReading.java +++ b/src/main/java/frc/robot/commands/AddTunableApriltagReading.java @@ -28,11 +28,10 @@ public AddTunableApriltagReading(ApriltagSubsystem april) { @Override public void execute() { - if (posX.hasChanged(0) || posY.hasChanged(0) || poseYaw.hasChanged(0) - || distanceToTag.hasChanged(0) || apriltagNumber.hasChanged(0) || latency.hasChanged(0)) { + april.addSimReading(new ApriltagReading(posX.getAsDouble(), posY.getAsDouble(), poseYaw.getAsDouble(), distanceToTag.getAsDouble(), (int) apriltagNumber.getAsDouble(), latency.getAsDouble(), measurementTime.getAsDouble())); - } + } } diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 4b0e34a8..c3e55919 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -12,13 +12,11 @@ public class ApriltagSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "ApriltagSubsystem"; private final ApriltagIO io; private final PoseEstimator estimator; - private static ApriltagInputs inputs; private final SwerveSubsystem drivebase; public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { this.drivebase = drivebase; this.io = io; - inputs = new ApriltagInputs(); drivebase.setVariance(VecBuilder.fill(10,10,10)); estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); } @@ -39,10 +37,6 @@ public static ApriltagIO createSimIo() { public void addSimReading(ApriltagReading reading) { io.addReading(reading); } - public ApriltagInputs getInputs(){ - return inputs; - } - public ApriltagIO getIO(){ return io; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 742c0108..872413e7 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -113,21 +113,21 @@ private boolean validAprilTagPose(double[] measurement) { } public void updateInputs(){ - apriltagSystem.getIO().updateInputs(apriltagSystem.getInputs()); + apriltagSystem.getIO().updateInputs(apriltagSystem.getIO().getInputs()); } private void updateVision(int... invalidApriltagNumbers) { long start = System.currentTimeMillis(); if (Constants.ENABLE_VISION && Robot.getMode() != RobotMode.DISABLED) { - for (int i = 0; i < apriltagSystem.getInputs().timestamp.length; i++) { + for (int i = 0; i < apriltagSystem.getIO().getInputs().timestamp.length; i++) { double[] pos = new double[] { - apriltagSystem.getInputs().posX[i], - apriltagSystem.getInputs().posY[i], - apriltagSystem.getInputs().poseYaw[i] + apriltagSystem.getIO().getInputs().posX[i], + apriltagSystem.getIO().getInputs().posY[i], + apriltagSystem.getIO().getInputs().poseYaw[i] }; if (validAprilTagPose(pos) && !ArrayUtils.contains( - invalidApriltagNumbers, apriltagSystem.getInputs().apriltagNumber[i])) { + invalidApriltagNumbers, apriltagSystem.getIO().getInputs().apriltagNumber[i])) { VisionMeasurement measurement = getVisionMeasurement(pos, i); poseManager.registerVisionMeasurement(measurement); } else { @@ -142,11 +142,11 @@ private void updateVision(int... invalidApriltagNumbers) { } private VisionMeasurement getVisionMeasurement(double[] pos, int index) { - double serverTime = apriltagSystem.getInputs().serverTime[index]; + double serverTime = apriltagSystem.getIO().getInputs().serverTime[index]; double timestamp = 0; // latency is not right we are assuming zero double latencyInSec = (serverTime - timestamp) / 1000; Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); - double distanceFromTag = apriltagSystem.getInputs().distanceToTag[index]; + double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; return new VisionMeasurement(visionPose, distanceFromTag, latencyInSec); } diff --git a/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java b/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java index bc39c5a1..0fe698cb 100644 --- a/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java +++ b/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java @@ -10,7 +10,7 @@ public abstract class BaseIoImpl implements BaseIo { // The name of the "folder" where the logs from this IO will be logged private final String prefix; - private final I inputs; + protected final I inputs; public BaseIoImpl(String folder, I inputs) { this.inputs = inputs; From dd2958bb91699edb5f2257becfdc7c0bf58dea76 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 9 Feb 2026 20:48:55 -0500 Subject: [PATCH 18/67] smolchangefs --- src/main/java/frc/robot/apriltags/ApriltagIO.java | 1 - src/main/java/frc/robot/subsystems/ApriltagSubsystem.java | 1 - .../swervedrive/vision/estimation/PoseEstimator.java | 3 --- 3 files changed, 5 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/ApriltagIO.java b/src/main/java/frc/robot/apriltags/ApriltagIO.java index e667bec6..11b5bcc5 100644 --- a/src/main/java/frc/robot/apriltags/ApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/ApriltagIO.java @@ -5,6 +5,5 @@ public interface ApriltagIO extends BaseIo { // This is used to inject april tag readings manually and will pretty much only be used for simulation. void addReading(ApriltagReading reading); - void updateInputs(ApriltagInputs inputs); ApriltagInputs getInputs(); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index c3e55919..3e4a25b6 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -43,7 +43,6 @@ public ApriltagIO getIO(){ @Override public void periodic() { - estimator.updateInputs(); estimator.updateVision(); drivebase.addVisionMeasurement(estimator.getEstimatedPose()); io.periodic(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 872413e7..4ab117c3 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -112,9 +112,6 @@ private boolean validAprilTagPose(double[] measurement) { return !ArrayUtils.allMatch(measurement, -1.0) && measurement.length == 3; } - public void updateInputs(){ - apriltagSystem.getIO().updateInputs(apriltagSystem.getIO().getInputs()); - } private void updateVision(int... invalidApriltagNumbers) { long start = System.currentTimeMillis(); if (Constants.ENABLE_VISION && Robot.getMode() != RobotMode.DISABLED) { From 178d371efe4d0ffd1d34b855dbe98fd99100ff54 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 11 Feb 2026 20:05:42 -0500 Subject: [PATCH 19/67] made it look like less ig --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e6e9434f..1bc3cf95 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.AddTunableApriltagReading; import frc.robot.commands.AddApriltagReading; +import frc.robot.commands.AddGarbageReading; import frc.robot.commands.hopper.SpinHopper; import frc.robot.commands.drive.DriveDirectionTime; import frc.robot.commands.feeder.SpinFeeder; @@ -247,7 +248,7 @@ public void putShuffleboardCommands() { new SetShootingState(shootState, ShootState.SHUTTLING)); SmartDashboard.putData("AddTunedApriltagReading", new AddTunableApriltagReading(apriltagSubsystem)); SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem,new ApriltagReading(0, 0, 0, 0, 0, 0, 0))); - + SmartDashboard.putData("AddGarbageReading", new AddGarbageReading(apriltagSubsystem)); } //basic drive command if(!Constants.TESTBED){ From bd8dba238a450a39111b4d269d319d1fec9b992f Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 11 Feb 2026 20:09:26 -0500 Subject: [PATCH 20/67] nd --- .../frc/robot/commands/AddGarbageReading.java | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 src/main/java/frc/robot/commands/AddGarbageReading.java diff --git a/src/main/java/frc/robot/commands/AddGarbageReading.java b/src/main/java/frc/robot/commands/AddGarbageReading.java new file mode 100644 index 00000000..5cfb2fd6 --- /dev/null +++ b/src/main/java/frc/robot/commands/AddGarbageReading.java @@ -0,0 +1,39 @@ +package frc.robot.commands; + +import edu.wpi.first.epilogue.Logged; +import frc.robot.apriltags.ApriltagReading; +import frc.robot.subsystems.ApriltagSubsystem; +import frc.robot.utils.logging.LoggedTunableNumber; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class AddGarbageReading extends LoggableCommand { + private final ApriltagSubsystem april; + private LoggedTunableNumber posX; + private LoggedTunableNumber posY; + private LoggedTunableNumber poseYaw; + private LoggedTunableNumber distanceToTag; + private LoggedTunableNumber apriltagNumber; + private LoggedTunableNumber latency; + private LoggedTunableNumber measurementTime; + public AddGarbageReading(ApriltagSubsystem april) { + this.april = april; + posX = new LoggedTunableNumber("SimAprilTagX", 0); + posY = new LoggedTunableNumber("SimAprilTagY", 0); + poseYaw = new LoggedTunableNumber("SimAprilTagYaw", 0); + distanceToTag = new LoggedTunableNumber("SimAprilTagDistanceToTag", 0); + apriltagNumber = new LoggedTunableNumber("SimAprilTagApriltagNum", 0); + latency = new LoggedTunableNumber("SimAprilTagLatency", 0); + measurementTime = new LoggedTunableNumber("SimAprilTagMeasurementTime", 0); + } + + @Override + public void execute() { + + april.addSimReading(new ApriltagReading(Math.random()*100, Math.random()*100, Math.random()*100, Math.random()*100, (int) (Math.random()*32+1), Math.random()*100, Math.random()*100)); + + } + @Override + public boolean isFinished() { + return false; + } +} From 5af7386c0c4a32459cb3b445add642acb600237e Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Fri, 13 Feb 2026 20:25:57 -0500 Subject: [PATCH 21/67] changed for testing --- src/main/java/frc/robot/commands/AddGarbageReading.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/AddGarbageReading.java b/src/main/java/frc/robot/commands/AddGarbageReading.java index 5cfb2fd6..89924a76 100644 --- a/src/main/java/frc/robot/commands/AddGarbageReading.java +++ b/src/main/java/frc/robot/commands/AddGarbageReading.java @@ -29,7 +29,7 @@ public AddGarbageReading(ApriltagSubsystem april) { @Override public void execute() { - april.addSimReading(new ApriltagReading(Math.random()*100, Math.random()*100, Math.random()*100, Math.random()*100, (int) (Math.random()*32+1), Math.random()*100, Math.random()*100)); + april.addSimReading(new ApriltagReading(Math.random()*10, Math.random()*10, Math.random()*10, Math.random()*10, (int) (Math.random()*32+1), Math.random()*10, Math.random()*10)); } @Override diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 9ed1e83b..b18ed114 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -112,7 +112,7 @@ public enum Mode { public static final int SHIFT_4_START = 55; public static final int ENDGAME_START = 30; - public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; + public static final double VISION_CONSISTANCY_THRESHOLD = 100.25; public static final boolean ENABLE_VISION = true; public static final double POSE_BUFFER_STORAGE_TIME = 2; } From f50030a45ac07cd477c038ecd79b2ad98216687a Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 14 Feb 2026 13:37:27 -0500 Subject: [PATCH 22/67] newest changes --- .../java/frc/robot/commands/AddTunableApriltagReading.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/AddTunableApriltagReading.java b/src/main/java/frc/robot/commands/AddTunableApriltagReading.java index 20b6ed8d..8c392b3e 100644 --- a/src/main/java/frc/robot/commands/AddTunableApriltagReading.java +++ b/src/main/java/frc/robot/commands/AddTunableApriltagReading.java @@ -15,6 +15,7 @@ public class AddTunableApriltagReading extends LoggableCommand { private LoggedTunableNumber apriltagNumber; private LoggedTunableNumber latency; private LoggedTunableNumber measurementTime; + private LoggedTunableNumber numReadings; public AddTunableApriltagReading(ApriltagSubsystem april) { this.april = april; posX = new LoggedTunableNumber("SimAprilTagX", 0); @@ -24,14 +25,15 @@ public AddTunableApriltagReading(ApriltagSubsystem april) { apriltagNumber = new LoggedTunableNumber("SimAprilTagApriltagNum", 0); latency = new LoggedTunableNumber("SimAprilTagLatency", 0); measurementTime = new LoggedTunableNumber("SimAprilTagMeasurementTime", 0); + numReadings = new LoggedTunableNumber("NumReadingsPerTick", 1); } @Override public void execute() { - + for (int i=0; i Date: Sat, 14 Feb 2026 14:15:04 -0500 Subject: [PATCH 23/67] Added queue readings --- .../java/frc/robot/subsystems/ApriltagSubsystem.java | 1 + .../robot/subsystems/swervedrive/SwerveSubsystem.java | 4 ++++ .../swervedrive/vision/estimation/PoseEstimator.java | 6 +++--- .../swervedrive/vision/estimation/PoseManager.java | 10 +++++----- 4 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 3e4a25b6..9a66c222 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -44,6 +44,7 @@ public ApriltagIO getIO(){ @Override public void periodic() { estimator.updateVision(); + estimator.updatePosition(drivebase.getOdom()); drivebase.addVisionMeasurement(estimator.getEstimatedPose()); io.periodic(); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index eda785e0..423610f1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -297,6 +297,10 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + //fix to only get odomtry + public Pose2d getOdom() { + return swerveDrive.getPose(); + } /** * Set chassis speeds with closed-loop velocity control. diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 4ab117c3..b23b313a 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -101,12 +101,12 @@ public Pose2d getVisionPose(VisionMeasurement measurement) { * * @see SwerveDrivePoseEstimator#update(Rotation2d, SwerveModulePosition[]) */ - /*public void updatePosition(OdometryMeasurement m) { + public void updatePosition(Pose2d pose) { if (!Robot.getMode().equals(RobotMode.DISABLED)) { - poseManager.addOdomMeasurement(m, Logger.getTimestamp()); + poseManager.addOdomMeasurement(pose, Logger.getTimestamp()); } field.setRobotPose(poseManager.getEstimatedPosition()); - }*/ + } private boolean validAprilTagPose(double[] measurement) { return !ArrayUtils.allMatch(measurement, -1.0) && measurement.length == 3; diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index c71e6cd9..b5ebe7c2 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -52,12 +52,12 @@ public PoseManager( TimeInterpolatableBuffer estimatedPoseBuffer) { this(new PoseDeviation(visionStd), kinematics, drivebase, estimatedPoseBuffer); } -/* - public void addOdomMeasurement(OdometryMeasurement m, long timestamp) { - Rotation2d gyroVal = Rotation2d.fromDegrees(m.gyroValueDeg()); - Pose2d pose = poseEstimator.update(gyroVal, m.modulePosition()); + + public void addOdomMeasurement(Pose2d pose, long timestamp) { + // Rotation2d gyroVal = Rotation2d.fromDegrees(pose.getRotation()); + //Pose2d pose = poseEstimator.update(gyroVal, m.modulePosition()); estimatedPoseBuffer.addSample(timestamp, pose); - }*/ + } public void registerVisionMeasurement(VisionMeasurement measurement) { if (measurement == null) { From 59b3c9a2bd135d6801a8ef61662fbdccb4e57010 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 14 Feb 2026 14:16:07 -0500 Subject: [PATCH 24/67] feagfg --- .../java/frc/robot/commands/AddTunableApriltagReading.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/AddTunableApriltagReading.java b/src/main/java/frc/robot/commands/AddTunableApriltagReading.java index 8c392b3e..306dd5da 100644 --- a/src/main/java/frc/robot/commands/AddTunableApriltagReading.java +++ b/src/main/java/frc/robot/commands/AddTunableApriltagReading.java @@ -1,6 +1,7 @@ package frc.robot.commands; -import edu.wpi.first.epilogue.Logged; +import org.littletonrobotics.junction.Logger; + import frc.robot.apriltags.ApriltagReading; import frc.robot.subsystems.ApriltagSubsystem; import frc.robot.utils.logging.LoggedTunableNumber; @@ -14,7 +15,6 @@ public class AddTunableApriltagReading extends LoggableCommand { private LoggedTunableNumber distanceToTag; private LoggedTunableNumber apriltagNumber; private LoggedTunableNumber latency; - private LoggedTunableNumber measurementTime; private LoggedTunableNumber numReadings; public AddTunableApriltagReading(ApriltagSubsystem april) { this.april = april; @@ -24,7 +24,6 @@ public AddTunableApriltagReading(ApriltagSubsystem april) { distanceToTag = new LoggedTunableNumber("SimAprilTagDistanceToTag", 0); apriltagNumber = new LoggedTunableNumber("SimAprilTagApriltagNum", 0); latency = new LoggedTunableNumber("SimAprilTagLatency", 0); - measurementTime = new LoggedTunableNumber("SimAprilTagMeasurementTime", 0); numReadings = new LoggedTunableNumber("NumReadingsPerTick", 1); } @@ -33,7 +32,7 @@ public void execute() { for (int i=0; i Date: Sat, 14 Feb 2026 14:22:33 -0500 Subject: [PATCH 25/67] deleted print statements --- .../swervedrive/vision/estimation/FilterablePoseManager.java | 1 - .../swervedrive/vision/truster/ConstantVisionTruster.java | 1 - .../swervedrive/vision/truster/SquareVisionTruster.java | 1 - 3 files changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java index 721d6ef6..3409fa1d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java @@ -65,7 +65,6 @@ public void processQueue() { FilterResult r = entry.getValue(); switch (r) { case ACCEPTED -> { - System.out.println("This isprint"); setVisionSTD(visionTruster.calculateTrust(v)); validMeasurements.add(v.measurement()); addVisionMeasurement(v); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java index 13e2c133..3017ccb1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java @@ -12,7 +12,6 @@ public ConstantVisionTruster(Vector initialSTD) { @Override public Vector calculateTrust(VisionMeasurement measurement) { - System.out.println("Thisbroooo it like sort kinda works cuz its like amazing and it will work cuz immmm sooooo coool"); return initialSTD; } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java index df882657..7a925fc8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java @@ -16,7 +16,6 @@ public SquareVisionTruster(Vector initialSTD, double constant) { @Override public Vector calculateTrust(VisionMeasurement measurement) { - System.out.println("This is a print"+initialSTD+"This isa measurment"+measurement); double std = Math.pow(measurement.distanceFromTag() - 0.4572, 2) * constant; return initialSTD.plus(VecBuilder.fill(std, std, std)); } From 6913c76bc8707807a3c8c9eb15cf8e5f37e80933 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 14 Feb 2026 15:27:09 -0500 Subject: [PATCH 26/67] small change to vision threshhold --- src/main/java/frc/robot/constants/GameConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index b18ed114..9ed1e83b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -112,7 +112,7 @@ public enum Mode { public static final int SHIFT_4_START = 55; public static final int ENDGAME_START = 30; - public static final double VISION_CONSISTANCY_THRESHOLD = 100.25; + public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; public static final boolean ENABLE_VISION = true; public static final double POSE_BUFFER_STORAGE_TIME = 2; } From 1e6676b3321974080ddd23cf9aa31cbd48042a11 Mon Sep 17 00:00:00 2001 From: ohad Date: Sat, 14 Feb 2026 16:57:38 -0500 Subject: [PATCH 27/67] Fix after rebase --- src/main/java/frc/robot/RobotContainer.java | 48 +++++++++------------ 1 file changed, 20 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 836eaaa8..c591263d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,6 @@ package frc.robot; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -46,11 +44,7 @@ import frc.robot.utils.simulation.RobotVisualizer; import swervelib.SwerveInputStream; import swervelib.imu.SwerveIMU; -import frc.robot.utils.logging.io.BaseIoImpl; -import frc.robot.apriltags.ApriltagInputs; import frc.robot.apriltags.ApriltagReading; -import frc.robot.apriltags.MockApriltagIo; -import frc.robot.apriltags.TCPApriltagIo; import java.io.File; @@ -78,7 +72,7 @@ public class RobotContainer { private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT); private ShootingState shootState = new ShootingState(ShootState.STOPPED); - // Replace with CommandPS4Controller or CommandJoystick if needed + // Replace with CommandPS4Controller or CommandJoystick if needed //new CommandXboxController(OperatorConstants.kDriverControllerPort);private final CommandXboxController controller = new CommandXboxController(Constants.XBOX_CONTROLLER_PORT); /** @@ -96,7 +90,6 @@ public RobotContainer() { feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo()); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createRealIo()); RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo(); ThreadedGyro threadedGyro = gyroIo.getThreadedGyro(); gyroSubsystem = new GyroSubsystem(gyroIo); @@ -111,7 +104,6 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo(), IntakeSubsystem.createMockDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo()); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createMockIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) @@ -126,7 +118,6 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer), IntakeSubsystem.createSimDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer)); feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) @@ -165,13 +156,13 @@ private void configureBindings() { //example default command for angler- disabled for now if (false) { - new AimAngler( - anglerSubsystem, - () -> drivebase != null ? drivebase.getPose() : null, - shootState); + new AimAngler( + anglerSubsystem, + () -> drivebase != null ? drivebase.getPose() : null, + shootState); } - if(!Constants.TESTBED){ + if (!Constants.TESTBED) { SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), () -> driveJoystick.getY() * -1, () -> driveJoystick.getX() * -1) @@ -198,7 +189,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Tilt Down", new TiltDown(tiltSubsystem));*/ - + // TODO: These commands do not REQUIRE the subsystem therefore cannot be used in production SmartDashboard.putData( "Intake/Spin Forward", @@ -259,11 +250,11 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Spin Intake", new SpinIntake(intakeSubsystem)); - + SmartDashboard.putData( "Start Hopper", new SpinHopper(hopperSubsystem)); - + SmartDashboard.putData( "Spin Feeder", new SpinFeeder(feederSubsystem)); @@ -275,7 +266,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Shooting State: Stopped", new SetShootingState(shootState, ShootState.STOPPED)); - + SmartDashboard.putData( "Shooting State: Fixed", new SetShootingState(shootState, ShootState.FIXED)); @@ -292,13 +283,13 @@ public void putShuffleboardCommands() { "Shooting State: Shuttling", new SetShootingState(shootState, ShootState.SHUTTLING)); SmartDashboard.putData("AddTunedApriltagReading", new AddTunableApriltagReading(apriltagSubsystem)); - SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem,new ApriltagReading(0, 0, 0, 0, 0, 0, 0))); + SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem, new ApriltagReading(0, 0, 0, 0, 0, 0, 0))); SmartDashboard.putData("AddGarbageReading", new AddGarbageReading(apriltagSubsystem)); } - //basic drive command - if(!Constants.TESTBED) { - Command driveDirectionTime = new DriveDirectionTime(drivebase, 0.1,0.1, true, 1); + //basic drive command + if (!Constants.TESTBED) { + Command driveDirectionTime = new DriveDirectionTime(drivebase, 0.1, 0.1, true, 1); SmartDashboard.putData("Drive Command", driveDirectionTime); SmartDashboard.putData("Fake vision", new FakeVision(drivebase)); } @@ -311,22 +302,23 @@ public void putShuffleboardCommands() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return autoChooser.getCommand(); + return autoChooser.getCommand(); } public RobotVisualizer getRobotVisualizer() { - return robotVisualizer; + return robotVisualizer; } public AutoChooser getAutoChooser() { - return autoChooser; + return autoChooser; } public IntakeSubsystem getIntakeSubsystem() { return intakeSubsystem; } - public SwerveSubsystem getDriveBase(){ - return drivebase; + + public SwerveSubsystem getDriveBase() { + return drivebase; } public ShootingState getShootingState() { From 8ac445d1341cd54dc9309e68a970cf1b961a49da Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 14 Feb 2026 17:06:51 -0500 Subject: [PATCH 28/67] small fix for testbed --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c591263d..2a4bf051 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,7 +95,7 @@ public RobotContainer() { gyroSubsystem = new GyroSubsystem(gyroIo); SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro); drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null; - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase); + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase) : null; } case REPLAY -> { //rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); @@ -108,7 +108,7 @@ public RobotContainer() { // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase); + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null; } case SIM -> { robotVisualizer = new RobotVisualizer(); @@ -122,7 +122,7 @@ public RobotContainer() { // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase); + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null; } default -> { From 0943fcb07631650a92e03bdf80d15f58876da37a Mon Sep 17 00:00:00 2001 From: ohad Date: Sun, 15 Feb 2026 16:28:22 -0500 Subject: [PATCH 29/67] Change the pose manager to return the vision estimate rather than the drivebase one --- .../robot/subsystems/ApriltagSubsystem.java | 6 +- .../vision/estimation/PoseEstimator.java | 9 +- .../vision/estimation/PoseManager.java | 127 +++++++++--------- 3 files changed, 77 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 9a66c222..df7bc49e 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.apriltags.*; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -45,7 +46,10 @@ public ApriltagIO getIO(){ public void periodic() { estimator.updateVision(); estimator.updatePosition(drivebase.getOdom()); - drivebase.addVisionMeasurement(estimator.getEstimatedPose()); + Pose2d estimatedPose = estimator.getEstimatedPose(); + if (estimatedPose != null) { + drivebase.addVisionMeasurement(estimatedPose); + } io.periodic(); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index b23b313a..f402fcf8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -19,7 +18,6 @@ import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; -import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; import frc.robot.utils.Apriltag; import frc.robot.utils.math.ArrayUtils; @@ -82,7 +80,7 @@ public PoseEstimator( this.poseManager = new FilterablePoseManager( visionMeasurementStdDevs2, - kinematics, + kinematics, drivebase, m1Buffer, new BasicVisionFilter(m1Buffer) { @@ -105,7 +103,10 @@ public void updatePosition(Pose2d pose) { if (!Robot.getMode().equals(RobotMode.DISABLED)) { poseManager.addOdomMeasurement(pose, Logger.getTimestamp()); } - field.setRobotPose(poseManager.getEstimatedPosition()); + Pose2d estimatedPosition = poseManager.getEstimatedPosition(); + if (estimatedPosition != null) { + field.setRobotPose(estimatedPosition); + } } private boolean validAprilTagPose(double[] measurement) { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index b5ebe7c2..cab2809f 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -1,38 +1,36 @@ package frc.robot.subsystems.swervedrive.vision.estimation; import edu.wpi.first.math.Vector; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.PoseDeviation; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; -import swervelib.SwerveDrive; +import org.littletonrobotics.junction.Logger; import java.util.LinkedList; +import java.util.Optional; import java.util.Queue; -import org.littletonrobotics.junction.Logger; /** * Processes swerve odometry. Feeds odometry measurements and vision measurements into a Kalman * Filter which outputs a combined robot position */ public class PoseManager { - private final TimeInterpolatableBuffer estimatedPoseBuffer; - //private final SwerveDrivePoseEstimator poseEstimator; - protected final Queue visionMeasurementQueue = new LinkedList<>(); - private final SwerveSubsystem drivebase; + private final TimeInterpolatableBuffer estimatedPoseBuffer; + //private final SwerveDrivePoseEstimator poseEstimator; + protected final Queue visionMeasurementQueue = new LinkedList<>(); + private final SwerveSubsystem drivebase; - public PoseManager( - PoseDeviation PoseDeviation, - SwerveDriveKinematics kinematics, - SwerveSubsystem drivebase, - //OdometryMeasurement initialOdom, - TimeInterpolatableBuffer estimatedPoseBuffer) { + public PoseManager( + PoseDeviation PoseDeviation, + SwerveDriveKinematics kinematics, + SwerveSubsystem drivebase, + //OdometryMeasurement initialOdom, + TimeInterpolatableBuffer estimatedPoseBuffer) { /* this.poseEstimator = new SwerveDrivePoseEstimator( kinematics, @@ -41,54 +39,54 @@ public PoseManager( new Pose2d(), PoseDeviation.getWheelStd(), PoseDeviation.getVisionStd());*/ - this.estimatedPoseBuffer = estimatedPoseBuffer; - this.drivebase = drivebase; - } + this.estimatedPoseBuffer = estimatedPoseBuffer; + this.drivebase = drivebase; + } - public PoseManager( - Vector visionStd, - SwerveDriveKinematics kinematics, - SwerveSubsystem drivebase, - TimeInterpolatableBuffer estimatedPoseBuffer) { - this(new PoseDeviation(visionStd), kinematics, drivebase, estimatedPoseBuffer); - } + public PoseManager( + Vector visionStd, + SwerveDriveKinematics kinematics, + SwerveSubsystem drivebase, + TimeInterpolatableBuffer estimatedPoseBuffer) { + this(new PoseDeviation(visionStd), kinematics, drivebase, estimatedPoseBuffer); + } - public void addOdomMeasurement(Pose2d pose, long timestamp) { - // Rotation2d gyroVal = Rotation2d.fromDegrees(pose.getRotation()); - //Pose2d pose = poseEstimator.update(gyroVal, m.modulePosition()); - estimatedPoseBuffer.addSample(timestamp, pose); - } + public void addOdomMeasurement(Pose2d pose, long timestamp) { + // Rotation2d gyroVal = Rotation2d.fromDegrees(pose.getRotation()); + //Pose2d pose = poseEstimator.update(gyroVal, m.modulePosition()); + estimatedPoseBuffer.addSample(timestamp, pose); + } - public void registerVisionMeasurement(VisionMeasurement measurement) { - if (measurement == null) { - return; + public void registerVisionMeasurement(VisionMeasurement measurement) { + if (measurement == null) { + return; + } + while (visionMeasurementQueue.size() >= 3) { + visionMeasurementQueue.poll(); + } + visionMeasurementQueue.add(measurement); } - while (visionMeasurementQueue.size() >= 3) { - visionMeasurementQueue.poll(); + + // override for filtering + public void processQueue() { + VisionMeasurement m = visionMeasurementQueue.poll(); + while (m != null) { + addVisionMeasurement(m); + m = visionMeasurementQueue.poll(); + } } - visionMeasurementQueue.add(measurement); - } - // override for filtering - public void processQueue() { - VisionMeasurement m = visionMeasurementQueue.poll(); - while (m != null) { - addVisionMeasurement(m); - m = visionMeasurementQueue.poll(); + protected void addVisionMeasurement(VisionMeasurement measurement) { + drivebase.addVisionMeasurement(measurement.measurement(), measurement.timeOfMeasurement()); } - } - protected void addVisionMeasurement(VisionMeasurement measurement) { - drivebase.addVisionMeasurement(measurement.measurement(), measurement.timeOfMeasurement()); - } + protected void setVisionSTD(Vector visionMeasurementStdDevs123) { + Logger.recordOutput( + "Apriltag/VisionAppliedCovariance", + new double[]{visionMeasurementStdDevs123.get(0), visionMeasurementStdDevs123.get(1)}); - protected void setVisionSTD(Vector visionMeasurementStdDevs123) { - Logger.recordOutput( - "Apriltag/VisionAppliedCovariance", - new double[] {visionMeasurementStdDevs123.get(0), visionMeasurementStdDevs123.get(1)}); - - drivebase.setVariance(visionMeasurementStdDevs123); - } + drivebase.setVariance(visionMeasurementStdDevs123); + } /* public void resetPose(OdometryMeasurement m, Translation2d initialPose) { poseEstimator.resetPosition( @@ -97,11 +95,20 @@ public void resetPose(OdometryMeasurement m, Translation2d initialPose) { new Pose2d(initialPose, Rotation2d.fromDegrees(m.gyroValueDeg()))); }*/ - public TimeInterpolatableBuffer getPoseBuffer() { - return estimatedPoseBuffer; - } + public TimeInterpolatableBuffer getPoseBuffer() { + return estimatedPoseBuffer; + } - public Pose2d getEstimatedPosition() { - return drivebase.getPose(); - } + /** + * Get the estimated position from the vision pose estimator. + * @return the current pose estimation, null if none found + */ + public Pose2d getEstimatedPosition() { + Optional sample = estimatedPoseBuffer.getSample(Timer.getFPGATimestamp()); + if (sample.isEmpty()) { + return null; + } else { + return sample.get(); + } + } } From ac36a47f58be199ecc7241ffc73c90ddca62a6dd Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 15 Feb 2026 18:26:35 -0500 Subject: [PATCH 30/67] completely fixed -_- --- .idea/compiler.xml | 7 +++++-- .idea/gradle.xml | 1 + .idea/misc.xml | 2 +- .idea/modules.xml | 2 +- .../commands/AddTunableApriltagReading.java | 19 +++++++++++-------- .../robot/subsystems/ApriltagSubsystem.java | 4 ---- 6 files changed, 19 insertions(+), 16 deletions(-) diff --git a/.idea/compiler.xml b/.idea/compiler.xml index 3b8c5d3b..8dc901c4 100644 --- a/.idea/compiler.xml +++ b/.idea/compiler.xml @@ -5,12 +5,15 @@ - - + + + + \ No newline at end of file diff --git a/.idea/gradle.xml b/.idea/gradle.xml index ce1c62c7..0cb1776d 100644 --- a/.idea/gradle.xml +++ b/.idea/gradle.xml @@ -5,6 +5,7 @@