diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 48fc76f2..951deafa 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -1,14 +1,10 @@ package org.team1540.robot2024; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; 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 java.io.IOException; - /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -84,14 +80,6 @@ public static class Vision { public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0; public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05; - public static final AprilTagFieldLayout SIM_APRILTAG_LAYOUT; - static { - try { - SIM_APRILTAG_LAYOUT = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); - } catch (IOException e) { - throw new RuntimeException(e); - } - } public static final int SIM_RES_WIDTH = 1280; public static final int SIM_RES_HEIGHT = 960; public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(100); diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java index f92910ed..03758d9e 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java @@ -1,5 +1,7 @@ package org.team1540.robot2024.subsystems.vision; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; @@ -11,6 +13,7 @@ import org.photonvision.simulation.VisionSystemSim; import org.photonvision.targeting.PhotonTrackedTarget; +import java.io.IOException; import java.util.List; import java.util.Optional; import java.util.function.Supplier; @@ -31,7 +34,14 @@ public class AprilTagVisionIOSim implements AprilTagVisionIO { public AprilTagVisionIOSim(String name, Pose3d cameraOffsetMeters, Supplier poseSupplier) { this.visionSystemSim = new VisionSystemSim(name); this.poseSupplier = poseSupplier; - visionSystemSim.addAprilTags(SIM_APRILTAG_LAYOUT); + + AprilTagFieldLayout aprilTagLayout; + try { + aprilTagLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + } catch (IOException e) { + throw new RuntimeException(e); + } + visionSystemSim.addAprilTags(aprilTagLayout); SimCameraProperties cameraProps = new SimCameraProperties(); cameraProps.setCalibration(SIM_RES_WIDTH, SIM_RES_HEIGHT, SIM_DIAGONAL_FOV); @@ -44,7 +54,7 @@ public AprilTagVisionIOSim(String name, Pose3d cameraOffsetMeters, Supplier