From f5319d614dafcc16417cacc918cf6d6ac0191984 Mon Sep 17 00:00:00 2001 From: akasbyju0 <94997492+akasbyju0@users.noreply.github.com> Date: Sat, 17 Jan 2026 14:42:14 -0500 Subject: [PATCH 1/6] Built vision --- src/main/java/frc/robot/Robot.java | 1 - .../frc/robot/subsystems/vision/Vision.java | 156 ++++++++++++++++++ .../subsystems/vision/VisionConstants.java | 30 ++++ .../frc/robot/subsystems/vision/VisionIO.java | 34 ++++ .../subsystems/vision/VisionIOLimelight.java | 109 ++++++++++++ .../vision/VisionIOPhotonVision.java | 103 ++++++++++++ .../vision/VisionIOPhotonVisionSim.java | 38 +++++ 7 files changed, 470 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/vision/Vision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionConstants.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 68c00ed..a670d71 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -137,4 +137,3 @@ public void simulationInit() {} @Override public void simulationPeriodic() {} } - diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..e00af7f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,156 @@ +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +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.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.VisionIO.PoseObservationType; +import java.util.LinkedList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +public class Vision extends SubsystemBase { + private final VisionConsumer consumer; + private final VisionIO[] io; + private final VisionIOInputsAutoLogged[] inputs; + private final Alert[] disconnectedAlerts; + + public Vision(VisionConsumer consumer, VisionIO... io) { + this.consumer = consumer; + this.io = io; + + this.inputs = new VisionIOInputsAutoLogged[io.length]; + for (int i = 0; i < inputs.length; i++) { + inputs[i] = new VisionIOInputsAutoLogged(); + } + + this.disconnectedAlerts = new Alert[io.length]; + for (int i = 0; i < inputs.length; i++) { + disconnectedAlerts[i] = + new Alert( + "Vision camera " + Integer.toString(i) + " is disconnected.", + AlertType.kWarning); + } + } + + public Rotation2d getTargetX(int cameraIndex) { + return inputs[cameraIndex].latestTargetObservation.tx(); + } + + @Override + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + } + + List allTagPoses = new LinkedList<>(); + List allRobotPoses = new LinkedList<>(); + List allRobotPosesAccepted = new LinkedList<>(); + List allRobotPosesRejected = new LinkedList<>(); + + for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { + disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + + List tagPoses = new LinkedList<>(); + List robotPoses = new LinkedList<>(); + List robotPosesAccepted = new LinkedList<>(); + List robotPosesRejected = new LinkedList<>(); + + for (int tagId : inputs[cameraIndex].tagIds) { + var tagPose = aprilTagLayout.getTagPose(tagId); + if (tagPose.isPresent()) { + tagPoses.add(tagPose.get()); + } + } + + for (var observation : inputs[cameraIndex].poseObservations) { + boolean rejectPose = + observation.tagCount() == 0 // Must have at least one tag + || (observation.tagCount() == 1 + && observation.ambiguity() + > maxAmbiguity) // Cannot be high ambiguity + || Math.abs(observation.pose().getZ()) + > maxZError // Must have realistic Z coordinate + || observation.pose().getX() < 0.0 + || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getY() < 0.0 + || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + + robotPoses.add(observation.pose()); + if (rejectPose) { + robotPosesRejected.add(observation.pose()); + } else { + robotPosesAccepted.add(observation.pose()); + } + + if (rejectPose) { + continue; + } + + double stdDevFactor = + Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); + double linearStdDev = linearStdDevBaseline * stdDevFactor; + double angularStdDev = angularStdDevBaseline * stdDevFactor; + if (observation.type() == PoseObservationType.MEGATAG_2) { + linearStdDev *= linearStdDevMegatag2Factor; + angularStdDev *= angularStdDevMegatag2Factor; + } + if (cameraIndex < cameraStdDevFactors.length) { + linearStdDev *= cameraStdDevFactors[cameraIndex]; + angularStdDev *= cameraStdDevFactors[cameraIndex]; + } + + consumer.accept( + observation.pose().toPose2d(), + observation.timestamp(), + VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); + } + + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", + tagPoses.toArray(new Pose3d[tagPoses.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", + robotPoses.toArray(new Pose3d[robotPoses.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", + robotPosesAccepted.toArray(new Pose3d[robotPosesAccepted.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", + robotPosesRejected.toArray(new Pose3d[robotPosesRejected.size()])); + allTagPoses.addAll(tagPoses); + allRobotPoses.addAll(robotPoses); + allRobotPosesAccepted.addAll(robotPosesAccepted); + allRobotPosesRejected.addAll(robotPosesRejected); + } + + Logger.recordOutput( + "Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPoses", + allRobotPoses.toArray(new Pose3d[allRobotPoses.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPosesAccepted", + allRobotPosesAccepted.toArray(new Pose3d[allRobotPosesAccepted.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPosesRejected", + allRobotPosesRejected.toArray(new Pose3d[allRobotPosesRejected.size()])); + } + + @FunctionalInterface + public interface VisionConsumer { + void accept( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java new file mode 100644 index 0000000..6d2e880 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; + +public class VisionConstants { + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + public static double[] cameraStdDevFactors = new double[] {1.0, 1.0}; + + public static double linearStdDevMegatag2Factor = 0.5; + public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..339ab8a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface VisionIO { + @AutoLog + class VisionIOInputs { + public boolean connected = false; + public TargetObservation latestTargetObservation = + new TargetObservation(new Rotation2d(), new Rotation2d()); + public PoseObservation[] poseObservations = new PoseObservation[0]; + public int[] tagIds = new int[0]; + } + + record TargetObservation(Rotation2d tx, Rotation2d ty) {} + + record PoseObservation( + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + PoseObservationType type) {} + + enum PoseObservationType { + MEGATAG_1, + MEGATAG_2, + PHOTONVISION + } + + default void updateInputs(VisionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java new file mode 100644 index 0000000..7dfa152 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -0,0 +1,109 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; + +public class VisionIOLimelight implements VisionIO { + private final Supplier rotationSupplier; + private final DoubleArrayPublisher orientationPublisher; + + private final DoubleSubscriber latencySubscriber; + private final DoubleSubscriber txSubscriber; + private final DoubleSubscriber tySubscriber; + private final DoubleArraySubscriber megatag1Subscriber; + private final DoubleArraySubscriber megatag2Subscriber; + + public VisionIOLimelight(String name, Supplier rotationSupplier) { + var table = NetworkTableInstance.getDefault().getTable(name); + this.rotationSupplier = rotationSupplier; + orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); + latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); + tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); + megatag1Subscriber = + table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); + megatag2Subscriber = + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + inputs.connected = + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; + + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(txSubscriber.get()), + Rotation2d.fromDegrees(tySubscriber.get())); + + orientationPublisher.accept( + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + NetworkTableInstance.getDefault() + .flush(); // Increases network traffic but recommended by Limelight + + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var rawSample : megatag1Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + parsePose(rawSample.value), + rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, + (int) rawSample.value[7], + rawSample.value[9], + PoseObservationType.MEGATAG_1)); + } + for (var rawSample : megatag2Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + parsePose(rawSample.value), + 0.0, + (int) rawSample.value[7], + rawSample.value[9], + PoseObservationType.MEGATAG_2)); + } + + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } + + private static Pose3d parsePose(double[] rawLLArray) { + return new Pose3d( + rawLLArray[0], + rawLLArray[1], + rawLLArray[2], + new Rotation3d( + Units.degreesToRadians(rawLLArray[3]), + Units.degreesToRadians(rawLLArray[4]), + Units.degreesToRadians(rawLLArray[5]))); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java new file mode 100644 index 0000000..2dff379 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -0,0 +1,103 @@ +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import org.photonvision.PhotonCamera; + +public class VisionIOPhotonVision implements VisionIO { + protected final PhotonCamera camera; + protected final Transform3d robotToCamera; + + public VisionIOPhotonVision(String name, Transform3d robotToCamera) { + camera = new PhotonCamera(name); + this.robotToCamera = robotToCamera; + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + inputs.connected = camera.isConnected(); + + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var result : camera.getAllUnreadResults()) { + if (result.hasTargets()) { + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + } else { + inputs.latestTargetObservation = + new TargetObservation(new Rotation2d(), new Rotation2d()); + } + + if (result.multitagResult.isPresent()) { + var multitagResult = result.multitagResult.get(); + + Transform3d fieldToCamera = multitagResult.estimatedPose.best; + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = + new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + double totalTagDistance = 0.0; + for (var target : result.targets) { + totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); + } + + tagIds.addAll(multitagResult.fiducialIDsUsed); + + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), + robotPose, + multitagResult.estimatedPose.ambiguity, + multitagResult.fiducialIDsUsed.size(), + totalTagDistance / result.targets.size(), + PoseObservationType.PHOTONVISION)); + + } else if (!result.targets.isEmpty()) { + var target = result.targets.get(0); + + var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = + new Transform3d( + tagPose.get().getTranslation(), tagPose.get().getRotation()); + Transform3d cameraToTarget = target.bestCameraToTarget; + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = + new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + tagIds.add((short) target.fiducialId); + + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), + robotPose, + target.poseAmbiguity, + 1, + cameraToTarget.getTranslation().getNorm(), + PoseObservationType.PHOTONVISION)); + } + } + } + + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java new file mode 100644 index 0000000..9b97280 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.function.Supplier; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { + private static VisionSystemSim visionSim; + + private final Supplier poseSupplier; + private final PhotonCameraSim cameraSim; + + public VisionIOPhotonVisionSim( + String name, Transform3d robotToCamera, Supplier poseSupplier) { + super(name, robotToCamera); + this.poseSupplier = poseSupplier; + + if (visionSim == null) { + visionSim = new VisionSystemSim("main"); + visionSim.addAprilTags(aprilTagLayout); + } + + var cameraProperties = new SimCameraProperties(); + cameraSim = new PhotonCameraSim(camera, cameraProperties); + visionSim.addCamera(cameraSim, robotToCamera); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + visionSim.update(poseSupplier.get()); + super.updateInputs(inputs); + } +} From 1d93ae73da2ee3ba1e30472359009a8a6ded338f Mon Sep 17 00:00:00 2001 From: akasbyju0 <94997492+akasbyju0@users.noreply.github.com> Date: Sat, 17 Jan 2026 17:49:12 -0500 Subject: [PATCH 2/6] Finished robot container --- src/main/java/frc/robot/RobotContainer.java | 53 +++++++++++++++---- .../frc/robot/subsystems/drive/Drive.java | 8 ++- 2 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b2016e..7e43920 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,8 +8,8 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -24,8 +24,11 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.vision.*; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import static frc.robot.subsystems.vision.VisionConstants.robotToCamera0; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -35,6 +38,7 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final Vision vision; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -71,18 +75,44 @@ public RobotContainer() { // new ModuleIOTalonFXS(TunerConstants.FrontRight), // new ModuleIOTalonFXS(TunerConstants.BackLeft), // new ModuleIOTalonFXS(TunerConstants.BackRight)); + vision = new Vision( + drive, + new VisionIOLimelight("Front Camera", drive::getRotation), + new VisionIOLimelight("Back Camera", drive::getRotation)); break; case SIM: // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(TunerConstants.FrontLeft), - new ModuleIOSim(TunerConstants.FrontRight), - new ModuleIOSim(TunerConstants.BackLeft), - new ModuleIOSim(TunerConstants.BackRight)); - break; + drive = new Drive( + new GyroIO() {}, + new ModuleIOSim(TunerConstants.FrontLeft), + new ModuleIOSim(TunerConstants.FrontRight), + new ModuleIOSim(TunerConstants.BackLeft), + new ModuleIOSim(TunerConstants.BackRight)); + vision = new Vision( + drive, + new VisionIOPhotonVisionSim( + "Front Camera", + new Transform3d( + new Translation3d( + Units.inchesToMeters(-3), + Units.inchesToMeters(0), + Units.inchesToMeters(15)), + new Rotation3d(0, Math.toRadians(0), Math.toRadians(180))), + drive::getPose), + new VisionIOPhotonVisionSim( + "Back Camera", + new Transform3d( + new Translation3d( + Units.inchesToMeters(3), + Units.inchesToMeters(0), + Units.inchesToMeters(15)), + new Rotation3d(0, Math.toRadians(0), Math.toRadians(0))), + drive::getPose)); + + + + break; default: // Replayed robot, disable IO implementations @@ -93,7 +123,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - break; + vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); + } // Set up auto routines diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 1c24394..61188c7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -40,13 +40,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.vision.Vision; import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Drive extends SubsystemBase { +public class Drive extends SubsystemBase implements Vision.VisionConsumer { // TunerConstants doesn't include these constants, so they are declared locally static final double ODOMETRY_FREQUENCY = new CANBus(TunerConstants.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; @@ -373,4 +374,9 @@ public static Translation2d[] getModuleTranslations() { TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } + + @Override + public void accept(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + } } From e79da900b0d70fe1fc446c0dc1343e1379dc051d Mon Sep 17 00:00:00 2001 From: akasbyju0 <94997492+akasbyju0@users.noreply.github.com> Date: Fri, 23 Jan 2026 18:32:57 -0500 Subject: [PATCH 3/6] resolve merge conflicts --- src/main/java/frc/robot/RobotContainer.java | 19 ------------------- .../frc/robot/subsystems/drive/Drive.java | 8 ++++++-- 2 files changed, 6 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 09a8999..7e43920 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,8 +25,6 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.vision.*; -import frc.robot.subsystems.flywheel.FlywheelIO; -import frc.robot.subsystems.flywheel.FlywheelIOTalonFX; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import static frc.robot.subsystems.vision.VisionConstants.robotToCamera0; @@ -41,7 +39,6 @@ public class RobotContainer { // Subsystems private final Drive drive; private final Vision vision; - private final FlywheelIO flywheel; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -82,8 +79,6 @@ public RobotContainer() { drive, new VisionIOLimelight("Front Camera", drive::getRotation), new VisionIOLimelight("Back Camera", drive::getRotation)); - - flywheel = new FlywheelIOTalonFX(); break; case SIM: @@ -118,16 +113,6 @@ public RobotContainer() { break; - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(TunerConstants.FrontLeft), - new ModuleIOSim(TunerConstants.FrontRight), - new ModuleIOSim(TunerConstants.BackLeft), - new ModuleIOSim(TunerConstants.BackRight)); - - flywheel = new FlywheelIOTalonFX(); - break; default: // Replayed robot, disable IO implementations @@ -140,9 +125,6 @@ public RobotContainer() { new ModuleIO() {}); vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); - - flywheel = new FlywheelIO() {}; - break; } // Set up auto routines @@ -212,7 +194,6 @@ private void configureButtonBindings() { Rotation2d.kZero)), drive) .ignoringDisable(true)); - controller.leftTrigger().whileTrue(Commands.run(() -> flywheel.setRoller(1))); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 61188c7..bfb41b6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -376,7 +376,11 @@ public static Translation2d[] getModuleTranslations() { } @Override - public void accept(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + public void accept( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } } From 3cdaa1381f746e52a18f4cd28f91b05d382c3403 Mon Sep 17 00:00:00 2001 From: akasbyju0 <94997492+akasbyju0@users.noreply.github.com> Date: Fri, 23 Jan 2026 19:47:31 -0500 Subject: [PATCH 4/6] took off mega tag 1 and decided on mega tag 2 --- .../subsystems/vision/VisionIOLimelight.java | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 7dfa152..e2852bd 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -22,7 +22,6 @@ public class VisionIOLimelight implements VisionIO { private final DoubleSubscriber latencySubscriber; private final DoubleSubscriber txSubscriber; private final DoubleSubscriber tySubscriber; - private final DoubleArraySubscriber megatag1Subscriber; private final DoubleArraySubscriber megatag2Subscriber; public VisionIOLimelight(String name, Supplier rotationSupplier) { @@ -32,8 +31,6 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); - megatag1Subscriber = - table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); megatag2Subscriber = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); } @@ -55,20 +52,7 @@ public void updateInputs(VisionIOInputs inputs) { Set tagIds = new HashSet<>(); List poseObservations = new LinkedList<>(); - for (var rawSample : megatag1Subscriber.readQueue()) { - if (rawSample.value.length == 0) continue; - for (int i = 11; i < rawSample.value.length; i += 7) { - tagIds.add((int) rawSample.value[i]); - } - poseObservations.add( - new PoseObservation( - rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, - parsePose(rawSample.value), - rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, - (int) rawSample.value[7], - rawSample.value[9], - PoseObservationType.MEGATAG_1)); - } + for (var rawSample : megatag2Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; for (int i = 11; i < rawSample.value.length; i += 7) { From 6bbda57778ec69b8a6cc9e0ab695be977c756a0b Mon Sep 17 00:00:00 2001 From: akasbyju0 <94997492+akasbyju0@users.noreply.github.com> Date: Fri, 30 Jan 2026 19:12:41 -0500 Subject: [PATCH 5/6] made changes to simulate robot and camera pose and logged transforms --- src/main/java/frc/robot/Robot.java | 6 ++++- src/main/java/frc/robot/RobotContainer.java | 23 +++++++++++++++++++ .../subsystems/vision/VisionConstants.java | 15 ++++++++---- 3 files changed, 39 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a670d71..d13e0b2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.sim.PhysicsSim; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -135,5 +136,8 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + PhysicsSim.getInstance().run(); + robotContainer.updateVisionSim(); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1ac7323..14499e5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import com.pathplanner.lib.auto.AutoBuilder; 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.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; @@ -26,6 +27,8 @@ import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.flywheel.FlywheelIO; import frc.robot.subsystems.flywheel.FlywheelIOTalonFX; +import frc.robot.subsystems.vision.*; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -38,6 +41,7 @@ public class RobotContainer { // Subsystems private final Drive drive; private final FlywheelIO flywheel; + private final Vision vision; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -45,6 +49,13 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; + public void updateVisionSim() { + Pose3d frontCameraPose = + new Pose3d(drive.getPose()).transformBy(VisionConstants.frontCamTrans); + + Logger.recordOutput("Front Cam Transform", frontCameraPose); + } + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { switch (Constants.currentMode) { @@ -74,6 +85,9 @@ public RobotContainer() { // new ModuleIOTalonFXS(TunerConstants.FrontRight), // new ModuleIOTalonFXS(TunerConstants.BackLeft), // new ModuleIOTalonFXS(TunerConstants.BackRight)); + vision = + new Vision( + drive, new VisionIOLimelight("Front Camera", drive::getRotation)); flywheel = new FlywheelIOTalonFX(); break; @@ -89,6 +103,14 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.BackRight)); flywheel = new FlywheelIOTalonFX(); + vision = + new Vision( + drive, + new VisionIOPhotonVisionSim( + "Front Camera", + VisionConstants.frontCamTrans, + drive::getPose)); + break; default: @@ -101,6 +123,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); + vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); flywheel = new FlywheelIO() {}; break; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 6d2e880..c3bea71 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -4,18 +4,17 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; public class VisionConstants { public static AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; + public static String frontCam = "camera_0"; public static Transform3d robotToCamera0 = new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); public static double maxAmbiguity = 0.3; public static double maxZError = 0.75; @@ -27,4 +26,12 @@ public class VisionConstants { public static double linearStdDevMegatag2Factor = 0.5; public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; + + public static Transform3d frontCamTrans = + new Transform3d( + new Translation3d( + Units.inchesToMeters(13), + Units.inchesToMeters(0), + Units.inchesToMeters(19)), + new Rotation3d(0, Math.toRadians(-15), Math.toRadians(180))); } From 5b0ee3f2bb30ef06fc265f5a034ee30fc4e2b603 Mon Sep 17 00:00:00 2001 From: akasbyju0 <94997492+akasbyju0@users.noreply.github.com> Date: Sat, 31 Jan 2026 14:39:14 -0500 Subject: [PATCH 6/6] Made sone minor fixes to vision constants --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/vision/VisionConstants.java | 12 ++++-------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 14499e5..44bdcaa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,7 +123,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); + vision = new Vision(drive, new VisionIO() {}); flywheel = new FlywheelIO() {}; break; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index c3bea71..a311af2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -9,12 +9,8 @@ public class VisionConstants { public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); - public static String frontCam = "camera_0"; - - public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); public static double maxAmbiguity = 0.3; public static double maxZError = 0.75; @@ -30,8 +26,8 @@ public class VisionConstants { public static Transform3d frontCamTrans = new Transform3d( new Translation3d( - Units.inchesToMeters(13), + Units.inchesToMeters(17), Units.inchesToMeters(0), - Units.inchesToMeters(19)), - new Rotation3d(0, Math.toRadians(-15), Math.toRadians(180))); + Units.inchesToMeters(21)), + new Rotation3d(0, Math.toRadians(180), Math.toRadians(180))); }