Evil vision

src/main/java/frc/robot/Robot.java

@@ -132,11 +132,16 @@ public void testPeriodic() {}
   public void testExit() {}
 
   private void configureBindings() {
-    swerve.setDefaultCommand(swerve.run(() -> {
-      if (DriverStation.isTeleop()) {
-        swerve.driveTeleop(hardware.driverController.getLeftX(),hardware.driverController.getLeftY() , hardware.driverController.getRightX());
-      }
-    }));
+    swerve.setDefaultCommand(
+        swerve.run(
+            () -> {
+              if (DriverStation.isTeleop()) {
+                swerve.driveTeleop(
+                    hardware.driverController.getLeftX(),
+                    hardware.driverController.getLeftY(),
+                    hardware.driverController.getRightX());
+              }
+            }));
 
     hardware
         .driverController

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

@@ -11,7 +11,8 @@
 import frc.robot.util.state_machines.StateMachine;
 import frc.robot.vision.VisionResult;
 import frc.robot.vision.VisionSubsystem;
-import java.util.Optional;
+import java.util.ArrayList;
+import java.util.List;
 
 public class LocalizationSubsystem extends StateMachine<LocalizationState> {
   private final ImuSubsystem imu;
@@ -20,7 +21,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 +42,7 @@ public Pose2d getPose() {
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();
-
-    if (latestResult.isPresent()) {
-      var results = latestResult.get();
+    for (var results : latestResult) {
       Pose2d visionPose = results.pose();
 
       double visionTimestamp = results.timestamp();
@@ -60,8 +59,8 @@ public void robotPeriodic() {
                 RobotConfig.get().vision().thetaStdDev()));
         lastAddedVisionTimestamp = visionTimestamp;
       }
-    }
 
-    poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
+      poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
+    }
   }
 }

src/main/java/frc/robot/swerve/SwerveSubsystem.java

@@ -10,7 +10,6 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.config.RobotConfig;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.util.ControllerHelpers;
@@ -120,14 +119,11 @@ public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) {
   public void driveTeleop(double x, double y, double theta) {
     double leftY =
         -1.0
-            * ControllerHelpers.getExponent(
-                ControllerHelpers.getDeadbanded(x, leftYDeadband), 1.5);
+            * ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(x, leftYDeadband), 1.5);
     double leftX =
-        ControllerHelpers.getExponent(
-            ControllerHelpers.getDeadbanded(y, leftXDeadband), 1.5);
+        ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(y, leftXDeadband), 1.5);
     double rightX =
-        ControllerHelpers.getExponent(
-            ControllerHelpers.getDeadbanded(theta, rightXDeadband), 2);
+        ControllerHelpers.getExponent(ControllerHelpers.getDeadbanded(theta, rightXDeadband), 2);
 
     if (RobotConfig.get().swerve().invertRotation()) {
       rightX *= -1.0;

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

@@ -0,0 +1,28 @@
+package frc.robot.vision;
+
+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,37 @@
 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.ArrayList;
+import java.util.List;
 
 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;
-  }
-
-  private Optional<VisionResult> getRawVisionResult() {
-    var estimatePose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("");
-
-    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));
+    this.leftLimelight = leftLimelight;
+    this.rightLimelight = rightLimelight;
   }
 
   @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()));
+    var leftResult = leftLimelight.getRawVisionResult();
+    var rightResult = rightLimelight.getRawVisionResult();
+    processedVisionResult.clear();
+
+    if (leftResult.isPresent()) {
+      processedVisionResult.add(leftResult.get());
+    }
+    if (rightResult.isPresent()) {
+      processedVisionResult.add(rightResult.get());
     }
   }
 
-  public Optional<VisionResult> getVisionResult() {
+  public List<VisionResult> getVisionResult() {
     return processedVisionResult;
   }