refactor(odometry): Improve Limelight.

Author: haydenheroux

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

@@ -0,0 +1,33 @@
+package frc.robot.odometry;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.lib.LimelightHelpers;
+import frc.lib.LimelightHelpers.PoseEstimate;
+
+public class Limelight {
+
+    private final String name; 
+    
+    public Limelight(String name) {
+        this.name = name;
+    }
+
+    public void setTagFilter(int[] tags) {
+        LimelightHelpers.SetFiducialIDFiltersOverride(this.name, tags);
+    }
+
+    public void setYaw(Rotation2d yaw) {
+        LimelightHelpers.SetRobotOrientation(this.name, yaw.getDegrees(), 0, 0, 0, 0, 0);
+    }
+
+    public Optional<PoseEstimate> getPoseEstimate() {
+        LimelightHelpers.PoseEstimate megaTag2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(this.name);
+
+        if (megaTag2.tagCount < 1) return Optional.empty();
+
+        return Optional.of(megaTag2);
+    }
+
+}

src/main/java/frc/robot/odometry/Odometry.java

@@ -14,7 +14,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.lib.AllianceFlipHelper;
-import frc.lib.LimelightHelpers;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
 import frc.robot.odometry.GyroscopeIO.GyroscopeIOValues;
@@ -51,6 +50,8 @@ public class Odometry extends Subsystem {
   /** List of functions to be called when pose is manually updated. */
   private final List<Consumer<Rotation2d>> yawUpdateConsumers;
 
+  private final Limelight limelight;
+
   /** Creates a new instance of the odometry subsystem. */
   private Odometry() {
     gyroscope = OdometryFactory.createGyroscope(this);
@@ -72,12 +73,15 @@ private Odometry() {
             initialGyroRotation,
             swerveModulePositionsSupplier.get(),
             initialPose);
+    swervePoseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7,.7,9999999));
 
     field = new Field2d();
 
     yawUpdateConsumers = new ArrayList<Consumer<Rotation2d>>();
 
-    LimelightHelpers.SetFiducialIDFiltersOverride(OdometryConstants.LIMELIGHT_NAME, OdometryConstants.VALID_TAGS);
+    limelight = new Limelight("limelight");
+
+    limelight.setTagFilter(OdometryConstants.VALID_TAGS);
   }
 
   /**
@@ -97,19 +101,18 @@ public static Odometry getInstance() {
   public void periodic() {
     gyroscope.update(gyroscopeValues);
 
-    LimelightHelpers.SetRobotOrientation(OdometryConstants.LIMELIGHT_NAME, getFieldRelativeHeading().getDegrees(), 0, 0, 0, 0, 0);
-
-    LimelightHelpers.PoseEstimate megaTag2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(OdometryConstants.LIMELIGHT_NAME);
+    limelight.setYaw(getFieldRelativeHeading());
 
-    final boolean spinningFast = Math.abs(gyroscopeValues.yawVelocityRotations) > 1.0;
+    final boolean stationary = Math.abs(gyroscopeValues.yawVelocityRotations) > 1.0;
 
-    final boolean hasTag = megaTag2.tagCount != 0;
+    if (stationary) {
+      var pe = limelight.getPoseEstimate();
 
-    if(hasTag && !spinningFast) {
-      swervePoseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7,.7,9999999));
-      swervePoseEstimator.addVisionMeasurement(
-          megaTag2.pose,
-          megaTag2.timestampSeconds);
+      if (pe.isPresent()) {
+        swervePoseEstimator.addVisionMeasurement(
+            pe.get().pose,
+            pe.get().timestampSeconds);
+      }
     }
 
     swervePoseEstimator.update(