work on limelight

src/main/java/frc/robot/localization/LocalizationSubsystem.java

@@ -12,6 +12,8 @@
 import frc.robot.vision.VisionResult;
 import frc.robot.vision.VisionSubsystem;
 import java.util.Optional;
+import java.util.List;
+import java.util.ArrayList;
 
 public class LocalizationSubsystem extends StateMachine<LocalizationState> {
   private final ImuSubsystem imu;
@@ -20,7 +22,7 @@ public class LocalizationSubsystem extends StateMachine<LocalizationState> {
   private final TimeInterpolatableBuffer<Pose2d> poseHistory =
       TimeInterpolatableBuffer.createBuffer(1.5);
   private double lastAddedVisionTimestamp = 0;
-  private Optional<VisionResult> latestResult = Optional.empty();
+  private List<VisionResult> latestResult = new ArrayList<>();
 
   public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision) {
     super(SubsystemPriority.LOCALIZATION, LocalizationState.DEFAULT_STATE);
@@ -41,9 +43,8 @@ public Pose2d getPose() {
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();
-
-    if (latestResult.isPresent()) {
-      var results = latestResult.get();
+for (int resultnum : latestResult.size())
+       results = latestResult.get(resultnum);
       Pose2d visionPose = results.pose();
 
       double visionTimestamp = results.timestamp();
@@ -59,7 +60,7 @@ public void robotPeriodic() {
                 RobotConfig.get().vision().xyStdDev(),
                 RobotConfig.get().vision().thetaStdDev()));
         lastAddedVisionTimestamp = visionTimestamp;
-      }
+
     }
 
     poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());

src/main/java/frc/robot/vision/Limelight.java

@@ -0,0 +1,30 @@
+package frc.robot.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import java.util.Optional;
+
+public class Limelight{
+  private Optional<VisionResult> rawVisionResult;
+  private Optional<VisionResult> processedVisionResult;
+public String limeLightName = "";
+
+
+  public Optional<VisionResult> getRawVisionResult() {
+    var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limeLightName);
+
+    if (estimatePose == null) {
+      return Optional.empty();
+    }
+
+    if (estimatePose.tagCount == 0) {
+      return Optional.empty();
+    }
+
+    // This prevents pose estimator from having crazy poses if the Limelight loses power
+    if (estimatePose.pose.getX() == 0.0 && estimatePose.pose.getY() == 0.0) {
+      return Optional.empty();
+    }
+
+    return Optional.of(new VisionResult(estimatePose.pose, estimatePose.timestampSeconds));
+  }
+}

src/main/java/frc/robot/vision/VisionSubsystem.java

@@ -3,51 +3,39 @@
 import frc.robot.imu.ImuSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
-import java.util.Optional;
+import java.util.List;
+import java.util.ArrayList;
 
 public class VisionSubsystem extends StateMachine<VisionState> {
   private final ImuSubsystem imu;
-  private Optional<VisionResult> rawVisionResult;
-  private Optional<VisionResult> processedVisionResult;
+  private final Limelight leftLimelight;
+  private final Limelight rightLimelight;
+  private final List<VisionResult> processedVisionResult = new ArrayList<>();
 
-  public VisionSubsystem(ImuSubsystem imu) {
+
+  public VisionSubsystem(ImuSubsystem imu, Limelight leftLimelight, Limelight rightLimelight) {
     super(SubsystemPriority.VISION, VisionState.DEFAULT_STATE);
     this.imu = imu;
+    this.leftLimelight = leftLimelight;
+    this.rightLimelight = rightLimelight;
   }
 
-  private Optional<VisionResult> getRawVisionResult() {
-    var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("");
-
-    if (estimatePose == null) {
-      return Optional.empty();
-    }
+  @Override
+  protected void collectInputs() {
+    var leftResult = leftLimelight.getRawVisionResult();
+    var rightResult = rightLimelight.getRawVisionResult();
+    processedVisionResult.clear();
 
-    if (estimatePose.tagCount == 0) {
-      return Optional.empty();
+    if (leftResult.isPresent()) {
+      processedVisionResult.add(leftResult.get());
     }
-
-    // This prevents pose estimator from having crazy poses if the Limelight loses power
-    if (estimatePose.pose.getX() == 0.0 && estimatePose.pose.getY() == 0.0) {
-      return Optional.empty();
+    if(rightResult.isPresent()) {
+      processedVisionResult.add(rightResult.get());
     }
 
-    return Optional.of(new VisionResult(estimatePose.pose, estimatePose.timestampSeconds));
-  }
-
-  @Override
-  protected void collectInputs() {
-    rawVisionResult = getRawVisionResult();
-    if (rawVisionResult.isEmpty()) {
-      processedVisionResult = Optional.empty();
-    } else {
-      var rawData = rawVisionResult.get();
-      processedVisionResult =
-          Optional.of(
-              new VisionResult(VisionUtil.interpolatePose(rawData.pose()), rawData.timestamp()));
-    }
   }
 
-  public Optional<VisionResult> getVisionResult() {
+  public List<VisionResult> getVisionResult() {
     return processedVisionResult;
   }