diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 328ea44..7cd5957 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,6 +24,8 @@ import frc.robot.util.scheduling.LifecycleSubsystemManager; import frc.robot.vision.Limelight; import frc.robot.vision.VisionSubsystem; +import frc.robot.vision.interpolation.CameraDataset; +import frc.robot.vision.interpolation.InterpolatedVisionDataset; public class Robot extends TimedRobot { private Command autonomousCommand; @@ -39,8 +41,8 @@ public class Robot extends TimedRobot { new IntakeSubsystem(hardware.intakeMain, hardware.intakeCenteringMotor); private final SwerveSubsystem swerve = new SwerveSubsystem(); private final ImuSubsystem imu = new ImuSubsystem(swerve.drivetrainPigeon); - private final Limelight leftLimelight = new Limelight("left"); - private final Limelight rightLimelight = new Limelight("right"); + private final Limelight leftLimelight = new Limelight("left", InterpolatedVisionDataset.HOME.leftSet); + private final Limelight rightLimelight = new Limelight("right", InterpolatedVisionDataset.HOME.rightSet); private final VisionSubsystem vision = new VisionSubsystem(imu, leftLimelight, rightLimelight); private final LocalizationSubsystem localization = new LocalizationSubsystem(imu, vision, swerve); diff --git a/src/main/java/frc/robot/vision/Limelight.java b/src/main/java/frc/robot/vision/Limelight.java index 9825847..13815e9 100644 --- a/src/main/java/frc/robot/vision/Limelight.java +++ b/src/main/java/frc/robot/vision/Limelight.java @@ -1,21 +1,23 @@ package frc.robot.vision; import edu.wpi.first.math.geometry.Pose2d; -import frc.robot.vision.LimelightHelpers.LimelightResults; +import frc.robot.vision.interpolation.CameraDataset; import frc.robot.vision.interpolation.InterpolatedVision; + import java.util.Optional; import dev.doglog.DogLog; public class Limelight { - private Optional rawVisionResult; - private Optional processedVisionResult; private final String limelightTableName; private final String name; + private CameraDataset interpolationData; +; - public Limelight(String name) { + public Limelight(String name, CameraDataset interpolationData) { limelightTableName = "limelight-" + name; this.name = name; + this.interpolationData = interpolationData; } public Optional getInterpolatedVisionResult() { @@ -25,7 +27,7 @@ public Optional getInterpolatedVisionResult() { return Optional.empty(); } - Pose2d interpolatedPose = InterpolatedVision.interpolatePose(rawResult.get().pose()); + Pose2d interpolatedPose = InterpolatedVision.interpolatePose(rawResult.get().pose(), interpolationData); return Optional.of(new VisionResult(interpolatedPose, rawResult.get().timestamp())); } diff --git a/src/main/java/frc/robot/vision/interpolation/CameraDataset.java b/src/main/java/frc/robot/vision/interpolation/CameraDataset.java new file mode 100644 index 0000000..04650d6 --- /dev/null +++ b/src/main/java/frc/robot/vision/interpolation/CameraDataset.java @@ -0,0 +1,7 @@ +package frc.robot.vision.interpolation; + +import java.util.List; + +/** A interpolated vision data set for a specific camera. */ +public record CameraDataset( + List red, List blue) {} diff --git a/src/main/java/frc/robot/vision/interpolation/InterpolatedVision.java b/src/main/java/frc/robot/vision/interpolation/InterpolatedVision.java index 964c94b..5cfa326 100644 --- a/src/main/java/frc/robot/vision/interpolation/InterpolatedVision.java +++ b/src/main/java/frc/robot/vision/interpolation/InterpolatedVision.java @@ -5,15 +5,8 @@ import frc.robot.fms.FmsSubsystem; public class InterpolatedVision { - private static final InterpolatedVisionDataset usedSet = - RobotConfig.get().vision().interpolatedVisionSet(); - - /** - * @param visionInput - pose from the limelight - * @return a transformed pose that can be added to the pose estimator - */ - public static Pose2d interpolatePose(Pose2d visionInput) { - var usedDataPoints = FmsSubsystem.isRedAlliance() ? usedSet.redSet : usedSet.blueSet; + public static Pose2d interpolatePose(Pose2d visionInput, CameraDataset dataset) { + var usedDataPoints = FmsSubsystem.isRedAlliance() ? dataset.red() : dataset.blue(); return new Pose2d( InterpolationUtil.interpolateTranslation(usedDataPoints, visionInput.getTranslation()), diff --git a/src/main/java/frc/robot/vision/interpolation/InterpolatedVisionDataset.java b/src/main/java/frc/robot/vision/interpolation/InterpolatedVisionDataset.java index dca9d94..9b908a4 100644 --- a/src/main/java/frc/robot/vision/interpolation/InterpolatedVisionDataset.java +++ b/src/main/java/frc/robot/vision/interpolation/InterpolatedVisionDataset.java @@ -3,79 +3,85 @@ import edu.wpi.first.math.geometry.Translation2d; import java.util.List; +/** A interpolated vision data set for each field, for all cameras. */ public enum InterpolatedVisionDataset { // ALL POINTS ARE NOT TESTED OR VALIDATED HOME( - List.of( - new VisionInterpolationData( - new Translation2d(15.2245, 5.522), new Translation2d(15.194, 5.634), "SUBWOOFER"), - new VisionInterpolationData( - new Translation2d(13.0745, 5.522), - new Translation2d(13.125, 5.722), - "PODIUM_SPEAKER_INTERSECTION"), - new VisionInterpolationData( - new Translation2d(11.059, 6.842), - new Translation2d(11.16, 6.845), - "WING_LINE_MIDDLE"), - new VisionInterpolationData( - new Translation2d(13.799, 4.202), - new Translation2d(13.905, 4.361), - "FRONT_PODIUM_MIDDLE")), - List.of()), + new CameraDataset( + List.of( + new VisionInterpolationData( + new Translation2d(15.2245, 5.522), new Translation2d(15.194, 5.634), "SUBWOOFER"), + new VisionInterpolationData( + new Translation2d(13.0745, 5.522), + new Translation2d(13.125, 5.722), + "PODIUM_SPEAKER_INTERSECTION"), + new VisionInterpolationData( + new Translation2d(11.059, 6.842), + new Translation2d(11.16, 6.845), + "WING_LINE_MIDDLE"), + new VisionInterpolationData( + new Translation2d(13.799, 4.202), + new Translation2d(13.905, 4.361), + "FRONT_PODIUM_MIDDLE")), + List.of()), + new CameraDataset( + List.of( + new VisionInterpolationData( + new Translation2d(15.2245, 5.522), new Translation2d(15.194, 5.634), "SUBWOOFER"), + new VisionInterpolationData( + new Translation2d(13.0745, 5.522), + new Translation2d(13.125, 5.722), + "PODIUM_SPEAKER_INTERSECTION"), + new VisionInterpolationData( + new Translation2d(11.059, 6.842), + new Translation2d(11.16, 6.845), + "WING_LINE_MIDDLE"), + new VisionInterpolationData( + new Translation2d(13.799, 4.202), + new Translation2d(13.905, 4.361), + "FRONT_PODIUM_MIDDLE")), + List.of())), BELLARMINE( - List.of( - new VisionInterpolationData( - new Translation2d(15.2245, 5.522), new Translation2d(15.125, 5.581), "SUBWOOFER"), - new VisionInterpolationData( - new Translation2d(13.0745, 5.522), - new Translation2d(13.103, 5.560), - "PODIUM_SPEAKER_INTERSECTION"), - new VisionInterpolationData( - new Translation2d(11.059, 6.842), - new Translation2d(11.18, 6.932), - "WING_LINE_MIDDLE"), - new VisionInterpolationData( - new Translation2d(13.799, 4.202), - new Translation2d(13.67, 4.106), - "FRONT_PODIUM_MIDDLE")), - List.of()), - CHEZY_CHAMPS( - List.of( - new VisionInterpolationData( - new Translation2d(15.2245, 5.522), new Translation2d(15.215, 5.553), "RED_SUBWOOFER"), - new VisionInterpolationData( - new Translation2d(13.0745, 5.522), - new Translation2d(13.351, 5.572), - "RED_PODIUM_SPEAKER_INTERSECTION"), - new VisionInterpolationData( - new Translation2d(11.059, 6.842), - new Translation2d(11.42, 6.958), - "RED_WING_LINE_MIDDLE"), - new VisionInterpolationData( - new Translation2d(13.799, 4.202), - new Translation2d(13.929, 4.152), - "RED_FRONT_PODIUM_MIDDLE")), - List.of( - new VisionInterpolationData( - new Translation2d(1.315, 5.522), new Translation2d(1.34, 5.51), "BLUE_SUBWOOFER"), - new VisionInterpolationData( - new Translation2d(3.465, 5.522), - new Translation2d(3.295, 5.35), - "BLUE_PODIUM_SPEAKER_INTERSECTION"), - new VisionInterpolationData( - new Translation2d(5.481, 6.842), - new Translation2d(5.256, 6.751), - "BLUE_WING_LINE_MIDDLE"), - new VisionInterpolationData( - new Translation2d(2.741, 4.202), - new Translation2d(2.630, 3.974), - "BLUE_FRONT_PODIUM_MIDDLE"))); + new CameraDataset( + List.of( + new VisionInterpolationData( + new Translation2d(15.2245, 5.522), new Translation2d(15.125, 5.581), "SUBWOOFER"), + new VisionInterpolationData( + new Translation2d(13.0745, 5.522), + new Translation2d(13.103, 5.560), + "PODIUM_SPEAKER_INTERSECTION"), + new VisionInterpolationData( + new Translation2d(11.059, 6.842), + new Translation2d(11.18, 6.932), + "WING_LINE_MIDDLE"), + new VisionInterpolationData( + new Translation2d(13.799, 4.202), + new Translation2d(13.67, 4.106), + "FRONT_PODIUM_MIDDLE")), + List.of()), + new CameraDataset( + List.of( + new VisionInterpolationData( + new Translation2d(15.2245, 5.522), new Translation2d(15.125, 5.581), "SUBWOOFER"), + new VisionInterpolationData( + new Translation2d(13.0745, 5.522), + new Translation2d(13.103, 5.560), + "PODIUM_SPEAKER_INTERSECTION"), + new VisionInterpolationData( + new Translation2d(11.059, 6.842), + new Translation2d(11.18, 6.932), + "WING_LINE_MIDDLE"), + new VisionInterpolationData( + new Translation2d(13.799, 4.202), + new Translation2d(13.67, 4.106), + "FRONT_PODIUM_MIDDLE")), + List.of())); - public final List redSet; - public final List blueSet; + public final CameraDataset leftSet; + public final CameraDataset rightSet; - InterpolatedVisionDataset(List red, List blue) { - this.redSet = red; - this.blueSet = blue; + InterpolatedVisionDataset(CameraDataset left, CameraDataset right) { + this.leftSet = left; + this.rightSet = right; } }