Clean up MegaTag2 code a bit

Author: codeNinja-1

src/main/java/frc/robot/constants/Constants.java

@@ -28,6 +28,7 @@
 import frc.robot.util.CachedRobotState;
 import java.util.Map;
 import java.util.NavigableMap;
+import java.util.Set;
 import java.util.TreeMap;
 import java.util.function.Supplier;
 
@@ -104,6 +105,13 @@ public static final class ALGAE {
   // Exposure: 750
   // Sensor Gain: 10
   public static final class LIMELIGHT {
+    public static final Set<String> APRILTAG_CAMERA_IDS = Set.of(
+      "limelight-ftag",
+      "limelight-btag",
+      "limelight-rtag",
+      "limelight-ltag"
+    );
+
     // x is front-to-back
     // y is left-to-right
     // z it top-to-bottom

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

@@ -2,7 +2,8 @@
 
 import static edu.wpi.first.units.Units.Seconds;
 
-import java.util.Set;
+import java.util.HashMap;
+import java.util.Map;
 
 import com.team6962.lib.swerve.SwerveDrive;
 import com.team6962.lib.telemetry.Logger;
@@ -19,10 +20,24 @@
 import io.limelightvision.LimelightHelpers.PoseEstimate;
 import io.limelightvision.LimelightHelpers.RawFiducial;
 
+/**
+ * The MegaTag2 class is responsible for pose estimation using a combination
+ * of MegaTag1 and MegaTag2.
+ */
 public class MegaTag2 extends SubsystemBase {
     private SwerveDrive swerveDrive;
+
+    /**
+     * Pose estimator used to find field-relative headings with MegaTag1.
+     */
     private SwerveDrivePoseEstimator megaTag1PoseEstimator;
 
+    /**
+     * Map from limelight ids to most recent MegaTag2-generated pose estimates,
+     * used for logging.
+     */
+    private Map<String, Pose2d> megaTag2PoseEstimates = new HashMap<>(LIMELIGHT.APRILTAG_CAMERA_IDS.size());
+
     public MegaTag2(SwerveDrive swerveDrive) {
         this.swerveDrive = swerveDrive;
 
@@ -32,6 +47,8 @@ public MegaTag2(SwerveDrive swerveDrive) {
             KinematicsUtils.blankModulePositions(4),
             new Pose2d()
         );
+
+        Logger.addUpdate("poseEstimationData", this::logPoseEstimationData);
     }
 
     @Override
@@ -41,25 +58,33 @@ public void periodic() {
         addVisionEstimates();
     }
 
-    // 1. Get pose estimates from MegaTag1
-    // 2. Filter out bad pose estimates
-    // 3. Add offset to gyroscope angle
+    private void logPoseEstimationData() {
+        for (String cameraId : LIMELIGHT.APRILTAG_CAMERA_IDS) {
+            PoseEstimate estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(cameraId);
 
-    public void seedGyroscopeAngle() {
-        Set<String> cameraIds = LIMELIGHT.APRILTAG_CAMERA_POSES.keySet();
+            Logger.log("PoseEstimation/" + cameraId + "/pose", estimate.pose);
+            Logger.log("PoseEstimation/" + cameraId + "/latency", estimate.latency);
+            Logger.log("PoseEstimation/" + cameraId + "/tagCount", estimate.tagCount);
+            Logger.log("PoseEstimation/" + cameraId + "/timestampSeconds", estimate.timestampSeconds);
 
-        for (String cameraId : cameraIds) {
-            PoseEstimate estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(cameraId);
-            
             for (RawFiducial fiducial : estimate.rawFiducials) {
-                Logger.log("Fiducials/" + cameraId + "/" + fiducial.id + "/ambiguity", fiducial.ambiguity);
-                Logger.log("Fiducials/" + cameraId + "/" + fiducial.id + "/distToCamera", fiducial.distToCamera);
-                Logger.log("Fiducials/" + cameraId + "/" + fiducial.id + "/tagArea", fiducial.ta);
+                Logger.log("PoseEstimation/" + cameraId + "/tags/" + fiducial.id + "/ambiguity", fiducial.ambiguity);
+                Logger.log("PoseEstimation/" + cameraId + "/tags/" + fiducial.id + "/distToCamera", fiducial.distToCamera);
+                Logger.log("PoseEstimation/" + cameraId + "/tags/" + fiducial.id + "/tagArea", fiducial.ta);
             }
 
-            if (!isEstimateGood(estimate)) continue;
+            Logger.log("PoseEstimation/" + cameraId + "/accurate", isEstimateAccurate(estimate));
+        }
+    }
 
-            Logger.getField().getObject("Estimate from " + cameraId).setPose(estimate.pose);
+    /**
+     * Gives MegaTag2 field-relative gyroscope angles detected by MegaTag1.
+     */
+    public void seedGyroscopeAngle() {
+        for (String cameraId : LIMELIGHT.APRILTAG_CAMERA_IDS) {
+            PoseEstimate estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(cameraId);
+            
+            if (!isEstimateAccurate(estimate)) continue;
 
             megaTag1PoseEstimator.addVisionMeasurement(
                 estimate.pose,
@@ -75,9 +100,7 @@ public void seedGyroscopeAngle() {
 
         Rotation2d fieldRelativeHeading = megaTag1PoseEstimator.getEstimatedPosition().getRotation();
 
-        Logger.getField().getObject("Field Relative Heading").setPose(megaTag1PoseEstimator.getEstimatedPosition());
-
-        for (String cameraId : cameraIds) {
+        for (String cameraId : LIMELIGHT.APRILTAG_CAMERA_IDS) {
             LimelightHelpers.SetRobotOrientation(
                 cameraId,
                 fieldRelativeHeading.getDegrees(),
@@ -90,28 +113,31 @@ public void seedGyroscopeAngle() {
         }
     }
 
-    public boolean isEstimateGood(PoseEstimate estimate) {
+    /**
+     * Checks if a pose estimate is roughly accurate.
+     * @param estimate The pose estimate to check the accuracy of.
+     * @return True if it's somewhat accurate.
+     */
+    public boolean isEstimateAccurate(PoseEstimate estimate) {
         for (RawFiducial fiducial : estimate.rawFiducials) {
             if (fiducial.ambiguity < 0.4) return true;
         }
 
         return false;
     }
 
-    // 4. Get pose estimates from MegaTag2
-    // 5. Filter out bad pose estimates
-    // 6. Send MegaTag2 estimates to swerve
-
+    /**
+     * Adds vision measurements from MegaTag2 to the swerve drive pose
+     * estimator.
+     */
     public void addVisionEstimates() {
-        Set<String> cameraIds = LIMELIGHT.APRILTAG_CAMERA_POSES.keySet();
-
-        for (String cameraId : cameraIds) {
+        for (String cameraId : LIMELIGHT.APRILTAG_CAMERA_IDS) {
             PoseEstimate estimate = CachedRobotState.isBlue().orElse(false) ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(cameraId) :
                 LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(cameraId);
 
-            if (!isEstimateGood(estimate)) continue;
+            if (!isEstimateAccurate(estimate)) continue;
 
-            Logger.getField().getObject("MegaTag2 estimate from " + cameraId).setPose(estimate.pose);
+            megaTag2PoseEstimates.put(cameraId, estimate.pose);
 
             swerveDrive.addVisionMeasurement(
                 estimate.pose,