Interpolated vision

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);

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<VisionResult> rawVisionResult;
-  private Optional<VisionResult> 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<VisionResult> getInterpolatedVisionResult() {
@@ -25,7 +27,7 @@ public Optional<VisionResult> 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()));
   }
 

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<VisionInterpolationData> red, List<VisionInterpolationData> blue) {}

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()),

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<VisionInterpolationData> redSet;
-  public final List<VisionInterpolationData> blueSet;
+  public final CameraDataset leftSet;
+  public final CameraDataset rightSet;
 
-  InterpolatedVisionDataset(List<VisionInterpolationData> red, List<VisionInterpolationData> blue) {
-    this.redSet = red;
-    this.blueSet = blue;
+  InterpolatedVisionDataset(CameraDataset left, CameraDataset right) {
+    this.leftSet = left;
+    this.rightSet = right;
   }
 }