From 88b1f69786028b45b7801e587ee9140177128a3b Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 21 Feb 2026 17:30:41 -0500 Subject: [PATCH 01/13] initial commit --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/drive/DriveCircle.java | 44 ++++++ .../frc/robot/constants/GameConstants.java | 12 +- .../robot/subsystems/ApriltagSubsystem.java | 41 +++++- .../swervedrive/SwerveSubsystem.java | 3 + .../vision/estimation/PoseEstimator.java | 4 + .../vision/truster/BasicVisionFilter.java | 4 +- src/main/java/frc/robot/utils/Apriltag.java | 138 +++++++++++------- 8 files changed, 194 insertions(+), 54 deletions(-) create mode 100644 src/main/java/frc/robot/commands/drive/DriveCircle.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 37632535..326ca6c7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -538,7 +538,7 @@ public SwerveSubsystem getDriveBase() { return drivebase; } - public ShootingState getShootingState() { + public ShootingState getShootingState() { return shootState; } } diff --git a/src/main/java/frc/robot/commands/drive/DriveCircle.java b/src/main/java/frc/robot/commands/drive/DriveCircle.java new file mode 100644 index 00000000..76366ba8 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveCircle.java @@ -0,0 +1,44 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DriveCircle extends LoggableCommand { + private final SwerveSubsystem drivebase; + private final double radiusMeters; + private final double angularVelocityRadPerSec; + private final double time; + private final Timer timer; + public DriveCircle(SwerveSubsystem drivebase, double radiusMeters, double angularVelocityRadPerSec, double time) { + this.drivebase = drivebase; + this.radiusMeters = radiusMeters; + this.angularVelocityRadPerSec = angularVelocityRadPerSec; + this.time = time; + this.timer = new Timer(); + addRequirements(drivebase); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + double tangentialSpeed = radiusMeters * angularVelocityRadPerSec; + double yVelocity = -tangentialSpeed; + drivebase.drive(new Translation2d(0, yVelocity), angularVelocityRadPerSec, false); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(time); + } + + @Override + public void end(boolean interrupted) { + drivebase.drive(new Translation2d(), 0, false); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 5e9cf45f..bd7205ba 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,6 +1,8 @@ package frc.robot.constants; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -105,7 +107,7 @@ public enum Mode { public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_LIMIT_SPEED = 0.2; - + // turret (pan angle) PID public static final double TURRET_P = .5; public static final double TURRET_I = 0.000001; @@ -149,4 +151,12 @@ public enum Mode { public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed) public static final boolean ENABLE_VISION = true; public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back) + + // Vision + public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change + public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later + public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later + public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later + public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later + public static final double MAX_VISION_DISTANCE_SIMULATION = 60; } diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index adb2bfc9..9470e707 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -1,12 +1,24 @@ package frc.robot.subsystems; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.apriltags.*; +import frc.robot.constants.Constants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; +import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.utils.Apriltag; import frc.robot.utils.logging.io.BaseIoImpl; +import org.littletonrobotics.junction.Logger; + +import java.util.Random; +import java.util.function.Supplier; public class ApriltagSubsystem extends SubsystemBase { @@ -14,11 +26,15 @@ public class ApriltagSubsystem extends SubsystemBase { private final ApriltagIO io; private final PoseEstimator estimator; private final SwerveSubsystem drivebase; + private final Random random = new Random(); + private final Supplier robotPoseSupplier; + public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { this.drivebase = drivebase; this.io = io; - estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); + estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); + robotPoseSupplier = drivebase::getSimulationPose; } public static ApriltagIO createRealIo() { @@ -47,4 +63,27 @@ public void periodic() { estimator.updatePosition(drivebase.getOdom()); io.periodic(); } + public void simReadings() { + for (Apriltag tag : Apriltag.values()) { + Pose3d cameraPos = new Pose3d(robotPoseSupplier.get()).transformBy(Constants.ROBOT_TO_CAMERA); + if (tag.canSee(cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { + Pose3d adjPose = tag.getPose().relativeTo(cameraPos); + double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); + double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); + if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0); + Vector stdDevs = estimator.getVisionTruster().calculateTrust(measurement); + double readingX = robotPoseSupplier.get().getX() + random.nextGaussian() * stdDevs.get(0); + double readingY = robotPoseSupplier.get().getY() + random.nextGaussian() * stdDevs.get(1); + double readingYaw = robotPoseSupplier.get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); + Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); + distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); + if (BasicVisionFilter.inBounds(readingPos)) { + io.addReading(new ApriltagReading(readingX, readingY, readingYaw, + distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0)); + } + } + } + } + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 570876f4..980b7b1b 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -297,6 +297,9 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + public Pose2d getSimulationPose() { + return swerveDrive.getSimulationDriveTrainPose().orElse(new Pose2d()); + } // Todo: fix to only get odomtry public Pose2d getOdom() { return swerveDrive.getPose(); 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 e5ef49ae..5426c80a 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.BasicVisionFilter; import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.Apriltag; import frc.robot.utils.math.ArrayUtils; @@ -202,4 +203,7 @@ public void addMockVisionMeasurement() { poseManager.registerVisionMeasurement( new VisionMeasurement(getEstimatedPose(), 0, Logger.getTimestamp() / 1e6)); } + public VisionTruster getVisionTruster() { + return poseManager.getVisionTruster(); + } } 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 index 5d160e28..ae99851c 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -79,7 +79,7 @@ private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD; } - private boolean inBounds(Pose2d pose2d) { - return pose2d.getX() > 0 && pose2d.getX() < 20 && pose2d.getY() > 0 && pose2d.getY() < 20; + public static boolean inBounds(Pose2d pose2d) { + return pose2d.getX() > 0 && pose2d.getX() < 16.5 && pose2d.getY() > 0 && pose2d.getY() < 8.1; } } diff --git a/src/main/java/frc/robot/utils/Apriltag.java b/src/main/java/frc/robot/utils/Apriltag.java index 66449f22..66be774e 100644 --- a/src/main/java/frc/robot/utils/Apriltag.java +++ b/src/main/java/frc/robot/utils/Apriltag.java @@ -1,40 +1,49 @@ package frc.robot.utils; -import edu.wpi.first.math.geometry.Translation3d; - -public enum Apriltag { //Andymark field: - ONE(467.08,291.79, 35.00), //Trench, Red z rotation:180 - TWO(468.56,182.08, 44.25), //Hub, Red z rotation:90 - THREE( 444.80,172.32, 44.25), //Hub, Red z rotation:180 - FOUR(444.80,158.32, 44.25), //Hub, Red z rotation:180 - FIVE(468.56,134.56, 44.25), //Hub, Red z rotation:270 - SIX(467.08,24.85, 35.00), //Trench, Red z rotation:180 - SEVEN(470.03,24.85, 35.00), //Trench, Red z rotation:0 - EIGHT(482.56,134.56, 44.25), //Hub, Red z rotation:270 - NINE(492.33,144.32, 44.25), //Hub, Red z rotation:0 - TEN(492.33,158.32, 44.25), //Hub, Red z rotation:0 - ELEVEN(482.56,182.08, 44.25), //Hub, Red z rotation:90 - TWELVE(470.03,291.79, 35.00), //Trench, Red z rotation:0 - THIRTEEN(649.58,291.02, 21.75), //Outpost, Red z rotation:180 - FOURTEEN(649.58,274.02, 21.75), //Outpost, Red z rotation:180 - FIFTEEN(649.57,169.78, 21.75), //Tower, Red z rotation:180 - SIXTEEN(649.57,152.78, 21.75), //Tower, Red z rotation:180 - SEVENTEEN(183.03,24.85, 35.00), //Trench, Blue z rotation:0 - EIGHTEEN(181.56,134.56, 44.25), //Hub, Blue z rotation:270 - NINETEEN(205.32,144.32, 44.25), //Hub, Blue z rotation:0 - TWENTY(205.32,158.32, 44.52), //Hub, Blue z rotation:0 - TWENTY_ONE(181.56,182.08, 44.25), //Hub, Blue z rotation:90 - TWENTY_TWO(183.03, 291.79, 35.00), //Trench, Blue z rotation:0 - TWENTY_THREE(180.08, 291.79, 35.00),//Trench, Blue z rotation:180 - TWENTY_FOUR(167.56, 182.08, 44.25),//Hub, Blue z rotation:90 - TWENTY_FIVE(157.79, 172.32, 44.25),//Hub, Blue z rotation:180 - TWENTY_SIX(157.79, 158.32, 44.25),//Hub, Blue z rotation:180 - TWENTY_SEVEN(167.58, 134.56, 44.25),//Hub, Blue z rotation:270 - TWENTY_EIGHT(180.08, 24.85, 35.00),//Trench, Blue z rotation:180 - TWENTY_NINE(0.54, 25.62, 21.75),//Outpost, Blue z rotation:0 - THIRTY(0.54, 42.62, 21.75),//Outpost, Blue z rotation:0 - THIRTY_ONE(0.55, 146.86, 21.75),//Tower, Blue z rotation:0 - THIRTY_TWO(0.55, 163.86, 21.75);//Tower, Blue z rotation:0 +import edu.wpi.first.math.geometry.*; + +import java.lang.Math.*; + +import edu.wpi.first.math.util.Units; + +import static edu.wpi.first.units.Units.Radians; +import static java.lang.Math.PI; +import static java.lang.Math.abs; + +public enum Apriltag { + //Andymark field: + ONE(467.08,291.79, 35.00, PI), //Trench, Red z rotation:180 + TWO(468.56,182.08, 44.25, PI/2), //Hub, Red z rotation:90 + THREE( 444.80,172.32, 44.25, PI), //Hub, Red z rotation:180 + FOUR(444.80,158.32, 44.25, PI), //Hub, Red z rotation:180 + FIVE(468.56,134.56, 44.25, 3*PI/2), //Hub, Red z rotation:270 + SIX(467.08,24.85, 35.00,PI), //Trench, Red z rotation:180 + SEVEN(470.03,24.85, 35.00, 0), //Trench, Red z rotation:0 + EIGHT(482.56,134.56, 44.25, 3*PI/2), //Hub, Red z rotation:270 + NINE(492.33,144.32, 44.25, 0), //Hub, Red z rotation:0 + TEN(492.33,158.32, 44.25, 0), //Hub, Red z rotation:0 + ELEVEN(482.56,182.08, 44.25, PI/2), //Hub, Red z rotation:90 + TWELVE(470.03,291.79, 35.00, 0), //Trench, Red z rotation:0 + THIRTEEN(649.58,291.02, 21.75, PI), //Outpost, Red z rotation:180 + FOURTEEN(649.58,274.02, 21.75, PI), //Outpost, Red z rotation:180 + FIFTEEN(649.57,169.78, 21.75, PI), //Tower, Red z rotation:180 + SIXTEEN(649.57,152.78, 21.75, PI), //Tower, Red z rotation:180 + SEVENTEEN(183.03,24.85, 35.00, 0), //Trench, Blue z rotation:0 + EIGHTEEN(181.56,134.56, 44.25, 3*PI/2), //Hub, Blue z rotation:270 + NINETEEN(205.32,144.32, 44.25, 0), //Hub, Blue z rotation:0 + TWENTY(205.32,158.32, 44.52, 0), //Hub, Blue z rotation:0 + TWENTY_ONE(181.56,182.08, 44.25, PI/2), //Hub, Blue z rotation:90 + TWENTY_TWO(183.03, 291.79, 35.00, 0), //Trench, Blue z rotation:0 + TWENTY_THREE(180.08, 291.79, 35.00, PI),//Trench, Blue z rotation:180 + TWENTY_FOUR(167.56, 182.08, 44.25, PI/2),//Hub, Blue z rotation:90 + TWENTY_FIVE(157.79, 172.32, 44.25, PI),//Hub, Blue z rotation:180 + TWENTY_SIX(157.79, 158.32, 44.25, PI),//Hub, Blue z rotation:180 + TWENTY_SEVEN(167.58, 134.56, 44.25, 3*PI/2),//Hub, Blue z rotation:270 + TWENTY_EIGHT(180.08, 24.85, 35.00, PI),//Trench, Blue z rotation:180 + TWENTY_NINE(0.54, 25.62, 21.75, 0),//Outpost, Blue z rotation:0 + THIRTY(0.54, 42.62, 21.75, 0),//Outpost, Blue z rotation:0 + THIRTY_ONE(0.55, 146.86, 21.75, 0),//Tower, Blue z rotation:0 + THIRTY_TWO(0.55, 163.86, 21.75, 0);//Tower, Blue z rotation:0 /* Welded Field: @@ -73,15 +82,21 @@ public enum Apriltag { THIRTY_ONE(0.32, 147.47, 21.75),//Tower, Blue z rotation:0 THIRTY_TWO(0.32, 164.47, 21.75);//Tower, Blue z rotation:0 */ + public record TagPose(Apriltag tag, Pose3d pose) {} + private final double xMeters; + private final double yMeters; + private final double zMeters; + private final Pose3d pose; + private final Translation3d translation; + private final double rotation; - private final double x; - private final double y; - private final double z; - - Apriltag(double x, double y, double z) { - this.x = x; - this.y = y; - this.z = z; + Apriltag(double xInches, double yInches, double zInches, double rotation) { + this.xMeters = Units.inchesToMeters(xInches); + this.yMeters = Units.inchesToMeters(yInches); + this.zMeters = Units.inchesToMeters(zInches); + this.translation = new Translation3d(xMeters,yMeters,zMeters); + this.rotation = rotation; + this.pose = new Pose3d(translation,new Rotation3d(0,0,rotation)); } public static Apriltag of(int number) { @@ -92,22 +107,47 @@ public static Apriltag of(int number) { } public double getX() { - return x; + return xMeters; } public double getY() { - return y; + return yMeters; } public double getZ() { - return z; + return zMeters; + } + + public double getRotation() { + return rotation; } public Translation3d getTranslation() { - return new Translation3d(x, y, z); + return translation; + } + + public Pose3d getPose() { + return pose; } public int number() { - return ordinal(); + return ordinal()+1; + } + public TagPose getTagInfo() { + return new TagPose(this, pose); + } + public boolean canSee(Pose3d cameraPose, double HorizontalFOV, double VerticalFOV) { + Pose3d adjPose = getPose().relativeTo(cameraPose); + double horizontalAngle = Math.atan2(adjPose.getY(), adjPose.getX()); + double verticalAngle = Math.asin(adjPose.getZ()/adjPose.getTranslation().getNorm()); + double tagAngle = adjPose.getRotation().getZ()+PI; + double diffAngle = abs(horizontalAngle-tagAngle); + if (diffAngle>=2*PI) { + diffAngle-=2*PI; + } + if (diffAngle >= PI/2 && diffAngle <=3*PI/2) { + return false; + } + return abs(horizontalAngle) < HorizontalFOV/2 && abs(verticalAngle) < VerticalFOV/2;// TODO: Maybe change Later By Implementing Math Calculations (Linear Algebra) } -} +} \ No newline at end of file From 1781c98b2ef12ff262166bc482d28a528a8dad7c Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 21 Feb 2026 17:42:42 -0500 Subject: [PATCH 02/13] initial commit --- src/main/java/frc/robot/constants/GameConstants.java | 2 +- src/main/java/frc/robot/subsystems/ApriltagSubsystem.java | 1 + .../subsystems/swervedrive/vision/estimation/PoseEstimator.java | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index bd7205ba..89171ef9 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -158,5 +158,5 @@ public enum Mode { public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later - public static final double MAX_VISION_DISTANCE_SIMULATION = 60; + public static final double MAX_VISION_DISTANCE_SIMULATION = 6; } diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 9470e707..5b47c789 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -62,6 +62,7 @@ public void periodic() { estimator.updateVision(); estimator.updatePosition(drivebase.getOdom()); io.periodic(); + simReadings(); } public void simReadings() { for (Apriltag tag : Apriltag.values()) { 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 5426c80a..76e3c265 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 @@ -145,7 +145,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { double serverTime = apriltagSystem.getIO().getInputs().serverTime[index]; //double timestamp = 0; // latency is not right we are assuming zero double timestamp = apriltagSystem.getIO().getInputs().timestamp[index]; - Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); + Pose2d visionPose = new Pose2d(pos[0], pos[1], Rotation2d.fromDegrees(pos[2])); double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000); } From 16ab8032edca8fdf2e1da133aa66765e05348284 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 21 Feb 2026 18:14:17 -0500 Subject: [PATCH 03/13] initial commit --- src/main/java/frc/robot/Robot.java | 2 + .../robot/subsystems/ApriltagSubsystem.java | 2 +- .../swervedrive/SwerveSubsystem.java | 42 +++++++++++++++- .../estimation/FilterablePoseManager.java | 49 +++++++++++++------ .../vision/estimation/PoseEstimator.java | 4 +- .../vision/estimation/PoseManager.java | 2 +- .../vision/truster/VisionMeasurement.java | 3 +- 7 files changed, 82 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce872885..e0f28e6e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -122,6 +122,8 @@ public void robotPeriodic() { SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); if(!Constants.TESTBED){ Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); + Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom()); + Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose()); // Puts data on the elastic dashboard SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); SmartDashboard.putBoolean("Hub Active?", hubActive()); diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 5b47c789..2b6999c0 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -72,7 +72,7 @@ public void simReadings() { double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { - VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0); + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(),distance, 0); Vector stdDevs = estimator.getVisionTruster().calculateTrust(measurement); double readingX = robotPoseSupplier.get().getX() + random.nextGaussian() * stdDevs.get(0); double readingY = robotPoseSupplier.get().getY() + random.nextGaussian() * stdDevs.get(1); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 980b7b1b..0ee22462 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -13,16 +13,20 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.constants.Constants; import frc.robot.utils.logging.io.gyro.ThreadedGyro; +import org.littletonrobotics.junction.Logger; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -37,6 +41,7 @@ import java.io.File; import java.util.Arrays; +import java.util.concurrent.ConcurrentLinkedDeque; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -48,6 +53,11 @@ public class SwerveSubsystem extends SubsystemBase { */ private final SwerveDrive swerveDrive; private Vector variance = VecBuilder.fill(0.1,0.1,0.1); + private final Field2d rawOdomField = new Field2d(); + private SwerveDriveOdometry rawOdometry; + private final ConcurrentLinkedDeque poseError = new ConcurrentLinkedDeque<>(); + private record PoseErrorRecord(double timestamp, double error) { + } /** * Initialize {@link SwerveDrive} with the directory provided. * The SwerveIMU (which can be null) is the instance of the SwerveIMU to use. If non-null, @@ -86,7 +96,12 @@ public SwerveSubsystem(File directory, SwerveIMU swerveIMU) { swerveDrive.setModuleEncoderAutoSynchronize(Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE, Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE_DEADBAND); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible - + rawOdometry = new SwerveDriveOdometry( + swerveDrive.kinematics, + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions(), + startingPose + ); } /** @@ -108,6 +123,17 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig public void periodic() { //add vision pose here //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d())); + rawOdometry.update( + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions() + ); + + rawOdomField.setRobotPose(rawOdometry.getPoseMeters()); + double currentTime = Logger.getTimestamp()/1000000.0; + double oneSecondAgo = currentTime - 1.0; + poseError.removeIf(record -> record.timestamp < oneSecondAgo); + poseError.add(new PoseErrorRecord(currentTime, getError())); + Logger.recordOutput("AveragePoseError", getAverageError()); } @Override @@ -285,8 +311,14 @@ public SwerveDriveKinematics getKinematics() { * * @param initialHolonomicPose The pose to set the odometry to */ + // might be broken public void resetOdometry(Pose2d initialHolonomicPose) { swerveDrive.resetOdometry(initialHolonomicPose); + SwerveModulePosition[] modules = new SwerveModulePosition[4]; + for (int i=0; i<4; i++) { + modules[i] = new SwerveModulePosition(); + } + rawOdometry.resetPosition(initialHolonomicPose.getRotation(), modules, initialHolonomicPose); } /** @@ -297,12 +329,18 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + public double getError() { + return getPose().getTranslation().getDistance((getSimulationPose().getTranslation())); + } + public double getAverageError(){ + return poseError.stream().mapToDouble(record -> record.error).average().orElse(0); + } public Pose2d getSimulationPose() { return swerveDrive.getSimulationDriveTrainPose().orElse(new Pose2d()); } // Todo: fix to only get odomtry public Pose2d getOdom() { - return swerveDrive.getPose(); + return rawOdometry.getPoseMeters(); } /** 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 0d04e405..255b51c0 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 @@ -2,6 +2,7 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; @@ -12,10 +13,10 @@ 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 java.util.*; +import java.util.concurrent.ConcurrentLinkedDeque; + +import frc.robot.utils.Apriltag; import org.littletonrobotics.junction.Logger; /** @@ -24,6 +25,9 @@ */ public class FilterablePoseManager extends PoseManager { private final VisionFilter filter; + private record MeasurementRecord(Apriltag tag, double timestamp, FilterResult result) { } + public record VisionLog(VisionMeasurement measurement, FilterResult result) {} + private final ConcurrentLinkedDeque lastSecondMeasurements = new ConcurrentLinkedDeque<>(); public FilterablePoseManager( PoseDeviation PoseDeviation, @@ -53,29 +57,44 @@ public FilterablePoseManager( @Override public void processQueue() { + double currentTime = Logger.getTimestamp()/1000000.0; + double oneSecondAgo = currentTime - 1.0; + lastSecondMeasurements.removeIf(record -> record.timestamp < oneSecondAgo); + List log = new ArrayList<>(); + List validMeasurementsPose = new ArrayList<>(); + List invalidMeasurementsPose = new ArrayList<>();; + List acceptedTagsPose = new ArrayList<>(); + LinkedHashMap filteredData = - filter.filter(visionMeasurementQueue); + 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(); + for (Map.Entry filterEntry : filteredData.entrySet()) { + VisionMeasurement v = filterEntry.getKey(); + Apriltag tag = v.tag().tag(); + FilterResult r = filterEntry.getValue(); + log.add(new VisionLog(v, r)); + lastSecondMeasurements.add(new MeasurementRecord(tag, v.timeOfMeasurement(),r)); switch (r) { case ACCEPTED -> { setVisionSTD(visionTruster.calculateTrust(v)); - validMeasurements.add(v.measurement()); + validMeasurementsPose.add(v.measurement()); addVisionMeasurement(v); + acceptedTagsPose.add(tag.getPose()); + } case NOT_PROCESSED -> visionMeasurementQueue.add(v); case REJECTED -> { - invalidMeasurements.add(v.measurement()); + invalidMeasurementsPose.add(v.measurement()); } } } - Logger.recordOutput("Apriltag/acceptedMeasurements", validMeasurements.toArray(Pose2d[]::new)); - Logger.recordOutput( - "Apriltag/rejectedMeasurements", invalidMeasurements.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/numberAcceptedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.ACCEPTED).count()); + Logger.recordOutput("Apriltag/numberNotProcessedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.NOT_PROCESSED).count()); + Logger.recordOutput("Apriltag/numberRejectedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.REJECTED).count()); + Logger.recordOutput("Apriltag/acceptedTagPose", acceptedTagsPose.toArray(Pose3d[]::new)); + Logger.recordOutput("Apriltag/Log", log.toArray(VisionLog[]::new)); } public VisionTruster getVisionTruster() { 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 76e3c265..55f83804 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 @@ -147,7 +147,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { double timestamp = apriltagSystem.getIO().getInputs().timestamp[index]; Pose2d visionPose = new Pose2d(pos[0], pos[1], Rotation2d.fromDegrees(pos[2])); double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; - return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000); + return new VisionMeasurement(visionPose, Apriltag.of(apriltagSystem.getIO().getInputs().apriltagNumber[index]).getTagInfo(),distanceFromTag, timestamp/1000); } /** @@ -201,7 +201,7 @@ public FilterablePoseManager getPoseManager() { public void addMockVisionMeasurement() { poseManager.registerVisionMeasurement( - new VisionMeasurement(getEstimatedPose(), 0, Logger.getTimestamp() / 1e6)); + new VisionMeasurement(getEstimatedPose(), Apriltag.of(1).getTagInfo(), 0, Logger.getTimestamp() / 1e6)); } public VisionTruster getVisionTruster() { return poseManager.getVisionTruster(); 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 3f979f9f..aae43560 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 @@ -24,7 +24,7 @@ public class PoseManager { private final TimeInterpolatableBuffer estimatedPoseBuffer; //private final SwerveDrivePoseEstimator poseEstimator; protected final Queue visionMeasurementQueue = new LinkedList<>(); - private final SwerveSubsystem drivebase; + protected final SwerveSubsystem drivebase; protected final VisionTruster visionTruster; public PoseManager( 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 index edd9af38..82b54ead 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.utils.Apriltag; /** * @param measurement estimated robot position (meters) calculated from apriltag tag what tag produced the @@ -9,4 +10,4 @@ * @param timeOfMeasurement time when the pose was measured (seconds) */ public record VisionMeasurement( - Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} + Pose2d measurement, Apriltag.TagPose tag, double distanceFromTag, double timeOfMeasurement) {} From c5cce851be7df52bdfa4a220a92c4ec95d206a2d Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sat, 21 Feb 2026 18:57:26 -0500 Subject: [PATCH 04/13] initial commit --- .../robot/apriltags/TCPApriltagServer.java | 11 ++--- .../frc/robot/constants/GameConstants.java | 8 ++++ .../robot/subsystems/ApriltagSubsystem.java | 2 +- .../swervedrive/SwerveSubsystem.java | 6 ++- .../estimation/FilterablePoseManager.java | 42 ++++++++++--------- .../vision/estimation/PoseEstimator.java | 20 ++++----- .../vision/estimation/PoseManager.java | 22 +++++----- .../vision/truster/BasicVisionFilter.java | 25 +++++++---- .../vision/truster/ConstantVisionTruster.java | 3 +- .../vision/truster/LinearVisionTruster.java | 3 +- .../vision/truster/SquareVisionTruster.java | 9 ++-- .../vision/truster/VisionFilter.java | 4 +- .../vision/truster/VisionTruster.java | 3 +- 13 files changed, 93 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java index 70314611..86d152c5 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import java.io.DataInputStream; import java.io.IOException; +import frc.robot.constants.Constants; public class TCPApriltagServer extends TCPServer { @@ -20,7 +21,7 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc double posY = -1; double poseYaw = -1; double distanceToTag = -1; - double timestamp = -1; + double latency = -1; int apriltagNumber = -1; double now = 0; while (posX == -1 @@ -28,16 +29,16 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc && poseYaw == -1 && distanceToTag == -1 && apriltagNumber == -1 - && timestamp == -1) { + && latency == -1) { posX = stream.readDouble(); posY = stream.readDouble(); poseYaw = stream.readDouble(); distanceToTag = stream.readDouble(); - timestamp = stream.readDouble(); + latency = stream.readDouble(); apriltagNumber = stream.readInt(); - now = Timer.getFPGATimestamp() * 1000; + now = Timer.getFPGATimestamp() * 1000-Constants.AVERAGE_PIR_LATENCY-latency; } - return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, timestamp, now); + return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, latency, now); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 89171ef9..78ee8fe0 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,8 +1,11 @@ package frc.robot.constants; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -149,6 +152,7 @@ public enum Mode { public static final int ENDGAME_START = 30; public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed) + public static final double VISION_STD_THRESHOLD = 0.25; public static final boolean ENABLE_VISION = true; public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back) @@ -159,4 +163,8 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 6; + public static final double AVERAGE_PIR_LATENCY = 20; //ms + public static final double VISION_SMOOTHER = 1.0; + public static final Vector INITIAL_VISION_STD_DEVS = VecBuilder.fill(0,0,0); // TODO: Change later + public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later } diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 2b6999c0..a4a8ea7c 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -73,7 +73,7 @@ public void simReadings() { double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(),distance, 0); - Vector stdDevs = estimator.getVisionTruster().calculateTrust(measurement); + Vector stdDevs = estimator.getVisionTruster().calculateTrust(measurement,cameraPos); double readingX = robotPoseSupplier.get().getX() + random.nextGaussian() * stdDevs.get(0); double readingY = robotPoseSupplier.get().getY() + random.nextGaussian() * stdDevs.get(1); double readingYaw = robotPoseSupplier.get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 0ee22462..cc70e18e 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -9,6 +9,7 @@ import static edu.wpi.first.units.Units.Meter; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -329,6 +330,9 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + public Pose3d getCameraPose() { + return new Pose3d(getSimulationPose()).transformBy(Constants.ROBOT_TO_CAMERA); + } public double getError() { return getPose().getTranslation().getDistance((getSimulationPose().getTranslation())); } @@ -511,7 +515,7 @@ public SwerveDrive getSwerveDrive() { return swerveDrive; } public void setVariance(Vector variance){ - this.variance = variance; + this.variance = variance.times(Constants.VISION_SMOOTHER); } public void addVisionMeasurement(Pose2d pose, double visionTimestamp){ swerveDrive.addVisionMeasurement(pose, visionTimestamp, variance); 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 255b51c0..01d52891 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 @@ -64,27 +64,29 @@ public void processQueue() { List validMeasurementsPose = new ArrayList<>(); List invalidMeasurementsPose = new ArrayList<>();; List acceptedTagsPose = new ArrayList<>(); + for (Map.Entry> queueEntry : visionMeasurementQueueMap.entrySet()) { + Queue visionMeasurementQueue = queueEntry.getValue(); + LinkedHashMap filteredData = + filter.filter(visionMeasurementQueue,drivebase.getCameraPose()); + visionMeasurementQueue.clear(); + for (Map.Entry filterEntry : filteredData.entrySet()) { + VisionMeasurement v = filterEntry.getKey(); + Apriltag tag = v.tag().tag(); + FilterResult r = filterEntry.getValue(); + log.add(new VisionLog(v, r)); + lastSecondMeasurements.add(new MeasurementRecord(tag, v.timeOfMeasurement(), r)); + switch (r) { + case ACCEPTED -> { + setVisionSTD(visionTruster.calculateTrust(v,drivebase.getCameraPose())); + validMeasurementsPose.add(v.measurement()); + addVisionMeasurement(v); + acceptedTagsPose.add(tag.getPose()); - LinkedHashMap filteredData = - filter.filter(visionMeasurementQueue); - visionMeasurementQueue.clear(); - for (Map.Entry filterEntry : filteredData.entrySet()) { - VisionMeasurement v = filterEntry.getKey(); - Apriltag tag = v.tag().tag(); - FilterResult r = filterEntry.getValue(); - log.add(new VisionLog(v, r)); - lastSecondMeasurements.add(new MeasurementRecord(tag, v.timeOfMeasurement(),r)); - switch (r) { - case ACCEPTED -> { - setVisionSTD(visionTruster.calculateTrust(v)); - validMeasurementsPose.add(v.measurement()); - addVisionMeasurement(v); - acceptedTagsPose.add(tag.getPose()); - - } - case NOT_PROCESSED -> visionMeasurementQueue.add(v); - case REJECTED -> { - invalidMeasurementsPose.add(v.measurement()); + } + case NOT_PROCESSED -> visionMeasurementQueue.add(v); + case REJECTED -> { + invalidMeasurementsPose.add(v.measurement()); + } } } } 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 55f83804..73a3f651 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 @@ -16,10 +16,7 @@ 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.ConstantVisionTruster; -import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; -import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; +import frc.robot.subsystems.swervedrive.vision.truster.*; import frc.robot.utils.Apriltag; import frc.robot.utils.math.ArrayUtils; @@ -79,20 +76,19 @@ public PoseEstimator( initGyroValueDeg);*/ TimeInterpolatableBuffer m1Buffer = TimeInterpolatableBuffer.createBuffer(Constants.POSE_BUFFER_STORAGE_TIME); + VisionTruster truster = new SquareVisionTruster(Constants.INITIAL_VISION_STD_DEVS, Constants.VISION_STD_DEV_CONST); this.poseManager = new FilterablePoseManager( - visionMeasurementStdDevs2, + Constants.INITIAL_VISION_STD_DEVS, kinematics, drivebase, m1Buffer, - new BasicVisionFilter(m1Buffer) { + new BasicVisionFilter(m1Buffer,truster) { @Override public Pose2d getVisionPose(VisionMeasurement measurement) { return measurement.measurement(); } - }, - new ConstantVisionTruster(visionMeasurementStdDevs2)); - SmartDashboard.putData(field); + }, truster); } @@ -129,7 +125,7 @@ private void updateVision(int... invalidApriltagNumbers) { && !ArrayUtils.contains( invalidApriltagNumbers, apriltagSystem.getIO().getInputs().apriltagNumber[i])) { VisionMeasurement measurement = getVisionMeasurement(pos, i); - poseManager.registerVisionMeasurement(measurement); + poseManager.registerVisionMeasurement(measurement,measurement.tag().tag().number()); } else { invalidCounter++; Logger.recordOutput("Apriltag/ValidationFailureCount", invalidCounter); @@ -155,7 +151,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { * are sent to the {@link PoseManager} for further processing */ public void updateVision() { - updateVision(15, 4, 14, 5, 16, 3); + updateVision(0); } public void updateVision(Apriltag focusedTag) { @@ -201,7 +197,7 @@ public FilterablePoseManager getPoseManager() { public void addMockVisionMeasurement() { poseManager.registerVisionMeasurement( - new VisionMeasurement(getEstimatedPose(), Apriltag.of(1).getTagInfo(), 0, Logger.getTimestamp() / 1e6)); + new VisionMeasurement(getEstimatedPose(), Apriltag.of(1).getTagInfo(), 0, Logger.getTimestamp() / 1e6),1); } public VisionTruster getVisionTruster() { return poseManager.getVisionTruster(); 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 aae43560..4e1e4d81 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 @@ -12,6 +12,7 @@ import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import org.littletonrobotics.junction.Logger; +import java.util.LinkedHashMap; import java.util.LinkedList; import java.util.Optional; import java.util.Queue; @@ -23,7 +24,7 @@ public class PoseManager { private final TimeInterpolatableBuffer estimatedPoseBuffer; //private final SwerveDrivePoseEstimator poseEstimator; - protected final Queue visionMeasurementQueue = new LinkedList<>(); + protected final LinkedHashMap> visionMeasurementQueueMap = new LinkedHashMap<>(); protected final SwerveSubsystem drivebase; protected final VisionTruster visionTruster; @@ -62,23 +63,22 @@ public void addOdomMeasurement(Pose2d pose, long timestamp) { estimatedPoseBuffer.addSample(timestamp, pose); } - public void registerVisionMeasurement(VisionMeasurement measurement) { + public void registerVisionMeasurement(VisionMeasurement measurement, int tagId) { if (measurement == null) { return; } - while (visionMeasurementQueue.size() >= 3) { - visionMeasurementQueue.poll(); - } - visionMeasurementQueue.add(measurement); + visionMeasurementQueueMap.computeIfAbsent(tagId, k -> new LinkedList<>()).add(measurement); } // override for filtering public void processQueue() { - VisionMeasurement m = visionMeasurementQueue.poll(); - while (m != null) { - setVisionSTD(visionTruster.calculateTrust(m)); - addVisionMeasurement(m); - m = visionMeasurementQueue.poll(); + for (Queue visionMeasurementQueue : visionMeasurementQueueMap.values()) { + VisionMeasurement m = visionMeasurementQueue.poll(); + while (m != null) { + setVisionSTD(visionTruster.calculateTrust(m,drivebase.getCameraPose())); + addVisionMeasurement(m); + m = visionMeasurementQueue.poll(); + } } } 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 index ae99851c..452b6745 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.swervedrive.vision.truster; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.numbers.N3; import frc.robot.constants.Constants; import java.util.LinkedHashMap; import java.util.Optional; @@ -17,14 +20,16 @@ public abstract class BasicVisionFilter implements VisionFilter, VisionTransformer { private final TimeInterpolatableBuffer poseBuffer; + private final VisionTruster truster; - public BasicVisionFilter(TimeInterpolatableBuffer poseBuffer) { + public BasicVisionFilter(TimeInterpolatableBuffer poseBuffer, VisionTruster truster) { this.poseBuffer = poseBuffer; + this.truster = truster; } @Override public LinkedHashMap filter( - Queue measurements) { + Queue measurements, Pose3d cameraPose) { LinkedHashMap resultMap = new LinkedHashMap<>(); VisionMeasurement m1 = measurements.poll(); VisionMeasurement m2 = measurements.peek(); @@ -54,7 +59,7 @@ public LinkedHashMap filter( Pose2d vision1Pose = getVisionPose(m1); Pose2d vision2Pose = getVisionPose(m2); boolean valid1 = - filterVision(vision1Pose, vision2Pose, m1.timeOfMeasurement(), m2.timeOfMeasurement()); + filterVision(m1, m2, cameraPose); resultMap.put(m1, valid1 ? FilterResult.ACCEPTED : FilterResult.REJECTED); m1 = measurements.poll(); m2 = measurements.peek(); @@ -63,19 +68,23 @@ public LinkedHashMap filter( return resultMap; } - private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double m2Time) { - Optional odomPoseAtVis1 = poseBuffer.getSample(m1Time); - Optional odomPoseAtVis2 = poseBuffer.getSample(m2Time); + private boolean filterVision(VisionMeasurement m1, VisionMeasurement m2, Pose3d cameraPose) { + Optional odomPoseAtVis1 = poseBuffer.getSample(m1.timeOfMeasurement()); + Optional odomPoseAtVis2 = poseBuffer.getSample(m2.timeOfMeasurement()); if (odomPoseAtVis1.isEmpty() || odomPoseAtVis2.isEmpty()) { return false; } - if (!inBounds(m1Pose) || !inBounds(m2Pose)) { + if (!inBounds(m1.measurement()) || !inBounds(m2.measurement())) { return false; } double odomDiff1To2 = odomPoseAtVis1.get().getTranslation().getDistance(odomPoseAtVis2.get().getTranslation()); - double visionDiff1To2 = m1Pose.getTranslation().getDistance(m2Pose.getTranslation()); + double visionDiff1To2 = m1.measurement().getTranslation().getDistance(m2.measurement().getTranslation()); double diff = Math.abs(odomDiff1To2 - visionDiff1To2); + Vector std = truster.calculateTrust(m1, cameraPose); + if (std.get(0) > Constants.VISION_STD_THRESHOLD) { + return false; + } return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD; } 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..fe133bbe 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 @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; @@ -11,7 +12,7 @@ public ConstantVisionTruster(Vector initialSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { return initialSTD; } } 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 index e7f75ea6..78e6f094 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public class LinearVisionTruster extends DistanceVisionTruster { @@ -14,7 +15,7 @@ public LinearVisionTruster(Vector initialSTD, double slopeSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { 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/SquareVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java index 7a925fc8..5ffd8a0f 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 @@ -2,6 +2,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public class SquareVisionTruster extends DistanceVisionTruster { @@ -15,8 +16,10 @@ public SquareVisionTruster(Vector initialSTD, double 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)); + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { + Pose3d adjPose = measurement.tag().tag().getPose().relativeTo(cameraPose); + double cosIncidenceAngle = (-adjPose.getX()*Math.cos(adjPose.getRotation().getZ())-adjPose.getY()*Math.sin(adjPose.getRotation().getZ()))/(adjPose.getTranslation().getNorm()); + double std = Math.pow(measurement.distanceFromTag(), 2) * constant / cosIncidenceAngle; + return initialSTD.plus(VecBuilder.fill(std, std, std*10000)); } } 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 index e1b94831..6f156bad 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java @@ -1,8 +1,10 @@ package frc.robot.subsystems.swervedrive.vision.truster; +import edu.wpi.first.math.geometry.Pose3d; + import java.util.LinkedHashMap; import java.util.Queue; public interface VisionFilter { - LinkedHashMap filter(Queue measurements); + LinkedHashMap filter(Queue measurements, Pose3d cameraPose); } 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 index 7ce7c3d4..36279178 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public interface VisionTruster { - Vector calculateTrust(VisionMeasurement measurement); + Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose); } From f244daec8399067819f8da7e4ea2c57eeded6fde Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 22 Feb 2026 16:43:07 -0500 Subject: [PATCH 05/13] initial commit --- src/main/java/frc/robot/RobotContainer.java | 11 ++-- .../frc/robot/apriltags/SimApriltagIO.java | 51 ++++++++++++++++++- .../robot/subsystems/ApriltagSubsystem.java | 42 ++++----------- .../swervedrive/SwerveSubsystem.java | 5 +- .../vision/estimation/PoseEstimator.java | 6 +-- src/main/java/frc/robot/utils/Apriltag.java | 14 ----- .../robot/utils/logging/io/BaseIoImpl.java | 2 +- .../frc/robot/utils/math/ObjectUtils.java | 24 +++++++++ 8 files changed, 100 insertions(+), 55 deletions(-) create mode 100644 src/main/java/frc/robot/utils/math/ObjectUtils.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 326ca6c7..5117f5f3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,6 +61,8 @@ //import frc.robot.subsystems.RollerSubsystem; //import frc.robot.subsystems.TiltSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; +import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.logging.io.gyro.RealGyroIo; import frc.robot.utils.logging.io.gyro.ThreadedGyro; import frc.robot.utils.logging.io.gyro.ThreadedGyroSwerveIMU; @@ -79,6 +81,8 @@ import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; +import static frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator.visionMeasurementStdDevs2; + /** * This class is where the bulk of the robot should be declared. Since * Command-based is a @@ -116,6 +120,7 @@ public class RobotContainer { private AutoFactory autoFactory; private static AutoRoutine straightRoutine; private static AutoTrajectory straightTrajectory; + private final VisionTruster truster = new ConstantVisionTruster(visionMeasurementStdDevs2); // Replace with CommandPS4Controller or CommandJoystick if needed // new CommandXboxController(OperatorConstants.kDriverControllerPort);private @@ -150,7 +155,7 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null; - apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase) : null; + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; } @@ -169,7 +174,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 = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null; + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; } @@ -191,7 +196,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; - apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null; + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null; } default -> { diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 9c30fa5f..4f82ffc9 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -1,14 +1,63 @@ package frc.robot.apriltags; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N3; import frc.robot.constants.Constants; +import frc.robot.subsystems.ApriltagSubsystem; +import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; +import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.Apriltag; import frc.robot.utils.logging.io.BaseIoImpl; + +import java.util.Optional; import java.util.Queue; +import java.util.Random; +import java.util.function.Supplier; + +import frc.robot.utils.math.ObjectUtils; import org.littletonrobotics.junction.Logger; public class SimApriltagIO extends TCPApriltagIo { - public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server) { + private final Random random = new Random(); + private final VisionTruster truster; + private final Supplier> robotPoseSupplier; + public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server, VisionTruster truster, Supplier> robotPoseSupplier) { super(name, inputs, server); + this.truster = truster; + this.robotPoseSupplier = robotPoseSupplier; + } + public void simReadings() { + if (robotPoseSupplier.get().isPresent()) { + for (Apriltag tag : Apriltag.values()) { + Pose3d cameraPos = new Pose3d(robotPoseSupplier.get().get()).transformBy(Constants.ROBOT_TO_CAMERA); + if (ObjectUtils.canSee(tag.getPose(), cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { + Pose3d adjPose = tag.getPose().relativeTo(cameraPos); + double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); + double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); + if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0); + Vector stdDevs = truster.calculateTrust(measurement); + double readingX = robotPoseSupplier.get().get().getX() + random.nextGaussian() * stdDevs.get(0); + double readingY = robotPoseSupplier.get().get().getY() + random.nextGaussian() * stdDevs.get(1); + double readingYaw = robotPoseSupplier.get().get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); + Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); + distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); + if (BasicVisionFilter.inBounds(readingPos)) { + addReading(new ApriltagReading(readingX, readingY, readingYaw, + distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0)); + } + } + } + } + } + } + @Override + public void periodic() { + updateInputs(inputs); + Logger.processInputs(prefix, inputs); + simReadings(); } } \ 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 5b47c789..99c2bb33 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -13,8 +13,10 @@ import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.Apriltag; import frc.robot.utils.logging.io.BaseIoImpl; +import frc.robot.utils.math.ObjectUtils; import org.littletonrobotics.junction.Logger; import java.util.Random; @@ -26,15 +28,12 @@ public class ApriltagSubsystem extends SubsystemBase { private final ApriltagIO io; private final PoseEstimator estimator; private final SwerveSubsystem drivebase; - private final Random random = new Random(); - private final Supplier robotPoseSupplier; - public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { + public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase, VisionTruster truster) { this.drivebase = drivebase; this.io = io; - estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); - robotPoseSupplier = drivebase::getSimulationPose; + estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this, truster); } public static ApriltagIO createRealIo() { @@ -46,8 +45,8 @@ public static ApriltagIO createMockIo() { return new MockApriltagIo(LOGGING_NAME, new ApriltagInputs()); } - public static ApriltagIO createSimIo() { - return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0)); // port doesnt matter at all + public static ApriltagIO createSimIo(VisionTruster truster, SwerveSubsystem drivebase) { + return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0), truster, drivebase::getSimulationPose); // 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) { @@ -56,35 +55,16 @@ public void addSimReading(ApriltagReading reading) { public ApriltagIO getIO(){ return io; } - + public VisionTruster getVisionTruster() { + return estimator.getVisionTruster(); + } @Override public void periodic() { estimator.updateVision(); estimator.updatePosition(drivebase.getOdom()); io.periodic(); - simReadings(); } - public void simReadings() { - for (Apriltag tag : Apriltag.values()) { - Pose3d cameraPos = new Pose3d(robotPoseSupplier.get()).transformBy(Constants.ROBOT_TO_CAMERA); - if (tag.canSee(cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { - Pose3d adjPose = tag.getPose().relativeTo(cameraPos); - double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); - double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); - if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { - VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0); - Vector stdDevs = estimator.getVisionTruster().calculateTrust(measurement); - double readingX = robotPoseSupplier.get().getX() + random.nextGaussian() * stdDevs.get(0); - double readingY = robotPoseSupplier.get().getY() + random.nextGaussian() * stdDevs.get(1); - double readingYaw = robotPoseSupplier.get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); - Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); - distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); - if (BasicVisionFilter.inBounds(readingPos)) { - io.addReading(new ApriltagReading(readingX, readingY, readingYaw, - distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0)); - } - } - } - } + public Vector calculateTrust(VisionMeasurement measurement) { + return estimator.getVisionTruster().calculateTrust(measurement); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 980b7b1b..a4738fb1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -37,6 +37,7 @@ import java.io.File; import java.util.Arrays; +import java.util.Optional; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -297,8 +298,8 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } - public Pose2d getSimulationPose() { - return swerveDrive.getSimulationDriveTrainPose().orElse(new Pose2d()); + public Optional getSimulationPose() { + return swerveDrive.getSimulationDriveTrainPose(); } // Todo: fix to only get odomtry public Pose2d getOdom() { 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 76e3c265..581f7da0 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 @@ -62,7 +62,8 @@ public PoseEstimator( SwerveDriveKinematics kinematics, SwerveSubsystem drivebase, double initGyroValueDeg, - ApriltagSubsystem apriltagSystem) { + ApriltagSubsystem apriltagSystem, + VisionTruster truster) { /*this.frontLeft = frontLeftMotor; this.frontRight = frontRightMotor; this.backLeft = backLeftMotor; @@ -90,8 +91,7 @@ public PoseEstimator( public Pose2d getVisionPose(VisionMeasurement measurement) { return measurement.measurement(); } - }, - new ConstantVisionTruster(visionMeasurementStdDevs2)); + }, truster); SmartDashboard.putData(field); } diff --git a/src/main/java/frc/robot/utils/Apriltag.java b/src/main/java/frc/robot/utils/Apriltag.java index 66be774e..8b134452 100644 --- a/src/main/java/frc/robot/utils/Apriltag.java +++ b/src/main/java/frc/robot/utils/Apriltag.java @@ -136,18 +136,4 @@ public int number() { public TagPose getTagInfo() { return new TagPose(this, pose); } - public boolean canSee(Pose3d cameraPose, double HorizontalFOV, double VerticalFOV) { - Pose3d adjPose = getPose().relativeTo(cameraPose); - double horizontalAngle = Math.atan2(adjPose.getY(), adjPose.getX()); - double verticalAngle = Math.asin(adjPose.getZ()/adjPose.getTranslation().getNorm()); - double tagAngle = adjPose.getRotation().getZ()+PI; - double diffAngle = abs(horizontalAngle-tagAngle); - if (diffAngle>=2*PI) { - diffAngle-=2*PI; - } - if (diffAngle >= PI/2 && diffAngle <=3*PI/2) { - return false; - } - return abs(horizontalAngle) < HorizontalFOV/2 && abs(verticalAngle) < VerticalFOV/2;// TODO: Maybe change Later By Implementing Math Calculations (Linear Algebra) - } } \ No newline at end of file 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 0fe698cb..6113812b 100644 --- a/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java +++ b/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java @@ -9,7 +9,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; + protected final String prefix; protected final I inputs; public BaseIoImpl(String folder, I inputs) { diff --git a/src/main/java/frc/robot/utils/math/ObjectUtils.java b/src/main/java/frc/robot/utils/math/ObjectUtils.java new file mode 100644 index 00000000..773471e4 --- /dev/null +++ b/src/main/java/frc/robot/utils/math/ObjectUtils.java @@ -0,0 +1,24 @@ +package frc.robot.utils.math; + +import edu.wpi.first.math.geometry.Pose3d; + +import static java.lang.Math.PI; +import static java.lang.Math.abs; + +public class ObjectUtils { + // TODO: Implement obstruction + public static boolean canSee(Pose3d objectPose, Pose3d cameraPose, double HorizontalFOV, double VerticalFOV) { + Pose3d adjPose = objectPose.relativeTo(cameraPose); + double horizontalAngle = Math.atan2(adjPose.getY(), adjPose.getX()); + double verticalAngle = Math.asin(adjPose.getZ()/adjPose.getTranslation().getNorm()); + double tagAngle = adjPose.getRotation().getZ()+PI; + double diffAngle = abs(horizontalAngle-tagAngle); + if (diffAngle>=2*PI) { + diffAngle-=2*PI; + } + if (diffAngle >= PI/2 && diffAngle <=3*PI/2) { + return false; + } + return abs(horizontalAngle) < HorizontalFOV/2 && abs(verticalAngle) < VerticalFOV/2;// TODO: Maybe change Later By Implementing Math Calculations (Linear Algebra) + } +} From e27fbb2dfbc6f3240299cc9adf02c1ac74bfe33b Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 22 Feb 2026 19:21:35 -0500 Subject: [PATCH 06/13] improved math (needs further fixes) --- .../java/frc/robot/apriltags/SimApriltagIO.java | 8 +++++--- .../java/frc/robot/constants/GameConstants.java | 2 +- .../frc/robot/subsystems/ApriltagSubsystem.java | 3 --- .../subsystems/swervedrive/SwerveSubsystem.java | 6 +++++- .../vision/estimation/FilterablePoseManager.java | 16 ++++++++++------ .../vision/estimation/PoseEstimator.java | 4 ++-- .../vision/estimation/PoseManager.java | 5 ++++- .../vision/truster/SquareVisionTruster.java | 9 +++++++-- .../java/frc/robot/utils/math/ObjectUtils.java | 16 ++++++++++------ 9 files changed, 44 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 592cd6fe..57425787 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -35,11 +35,13 @@ public void simReadings() { Pose3d cameraPos = new Pose3d(robotPoseSupplier.get().get()).transformBy(Constants.ROBOT_TO_CAMERA); if (ObjectUtils.canSee(tag.getPose(), cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { Pose3d adjPose = tag.getPose().relativeTo(cameraPos); - double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); + Vector tagNormalVector = new Translation3d(new Translation2d(1,new Rotation2d(adjPose.getRotation().getZ()))).toVector(); + Vector tagToCameraVector = cameraPos.relativeTo(tag.getPose()).getTranslation().toVector(); + double distanceTimesCosIncidenceAngle = -tagNormalVector.dot(tagToCameraVector); double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); - if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + if (distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0); - Vector stdDevs = truster.calculateTrust(measurement); + Vector stdDevs = truster.calculateTrust(measurement, cameraPos); double readingX = robotPoseSupplier.get().get().getX() + random.nextGaussian() * stdDevs.get(0); double readingY = robotPoseSupplier.get().get().getY() + random.nextGaussian() * stdDevs.get(1); double readingYaw = robotPoseSupplier.get().get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 78ee8fe0..a5fa6acc 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -157,7 +157,7 @@ public enum Mode { public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back) // Vision - public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change + public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 1e56d534..e4cec7a7 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -64,7 +64,4 @@ public void periodic() { estimator.updatePosition(drivebase.getOdom()); io.periodic(); } - public Vector calculateTrust(VisionMeasurement measurement) { - return estimator.getVisionTruster().calculateTrust(measurement,cameraPos); - } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 9bc34dba..ec5ef8da 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -335,7 +335,11 @@ public Pose2d getPose() { return swerveDrive.getPose(); } public Pose3d getCameraPose() { - return new Pose3d(getSimulationPose()).transformBy(Constants.ROBOT_TO_CAMERA); + if (Constants.currentMode == GameConstants.Mode.SIM) { + return new Pose3d(getSimulationPose().get()).transformBy(Constants.ROBOT_TO_CAMERA); + } else { + return new Pose3d(getPose()).transformBy(Constants.ROBOT_TO_CAMERA); + } } public double getError() { return getPose().getTranslation().getDistance((getSimulationPose().get().getTranslation())); 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 01d52891..72e45b7d 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 @@ -28,7 +28,10 @@ public class FilterablePoseManager extends PoseManager { private record MeasurementRecord(Apriltag tag, double timestamp, FilterResult result) { } public record VisionLog(VisionMeasurement measurement, FilterResult result) {} private final ConcurrentLinkedDeque lastSecondMeasurements = new ConcurrentLinkedDeque<>(); - + private final List validMeasurementsPose = new ArrayList<>(); + private final List invalidMeasurementsPose = new ArrayList<>();; + private final List acceptedTagsPose = new ArrayList<>(); + private final List log = new ArrayList<>(); public FilterablePoseManager( PoseDeviation PoseDeviation, SwerveDriveKinematics kinematics, @@ -60,10 +63,6 @@ public void processQueue() { double currentTime = Logger.getTimestamp()/1000000.0; double oneSecondAgo = currentTime - 1.0; lastSecondMeasurements.removeIf(record -> record.timestamp < oneSecondAgo); - List log = new ArrayList<>(); - List validMeasurementsPose = new ArrayList<>(); - List invalidMeasurementsPose = new ArrayList<>();; - List acceptedTagsPose = new ArrayList<>(); for (Map.Entry> queueEntry : visionMeasurementQueueMap.entrySet()) { Queue visionMeasurementQueue = queueEntry.getValue(); LinkedHashMap filteredData = @@ -90,6 +89,8 @@ public void processQueue() { } } } + } + public void log() { Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new)); Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.toArray(Pose2d[]::new)); Logger.recordOutput("Apriltag/numberAcceptedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.ACCEPTED).count()); @@ -97,8 +98,11 @@ public void processQueue() { Logger.recordOutput("Apriltag/numberRejectedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.REJECTED).count()); Logger.recordOutput("Apriltag/acceptedTagPose", acceptedTagsPose.toArray(Pose3d[]::new)); Logger.recordOutput("Apriltag/Log", log.toArray(VisionLog[]::new)); + validMeasurementsPose.clear(); + invalidMeasurementsPose.clear(); + acceptedTagsPose.clear(); + log.clear(); } - 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 index ed51a0d9..f217a2e9 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 @@ -77,7 +77,6 @@ public PoseEstimator( initGyroValueDeg);*/ TimeInterpolatableBuffer m1Buffer = TimeInterpolatableBuffer.createBuffer(Constants.POSE_BUFFER_STORAGE_TIME); - VisionTruster truster = new SquareVisionTruster(Constants.INITIAL_VISION_STD_DEVS, Constants.VISION_STD_DEV_CONST); this.poseManager = new FilterablePoseManager( Constants.INITIAL_VISION_STD_DEVS, @@ -136,7 +135,8 @@ private void updateVision(int... invalidApriltagNumbers) { } long end = System.currentTimeMillis(); Logger.recordOutput("RegisteringVisionTimeMillis", end - start); - poseManager.processQueue(); + poseManager.processQueue(); + poseManager.log(); } private VisionMeasurement getVisionMeasurement(double[] pos, int index) { 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 4e1e4d81..7046e721 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 @@ -67,7 +67,10 @@ public void registerVisionMeasurement(VisionMeasurement measurement, int tagId) if (measurement == null) { return; } - visionMeasurementQueueMap.computeIfAbsent(tagId, k -> new LinkedList<>()).add(measurement); + while (visionMeasurementQueueMap.computeIfAbsent(tagId,k -> new LinkedList<>()).size() >= 3) { + visionMeasurementQueueMap.get(tagId).poll(); + } + visionMeasurementQueueMap.get(tagId).add(measurement); } // override for filtering 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 5ffd8a0f..edd28377 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 @@ -3,6 +3,9 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N3; public class SquareVisionTruster extends DistanceVisionTruster { @@ -18,8 +21,10 @@ public SquareVisionTruster(Vector initialSTD, double constant) { @Override public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { Pose3d adjPose = measurement.tag().tag().getPose().relativeTo(cameraPose); - double cosIncidenceAngle = (-adjPose.getX()*Math.cos(adjPose.getRotation().getZ())-adjPose.getY()*Math.sin(adjPose.getRotation().getZ()))/(adjPose.getTranslation().getNorm()); - double std = Math.pow(measurement.distanceFromTag(), 2) * constant / cosIncidenceAngle; + Vector tagNormalVector = new Translation3d(new Translation2d(1,new Rotation2d(adjPose.getRotation().getZ()))).toVector(); + Vector tagToCameraVector = cameraPose.relativeTo(measurement.tag().tag().getPose()).getTranslation().toVector(); + double distanceTimesCosIncidenceAngle = -tagNormalVector.dot(tagToCameraVector); + double std = Math.pow(measurement.distanceFromTag(), 2) * constant / distanceTimesCosIncidenceAngle; return initialSTD.plus(VecBuilder.fill(std, std, std*10000)); } } diff --git a/src/main/java/frc/robot/utils/math/ObjectUtils.java b/src/main/java/frc/robot/utils/math/ObjectUtils.java index 773471e4..55f39c6f 100644 --- a/src/main/java/frc/robot/utils/math/ObjectUtils.java +++ b/src/main/java/frc/robot/utils/math/ObjectUtils.java @@ -1,6 +1,12 @@ package frc.robot.utils.math; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N3; import static java.lang.Math.PI; import static java.lang.Math.abs; @@ -11,12 +17,10 @@ public static boolean canSee(Pose3d objectPose, Pose3d cameraPose, double Horizo Pose3d adjPose = objectPose.relativeTo(cameraPose); double horizontalAngle = Math.atan2(adjPose.getY(), adjPose.getX()); double verticalAngle = Math.asin(adjPose.getZ()/adjPose.getTranslation().getNorm()); - double tagAngle = adjPose.getRotation().getZ()+PI; - double diffAngle = abs(horizontalAngle-tagAngle); - if (diffAngle>=2*PI) { - diffAngle-=2*PI; - } - if (diffAngle >= PI/2 && diffAngle <=3*PI/2) { + Vector tagNormalVector = new Translation3d(new Translation2d(1,new Rotation2d(adjPose.getRotation().getZ()))).toVector(); + Vector tagToCameraVector = cameraPose.relativeTo(objectPose).getTranslation().toVector().unit(); + double cosIncidenceAngle = -tagNormalVector.dot(tagToCameraVector); + if (cosIncidenceAngle<=0) { return false; } return abs(horizontalAngle) < HorizontalFOV/2 && abs(verticalAngle) < VerticalFOV/2;// TODO: Maybe change Later By Implementing Math Calculations (Linear Algebra) From cae58085e11d89990f42643d95bc6908951700b1 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 22 Feb 2026 19:33:04 -0500 Subject: [PATCH 07/13] improved math (needs further fixes) --- src/main/java/frc/robot/apriltags/SimApriltagIO.java | 8 +++----- .../swervedrive/vision/truster/SquareVisionTruster.java | 6 ++---- src/main/java/frc/robot/utils/math/ObjectUtils.java | 8 ++++---- 3 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 57425787..0a1295c3 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -34,11 +34,9 @@ public void simReadings() { for (Apriltag tag : Apriltag.values()) { Pose3d cameraPos = new Pose3d(robotPoseSupplier.get().get()).transformBy(Constants.ROBOT_TO_CAMERA); if (ObjectUtils.canSee(tag.getPose(), cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { - Pose3d adjPose = tag.getPose().relativeTo(cameraPos); - Vector tagNormalVector = new Translation3d(new Translation2d(1,new Rotation2d(adjPose.getRotation().getZ()))).toVector(); - Vector tagToCameraVector = cameraPos.relativeTo(tag.getPose()).getTranslation().toVector(); - double distanceTimesCosIncidenceAngle = -tagNormalVector.dot(tagToCameraVector); - double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); + Translation3d adjPose2 = cameraPos.relativeTo(tag.getPose()).getTranslation(); + double distance = adjPose2.getNorm(); + double distanceTimesCosIncidenceAngle = adjPose2.getX(); if (distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0); Vector stdDevs = truster.calculateTrust(measurement, cameraPos); 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 edd28377..0dec2537 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 @@ -20,10 +20,8 @@ public SquareVisionTruster(Vector initialSTD, double constant) { @Override public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { - Pose3d adjPose = measurement.tag().tag().getPose().relativeTo(cameraPose); - Vector tagNormalVector = new Translation3d(new Translation2d(1,new Rotation2d(adjPose.getRotation().getZ()))).toVector(); - Vector tagToCameraVector = cameraPose.relativeTo(measurement.tag().tag().getPose()).getTranslation().toVector(); - double distanceTimesCosIncidenceAngle = -tagNormalVector.dot(tagToCameraVector); + Translation3d adjPose2 = cameraPose.relativeTo(measurement.tag().tag().getPose()).getTranslation(); + double distanceTimesCosIncidenceAngle = adjPose2.getX(); double std = Math.pow(measurement.distanceFromTag(), 2) * constant / distanceTimesCosIncidenceAngle; return initialSTD.plus(VecBuilder.fill(std, std, std*10000)); } diff --git a/src/main/java/frc/robot/utils/math/ObjectUtils.java b/src/main/java/frc/robot/utils/math/ObjectUtils.java index 55f39c6f..d203a7d8 100644 --- a/src/main/java/frc/robot/utils/math/ObjectUtils.java +++ b/src/main/java/frc/robot/utils/math/ObjectUtils.java @@ -17,10 +17,10 @@ public static boolean canSee(Pose3d objectPose, Pose3d cameraPose, double Horizo Pose3d adjPose = objectPose.relativeTo(cameraPose); double horizontalAngle = Math.atan2(adjPose.getY(), adjPose.getX()); double verticalAngle = Math.asin(adjPose.getZ()/adjPose.getTranslation().getNorm()); - Vector tagNormalVector = new Translation3d(new Translation2d(1,new Rotation2d(adjPose.getRotation().getZ()))).toVector(); - Vector tagToCameraVector = cameraPose.relativeTo(objectPose).getTranslation().toVector().unit(); - double cosIncidenceAngle = -tagNormalVector.dot(tagToCameraVector); - if (cosIncidenceAngle<=0) { + Translation3d adjPose2 = cameraPose.relativeTo(objectPose).getTranslation(); + double distanceToCamera = adjPose2.getNorm(); + double cosIncidence = adjPose2.getX() / distanceToCamera; + if (cosIncidence<=0) { return false; } return abs(horizontalAngle) < HorizontalFOV/2 && abs(verticalAngle) < VerticalFOV/2;// TODO: Maybe change Later By Implementing Math Calculations (Linear Algebra) From 127e6caf1e03d44bc461c6198be275d970c3f1ba Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 22 Feb 2026 21:59:38 -0500 Subject: [PATCH 08/13] commit --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/constants/GameConstants.java | 2 +- .../swervedrive/vision/estimation/PoseEstimator.java | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 50149239..4c7bd737 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,6 +62,7 @@ //import frc.robot.subsystems.TiltSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; +import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.logging.io.gyro.RealGyroIo; import frc.robot.utils.logging.io.gyro.ThreadedGyro; @@ -120,7 +121,7 @@ public class RobotContainer { private AutoFactory autoFactory; private static AutoRoutine straightRoutine; private static AutoTrajectory straightTrajectory; - private final VisionTruster truster = new ConstantVisionTruster(visionMeasurementStdDevs2); + private final VisionTruster truster = new SquareVisionTruster(visionMeasurementStdDevs2, Constants.VISION_STD_DEV_CONST); // Replace with CommandPS4Controller or CommandJoystick if needed // new CommandXboxController(OperatorConstants.kDriverControllerPort);private diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index a5fa6acc..46c1dd4e 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -162,7 +162,7 @@ public enum Mode { public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later - public static final double MAX_VISION_DISTANCE_SIMULATION = 6; + public static final double MAX_VISION_DISTANCE_SIMULATION = 3.5; public static final double AVERAGE_PIR_LATENCY = 20; //ms public static final double VISION_SMOOTHER = 1.0; public static final Vector INITIAL_VISION_STD_DEVS = VecBuilder.fill(0,0,0); // TODO: Change later 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 f217a2e9..b60b0348 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 @@ -48,7 +48,7 @@ public class PoseEstimator { 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); + public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0, 0, 100); private final FilterablePoseManager poseManager; public PoseEstimator( From 0bf7157f401d519900001880cae39d84750aaeed Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 1 Mar 2026 13:26:04 -0500 Subject: [PATCH 09/13] dsfgsdf --- .idea/modules.xml | 8 -------- simgui-ds.json | 8 ++++++++ src/main/java/frc/robot/apriltags/SimApriltagIO.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 2 +- .../swervedrive/vision/truster/SquareVisionTruster.java | 2 +- 5 files changed, 11 insertions(+), 11 deletions(-) delete mode 100644 .idea/modules.xml diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index 1a56902e..00000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713c..4726e2df 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,13 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + }, + { + "guid": "Keyboard1" + } ] } diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 0a1295c3..cca7487a 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -37,7 +37,7 @@ public void simReadings() { Translation3d adjPose2 = cameraPos.relativeTo(tag.getPose()).getTranslation(); double distance = adjPose2.getNorm(); double distanceTimesCosIncidenceAngle = adjPose2.getX(); - if (distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + if (distance*distance/distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0); Vector stdDevs = truster.calculateTrust(measurement, cameraPos); double readingX = robotPoseSupplier.get().get().getX() + random.nextGaussian() * stdDevs.get(0); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 46c1dd4e..b6bd8086 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -164,7 +164,7 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 3.5; public static final double AVERAGE_PIR_LATENCY = 20; //ms - public static final double VISION_SMOOTHER = 1.0; + public static final double VISION_SMOOTHER = 100.0; public static final Vector INITIAL_VISION_STD_DEVS = VecBuilder.fill(0,0,0); // TODO: Change later public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later } 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 0dec2537..6b1d76d2 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 @@ -21,7 +21,7 @@ public SquareVisionTruster(Vector initialSTD, double constant) { @Override public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { Translation3d adjPose2 = cameraPose.relativeTo(measurement.tag().tag().getPose()).getTranslation(); - double distanceTimesCosIncidenceAngle = adjPose2.getX(); + double distanceTimesCosIncidenceAngle = adjPose2.getX()/adjPose2.getNorm(); double std = Math.pow(measurement.distanceFromTag(), 2) * constant / distanceTimesCosIncidenceAngle; return initialSTD.plus(VecBuilder.fill(std, std, std*10000)); } From 2c7560aa570679ff3cade8a404d9711b9ea93026 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 1 Mar 2026 13:38:15 -0500 Subject: [PATCH 10/13] most pr comments addressed --- src/main/java/frc/robot/apriltags/SimApriltagIO.java | 12 ++++++------ src/main/java/frc/robot/constants/GameConstants.java | 3 ++- .../java/frc/robot/subsystems/ApriltagSubsystem.java | 6 ------ .../subsystems/swervedrive/SwerveSubsystem.java | 1 + .../vision/truster/BasicVisionFilter.java | 2 +- .../java/frc/robot/utils/logging/io/BaseIoImpl.java | 2 +- 6 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 4f82ffc9..913091aa 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -37,12 +37,13 @@ public void simReadings() { Pose3d adjPose = tag.getPose().relativeTo(cameraPos); double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); - if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + if (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0); Vector stdDevs = truster.calculateTrust(measurement); - double readingX = robotPoseSupplier.get().get().getX() + random.nextGaussian() * stdDevs.get(0); - double readingY = robotPoseSupplier.get().get().getY() + random.nextGaussian() * stdDevs.get(1); - double readingYaw = robotPoseSupplier.get().get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); + Pose2d pose = robotPoseSupplier.get().get(); + double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); + double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); + double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); if (BasicVisionFilter.inBounds(readingPos)) { @@ -56,8 +57,7 @@ public void simReadings() { } @Override public void periodic() { - updateInputs(inputs); - Logger.processInputs(prefix, inputs); + super.periodic(); simReadings(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 89171ef9..a76cb2ba 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -151,7 +151,8 @@ public enum Mode { public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed) public static final boolean ENABLE_VISION = true; public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back) - + public static final double FIELD_LENGTH = 16.5; //TODO: Change Later + public static final double FIELD_WIDTH = 8.1; //TODO: Change Later // Vision public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index 99c2bb33..ee38a4a6 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -55,16 +55,10 @@ public void addSimReading(ApriltagReading reading) { public ApriltagIO getIO(){ return io; } - public VisionTruster getVisionTruster() { - return estimator.getVisionTruster(); - } @Override public void periodic() { estimator.updateVision(); estimator.updatePosition(drivebase.getOdom()); io.periodic(); } - public Vector calculateTrust(VisionMeasurement measurement) { - return estimator.getVisionTruster().calculateTrust(measurement); - } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index a4738fb1..4dae0a3b 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -298,6 +298,7 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + public Optional getSimulationPose() { return swerveDrive.getSimulationDriveTrainPose(); } 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 index ae99851c..388715c1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -80,6 +80,6 @@ private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double } public static boolean inBounds(Pose2d pose2d) { - return pose2d.getX() > 0 && pose2d.getX() < 16.5 && pose2d.getY() > 0 && pose2d.getY() < 8.1; + return pose2d.getX() > 0 && pose2d.getX() < Constants.FIELD_LENGTH && pose2d.getY() > 0 && pose2d.getY() < Constants.FIELD_WIDTH; } } 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 6113812b..0fe698cb 100644 --- a/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java +++ b/src/main/java/frc/robot/utils/logging/io/BaseIoImpl.java @@ -9,7 +9,7 @@ */ public abstract class BaseIoImpl implements BaseIo { // The name of the "folder" where the logs from this IO will be logged - protected final String prefix; + private final String prefix; protected final I inputs; public BaseIoImpl(String folder, I inputs) { From 50d395e18063d1c77988faa52afe8898bd3aaea6 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 1 Mar 2026 13:44:48 -0500 Subject: [PATCH 11/13] all addressed or comment or question --- .../java/frc/robot/apriltags/SimApriltagIO.java | 13 +++++++------ .../frc/robot/subsystems/ApriltagSubsystem.java | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 913091aa..3434607b 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.numbers.N3; import frc.robot.constants.Constants; import frc.robot.subsystems.ApriltagSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; @@ -23,16 +24,17 @@ public class SimApriltagIO extends TCPApriltagIo { private final Random random = new Random(); private final VisionTruster truster; - private final Supplier> robotPoseSupplier; - public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server, VisionTruster truster, Supplier> robotPoseSupplier) { + private final SwerveSubsystem swerveSubsystem; + public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server, VisionTruster truster, SwerveSubsystem swerveSubsystem) { super(name, inputs, server); this.truster = truster; - this.robotPoseSupplier = robotPoseSupplier; + this.swerveSubsystem = swerveSubsystem; } public void simReadings() { - if (robotPoseSupplier.get().isPresent()) { + if (swerveSubsystem.getSimulationPose().isPresent()) { + Pose2d pose = swerveSubsystem.getSimulationPose().get(); for (Apriltag tag : Apriltag.values()) { - Pose3d cameraPos = new Pose3d(robotPoseSupplier.get().get()).transformBy(Constants.ROBOT_TO_CAMERA); + Pose3d cameraPos = new Pose3d(pose).transformBy(Constants.ROBOT_TO_CAMERA); if (ObjectUtils.canSee(tag.getPose(), cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { Pose3d adjPose = tag.getPose().relativeTo(cameraPos); double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); @@ -40,7 +42,6 @@ public void simReadings() { if (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0); Vector stdDevs = truster.calculateTrust(measurement); - Pose2d pose = robotPoseSupplier.get().get(); double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index ee38a4a6..7439c7c2 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -46,7 +46,7 @@ public static ApriltagIO createMockIo() { } public static ApriltagIO createSimIo(VisionTruster truster, SwerveSubsystem drivebase) { - return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0), truster, drivebase::getSimulationPose); // port doesnt matter at all + return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0), truster, drivebase); // 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) { From e059a2f47c3a4dd55fc71228e0e1215b5976b8c5 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 1 Mar 2026 13:53:02 -0500 Subject: [PATCH 12/13] pr comments fixed. --- .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 2 -- .../subsystems/swervedrive/vision/estimation/PoseManager.java | 2 +- 2 files changed, 1 insertion(+), 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 6efc2da8..8d7bd07e 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -55,7 +55,6 @@ public class SwerveSubsystem extends SubsystemBase { */ private final SwerveDrive swerveDrive; private Vector variance = VecBuilder.fill(0.1,0.1,0.1); - private final Field2d rawOdomField = new Field2d(); private SwerveDriveOdometry rawOdometry; private final ConcurrentLinkedDeque poseError = new ConcurrentLinkedDeque<>(); private record PoseErrorRecord(double timestamp, double error) { @@ -130,7 +129,6 @@ public void periodic() { swerveDrive.getModulePositions() ); - rawOdomField.setRobotPose(rawOdometry.getPoseMeters()); if (Constants.currentMode == GameConstants.Mode.SIM) { double currentTime = Logger.getTimestamp() / 1000000.0; double oneSecondAgo = currentTime - 1.0; 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 aae43560..3f979f9f 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 @@ -24,7 +24,7 @@ public class PoseManager { private final TimeInterpolatableBuffer estimatedPoseBuffer; //private final SwerveDrivePoseEstimator poseEstimator; protected final Queue visionMeasurementQueue = new LinkedList<>(); - protected final SwerveSubsystem drivebase; + private final SwerveSubsystem drivebase; protected final VisionTruster visionTruster; public PoseManager( From e26bea78b32a037ce6f964179273819f2837b527 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 1 Mar 2026 16:08:52 -0500 Subject: [PATCH 13/13] merged --- src/main/java/frc/robot/apriltags/SimApriltagIO.java | 2 +- .../subsystems/swervedrive/vision/estimation/PoseManager.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index f5b17ef4..6b4c7816 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -39,7 +39,7 @@ public void simReadings() { Translation3d adjPose2 = cameraPos.relativeTo(tag.getPose()).getTranslation(); double distance = adjPose2.getNorm(); double distanceTimesCosIncidenceAngle = adjPose2.getX(); - if (cosIncidenceAngle!=0 && distance*distance/distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + if (distanceTimesCosIncidenceAngle!=0 && distance*distance/distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0); Vector stdDevs = truster.calculateTrust(measurement, cameraPos); double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); 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 79f9ccf6..7046e721 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 @@ -25,7 +25,7 @@ public class PoseManager { private final TimeInterpolatableBuffer estimatedPoseBuffer; //private final SwerveDrivePoseEstimator poseEstimator; protected final LinkedHashMap> visionMeasurementQueueMap = new LinkedHashMap<>(); - private final SwerveSubsystem drivebase; + protected final SwerveSubsystem drivebase; protected final VisionTruster visionTruster; public PoseManager(