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/16] 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/16] 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/16] 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 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 04/16] 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 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 05/16] 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 06/16] 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 07/16] 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 49f822e83d274ad9615c531c5263e437c797fa70 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 4 Mar 2026 19:37:35 -0500 Subject: [PATCH 08/16] fixed pr comments --- .../frc/robot/commands/drive/DriveCircle.java | 44 ------------------- .../vision/estimation/PoseEstimator.java | 2 +- 2 files changed, 1 insertion(+), 45 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/drive/DriveCircle.java diff --git a/src/main/java/frc/robot/commands/drive/DriveCircle.java b/src/main/java/frc/robot/commands/drive/DriveCircle.java deleted file mode 100644 index 76366ba8..00000000 --- a/src/main/java/frc/robot/commands/drive/DriveCircle.java +++ /dev/null @@ -1,44 +0,0 @@ -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/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 581f7da0..7194dcad 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], Rotation2d.fromDegrees(pos[2])); + Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000); } From 5754b469cf55e70393d153671d2856e608508a77 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 4 Mar 2026 19:46:34 -0500 Subject: [PATCH 09/16] only way to not thrown null pointer --- .../swervedrive/vision/estimation/PoseEstimator.java | 7 ++++++- .../swervedrive/vision/estimation/PoseManager.java | 5 +++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 7194dcad..17e4bc14 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,12 @@ 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; + if (getEstimatedPose()!=null) { + visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); + } else { + visionPose = new Pose2d(pos[0], pos[1], poseManager.getRotation()); + } double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000); } 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..9658b8a8 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 @@ -2,6 +2,7 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; @@ -117,4 +118,8 @@ public Pose2d getEstimatedPosition() { return sample.get(); } } + + public Rotation2d getRotation() { + return drivebase.getHeading(); + } } From bd2fac079e1f00e8ec287907b5229141325bfd09 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 4 Mar 2026 20:14:17 -0500 Subject: [PATCH 10/16] small logging change --- .../estimation/FilterablePoseManager.java | 32 ++++++++++--------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java index 255b51c0..6abab188 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 @@ -6,6 +6,7 @@ import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; +import frc.robot.constants.GameConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.FilterResult; import frc.robot.subsystems.swervedrive.vision.truster.PoseDeviation; @@ -25,8 +26,7 @@ */ 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 record MeasurementRecord(int tag, double timestamp, FilterResult result) { } private final ConcurrentLinkedDeque lastSecondMeasurements = new ConcurrentLinkedDeque<>(); public FilterablePoseManager( @@ -60,9 +60,8 @@ 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 validMeasurementsPose = new ArrayList<>(); +// List invalidMeasurementsPose = new ArrayList<>(); List acceptedTagsPose = new ArrayList<>(); LinkedHashMap filteredData = @@ -72,29 +71,32 @@ public void processQueue() { 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)); + lastSecondMeasurements.add(new MeasurementRecord(tag.number(), v.timeOfMeasurement(),r)); switch (r) { case ACCEPTED -> { setVisionSTD(visionTruster.calculateTrust(v)); - validMeasurementsPose.add(v.measurement()); +// validMeasurementsPose.add(v.measurement()); addVisionMeasurement(v); - acceptedTagsPose.add(tag.getPose()); + if (GameConstants.currentMode == GameConstants.Mode.SIM) { + acceptedTagsPose.add(tag.getPose()); + } } case NOT_PROCESSED -> visionMeasurementQueue.add(v); case REJECTED -> { - invalidMeasurementsPose.add(v.measurement()); +// invalidMeasurementsPose.add(v.measurement()); } } } - Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new)); - Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.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/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)); + Logger.recordOutput("Apriltag/acceptedIdsLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.ACCEPTED).mapToInt(record->record.tag).distinct().toArray()); + if (GameConstants.currentMode == GameConstants.Mode.SIM) { + Logger.recordOutput("Apriltag/acceptedTagPose", acceptedTagsPose.toArray(Pose3d[]::new)); + } } public VisionTruster getVisionTruster() { From a47b8f7b0a0b7745cb867d07759ee3ec89414ecc Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 9 Mar 2026 20:00:05 -0400 Subject: [PATCH 11/16] fixed --- src/main/java/frc/robot/apriltags/SimApriltagIO.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 9 --------- 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 18e7adea..69add96e 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -49,7 +49,7 @@ public void simReadings() { 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)); + distance,0,tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, stdDevs.get(0),Logger.getTimestamp() / 1000.0)); } } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 605a71bf..0a2cd306 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -170,13 +170,4 @@ 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 = 6; public static final String DRIVER_CAM_IP_ADDRESS = "10.40.48.2:1181/?action=stream"; - 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 - 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; } From 76bf35f2c36fb725d8868906e4dee9d3fc047b17 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 9 Mar 2026 20:16:01 -0400 Subject: [PATCH 12/16] fixed --- .../frc/robot/apriltags/SimApriltagIO.java | 2 +- .../estimation/FilterablePoseManager.java | 28 ++++--------------- .../vision/estimation/PoseEstimator.java | 4 +-- .../vision/truster/VisionMeasurement.java | 2 +- src/main/java/frc/robot/utils/Apriltag.java | 4 --- 5 files changed, 10 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 69add96e..d66581b9 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -40,7 +40,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 (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { - VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0); + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0); Vector stdDevs = truster.calculateTrust(measurement); double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); 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 6abab188..4452c1d1 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 @@ -26,8 +26,6 @@ */ public class FilterablePoseManager extends PoseManager { private final VisionFilter filter; - private record MeasurementRecord(int tag, double timestamp, FilterResult result) { } - private final ConcurrentLinkedDeque lastSecondMeasurements = new ConcurrentLinkedDeque<>(); public FilterablePoseManager( PoseDeviation PoseDeviation, @@ -57,46 +55,32 @@ public FilterablePoseManager( @Override public void processQueue() { - double currentTime = Logger.getTimestamp()/1000000.0; - double oneSecondAgo = currentTime - 1.0; - lastSecondMeasurements.removeIf(record -> record.timestamp < oneSecondAgo); -// List validMeasurementsPose = new ArrayList<>(); -// List invalidMeasurementsPose = new ArrayList<>(); - List acceptedTagsPose = new ArrayList<>(); + List validMeasurementsPose = new ArrayList<>(); + List invalidMeasurementsPose = new ArrayList<>(); 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(); - lastSecondMeasurements.add(new MeasurementRecord(tag.number(), v.timeOfMeasurement(),r)); switch (r) { case ACCEPTED -> { setVisionSTD(visionTruster.calculateTrust(v)); -// validMeasurementsPose.add(v.measurement()); + validMeasurementsPose.add(v.measurement()); addVisionMeasurement(v); - if (GameConstants.currentMode == GameConstants.Mode.SIM) { - acceptedTagsPose.add(tag.getPose()); - } } case NOT_PROCESSED -> visionMeasurementQueue.add(v); case REJECTED -> { -// invalidMeasurementsPose.add(v.measurement()); + invalidMeasurementsPose.add(v.measurement()); } } } // 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/acceptedIdsLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.ACCEPTED).mapToInt(record->record.tag).distinct().toArray()); - if (GameConstants.currentMode == GameConstants.Mode.SIM) { - Logger.recordOutput("Apriltag/acceptedTagPose", acceptedTagsPose.toArray(Pose3d[]::new)); - } + Logger.recordOutput("Apriltag/numberAccepted", validMeasurementsPose.size()); + Logger.recordOutput("Apriltag/numberRejected", invalidMeasurementsPose.size()); } 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 477a67e4..c64365c6 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 @@ -152,7 +152,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { visionPose = new Pose2d(pos[0], pos[1], poseManager.getRotation()); } double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; - return new VisionMeasurement(visionPose, Apriltag.of(apriltagSystem.getIO().getInputs().apriltagNumber[index]).getTagInfo(),distanceFromTag, timestamp/1000); + return new VisionMeasurement(visionPose,distanceFromTag, timestamp/1000); } /** @@ -206,7 +206,7 @@ public FilterablePoseManager getPoseManager() { public void addMockVisionMeasurement() { poseManager.registerVisionMeasurement( - new VisionMeasurement(getEstimatedPose(), Apriltag.of(1).getTagInfo(), 0, Logger.getTimestamp() / 1e6)); + 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/VisionMeasurement.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java index 82b54ead..0fcc6d57 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 @@ -10,4 +10,4 @@ * @param timeOfMeasurement time when the pose was measured (seconds) */ public record VisionMeasurement( - Pose2d measurement, Apriltag.TagPose tag, double distanceFromTag, double timeOfMeasurement) {} + Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} diff --git a/src/main/java/frc/robot/utils/Apriltag.java b/src/main/java/frc/robot/utils/Apriltag.java index 8b134452..badbe99b 100644 --- a/src/main/java/frc/robot/utils/Apriltag.java +++ b/src/main/java/frc/robot/utils/Apriltag.java @@ -82,7 +82,6 @@ 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; @@ -133,7 +132,4 @@ public Pose3d getPose() { public int number() { return ordinal()+1; } - public TagPose getTagInfo() { - return new TagPose(this, pose); - } } \ No newline at end of file From 66f7fd91c5e06f5f75ab319f7705d90f709f039e Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 9 Mar 2026 20:38:53 -0400 Subject: [PATCH 13/16] fixed --- src/main/java/frc/robot/Robot.java | 28 ++++++++----------- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/constants/GameConstants.java | 2 +- .../vision/estimation/PoseEstimator.java | 2 +- .../vision/truster/VisionMeasurement.java | 3 +- 5 files changed, 15 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2d05c6cc..b0f17cb4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -139,24 +139,18 @@ public void robotPeriodic() { // Gets the alliance color. if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { allianceColor = DriverStation.getAlliance(); - }if (Constants.DEBUG) { - SmartDashboard.putNumber("driverXbox.getLeftY()",driverXbox.getLeftY()); - SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); - SmartDashboard.putString("DeploymentState", robotContainer.getDeployer().getDeploymentState().toString()); - if(!Constants.TESTBED){ - Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); - Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom()); - if (Constants.currentMode == GameConstants.Mode.SIM) { - Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get()); - } - // Puts data on the elastic dashboard - SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); - SmartDashboard.putBoolean("Hub Active?", hubActive()); - } + } - // Puts data on the elastic dashboard - SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); - SmartDashboard.putBoolean("Hub Active?", hubActive()); + if (Constants.DEBUG) { + SmartDashboard.putNumber("driverXbox.getLeftY()", driverXbox.getLeftY()); + SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); + SmartDashboard.putString("DeploymentState", robotContainer.getDeployer().getDeploymentState().toString()); + if (!Constants.TESTBED) { + Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); + // Puts data on the elastic dashboard + SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); + SmartDashboard.putBoolean("Hub Active?", hubActive()); + } } SmartDashboard.putString("Selected Action", robotContainer.getAutoChooser().getCommandDescription()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c24d753d..a1a0895e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,4 +1,4 @@ -// Copyright (c) FIRST and other WPILib contributors. + // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 0a2cd306..d93ac3e4 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -160,6 +160,7 @@ 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 String DRIVER_CAM_IP_ADDRESS = "10.40.48.2:1181/?action=stream"; public static final double FIELD_LENGTH = 16.5; //TODO: Change Later public static final double FIELD_WIDTH = 8.1; //TODO: Change Later // Vision @@ -169,5 +170,4 @@ 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 String DRIVER_CAM_IP_ADDRESS = "10.40.48.2:1181/?action=stream"; } 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 c64365c6..17e4bc14 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 @@ -152,7 +152,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { visionPose = new Pose2d(pos[0], pos[1], poseManager.getRotation()); } double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; - return new VisionMeasurement(visionPose,distanceFromTag, timestamp/1000); + return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000); } /** 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 0fcc6d57..c4913a8b 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 @@ -9,5 +9,4 @@ * @param distanceFromTag distance (meters) estimated robot pose was from the tag * @param timeOfMeasurement time when the pose was measured (seconds) */ -public record VisionMeasurement( - Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} +public record VisionMeasurement(Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} From 5c6089243d3ccbef2d0eeb93416d6a9fe9326561 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 9 Mar 2026 20:44:11 -0400 Subject: [PATCH 14/16] wip --- src/main/java/frc/robot/Robot.java | 15 ++++----------- .../java/frc/robot/constants/GameConstants.java | 2 +- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b0f17cb4..8ac88525 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,17 +4,6 @@ package frc.robot; -import java.util.Optional; -import java.util.concurrent.atomic.AtomicReference; - -import frc.robot.constants.GameConstants; -import frc.robot.utils.diag.Diagnostics; -import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -import org.littletonrobotics.junction.wpilog.WPILOGWriter; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; @@ -150,6 +139,10 @@ public void robotPeriodic() { // Puts data on the elastic dashboard SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); SmartDashboard.putBoolean("Hub Active?", hubActive()); + if (Constants.currentMode == Constants.Mode.SIM) { + Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get()); + Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom()); + } } } SmartDashboard.putString("Selected Action", diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index d93ac3e4..9f1dde9f 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick From cdde152efd7d843086ba857e0935a960f427ce09 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 9 Mar 2026 20:44:34 -0400 Subject: [PATCH 15/16] wip --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a1a0895e..82a30ca5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,7 +83,6 @@ import static frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator.visionMeasurementStdDevs2; -import static frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator.visionMeasurementStdDevs2; /** * This class is where the bulk of the robot should be declared. Since From e11869c8b1b7ee31b1b4196ce0b41189e9c4d8b9 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Mon, 9 Mar 2026 20:52:32 -0400 Subject: [PATCH 16/16] done --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../swervedrive/SwerveSubsystem.java | 33 +++++++++++-------- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 82a30ca5..5c5e5232 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,4 +1,4 @@ - // Copyright (c) FIRST and other WPILib contributors. +// Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. @@ -83,7 +83,6 @@ 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 diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 27b778f4..6053afc7 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -94,12 +94,14 @@ 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( + if (Constants.currentMode==Constants.simMode) { + rawOdometry = new SwerveDriveOdometry( swerveDrive.kinematics, swerveDrive.getOdometryHeading(), swerveDrive.getModulePositions(), - startingPose - ); + startingPose); + } + } /** @@ -121,17 +123,16 @@ 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() - ); - if (Constants.currentMode == GameConstants.Mode.SIM) { 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()); + rawOdometry.update( + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions() + ); } } @@ -313,11 +314,13 @@ public SwerveDriveKinematics getKinematics() { // 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(); + if (Constants.currentMode==Constants.simMode) { + SwerveModulePosition[] modules = new SwerveModulePosition[4]; + for (int i=0; i<4; i++) { + modules[i] = new SwerveModulePosition(); + } + rawOdometry.resetPosition(initialHolonomicPose.getRotation(), modules, initialHolonomicPose); } - rawOdometry.resetPosition(initialHolonomicPose.getRotation(), modules, initialHolonomicPose); } /** @@ -340,7 +343,11 @@ public Optional getSimulationPose() { } // Todo: fix to only get odomtry public Pose2d getOdom() { - return rawOdometry.getPoseMeters(); + if (Constants.currentMode==Constants.simMode) { + return rawOdometry.getPoseMeters(); + } else { + return getPose(); + } } /**