Merge pull request #64 from Manchester-Central/camera-logging-tweaks

more comments and cleanup than logs, really

Author: Pathal

src/main/java/com/chaos131/vision/AbstractChaosCamera.java

@@ -86,7 +86,7 @@ public enum CameraMode {
    * A high level operating mode for the camera system. Individual cameras may or may not implement
    * specific sub-states or sub-modes. For instance, multiple piece tracking modes.
    */
-  protected CameraMode m_mode;
+  protected CameraMode m_mode = CameraMode.LOCALIZATION;
 
   /*******************
    * Initializations *
@@ -102,7 +102,7 @@ public enum CameraMode {
   public AbstractChaosCamera(String name, CameraSpecs specs) {
     m_name = name;
     m_specs = specs;
-    m_useForOdometry = true;
+    setUseForOdometry(true);
     m_disconnectedTimer.start();
 
     m_poseData.reset();
@@ -221,6 +221,7 @@ protected void processUpdateQueue() {
       VisionData data = processMeasuredData(idx);
       if (data != null) {
         m_poseUpdator.accept(data);
+
         Logger.recordOutput(m_name + "/PoseTimestamp", data.getTimestampSeconds());
         Logger.recordOutput(m_name + "/RobotPose", data.getPose2d());
         Logger.recordOutput(m_name + "/RobotPose3d", data.getPose3d());
@@ -355,5 +356,6 @@ public abstract void updateCropFromRobotpose(
    */
   public void setUseForOdometry(boolean val) {
     m_useForOdometry = val;
+    Logger.recordOutput(m_name + "/useForOdometry", m_useForOdometry);
   }
 }

src/main/java/com/chaos131/vision/CameraSpecs.java

@@ -25,7 +25,9 @@ public class CameraSpecs {
   /** Deviation Coefficients */
   public LinearVelocity max_speed_acceptable;
 
+  /** Max speed that the robot can rotate before the data is discarded */
   public AngularVelocity max_rotation_acceptable;
 
+  /** Max average tag distance before the tag is discarded */
   public Distance max_distance_acceptable;
 }

src/main/java/com/chaos131/vision/LimelightCamera.java

@@ -19,7 +19,7 @@
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
-/** Implements a Camera behavior for the This is up to date for Limelight OS 2024.10.2 (10/28/24) */
+/** Implements a Camera behavior for the This is up to date for Limelight OS 2026.0 (Feb 17, 2026) */
 public class LimelightCamera extends AbstractChaosCamera {
   /** Limelight versions can help the implementation navigate features and calibration */
   public enum LimelightVersion {
@@ -28,43 +28,14 @@ public enum LimelightVersion {
     /** Limelight3 */
     LL3,
     /** Limelight3G (the global shutter / greyscale one) */
-    LL3G
+    LL3G,
+    /** Limelight4 with optional starboard Hailo attachment */
+    LL4
   }
 
   /** Version of the limelight instance */
   protected LimelightVersion m_limeLightVersion;
 
-  /**
-   * Represents which mode the robot is in.
-   *
-   * <p>APRIL_TAGS - The pipeline
-   *
-   * <p>PIECE_TRACKING - The pipeline used for finding notes on the field. This is typically for
-   * intake cameras, which may not be the forward camera.
-   */
-  public enum Mode {
-    /** Focus on localization, and processing April Tags */
-    APRIL_TAGS(0),
-    /** Focus on tracking one or many game pieces */
-    PIECE_TRACKING(1),
-    /** Focus on a primary Blue side objective */
-    BLUE_PRIMARY(2),
-    /** Focus on a primary Red side objective */
-    RED_PRIMARY(3),
-    /** Focus on analyzing the environment */
-    MAPPING(4);
-
-    /** The pipeline ID for these features on the Limelight itself */
-    public final Integer pipelineId;
-
-    /**
-     * @param pipelineId ID for the mode
-     */
-    private Mode(Integer pipelineId) {
-      this.pipelineId = pipelineId;
-    }
-  }
-
   /** NetworkTable name that corresponds with this Camera */
   protected NetworkTable m_visionTable;
 
@@ -74,7 +45,10 @@ private Mode(Integer pipelineId) {
   /** NetworkTable entry for MegaTag2 */
   protected NetworkTableEntry m_botposeMT2;
 
-  /** A data structure used by AdvantageKit to record and replay Pose data */
+  /**
+   * A data structure used by AdvantageKit to record and replay Limelight specific MegaTag2 Pose
+   * data. m_poseData already exists for standard megatag1 pose info.
+   */
   protected NetworkPoseDataAutoLogged m_poseDataMT2 = new NetworkPoseDataAutoLogged();
 
   /** Distance in meters before swapping from MT1 to MT2 */
@@ -397,8 +371,9 @@ public VisionData processMeasuredData(int idx) {
   protected void LoadNTQueueToVisionData() {
     /** TODO: Serious race condition concern here! I can't simply fix this, Limelight needs to. */
     NetworkTableValue[] mt1_poses = m_botpose.readQueue();
-    Logger.recordOutput(m_name + "/DataLength", mt1_poses.length);
+    Logger.recordOutput(m_name + "/MT1Length", mt1_poses.length);
     NetworkTableValue[] mt2_poses = m_botposeMT2.readQueue();
+    Logger.recordOutput(m_name + "/MT2Length", mt2_poses.length);
     { // TODO: Analyze actual logs to see if this block is still necessary
       // TODO: Investigate TableEntry issues...
       if (mt1_poses.length == 1 && mt1_poses[0] == null) {
@@ -458,6 +433,10 @@ protected void LoadNTQueueToVisionData() {
       m_poseData.tagCount[idx] = (int) data[idxTagCount];
       m_poseData.timestamps[idx] = timestampSeconds;
     } // End MT1
+    if (mt1_poses.length > 0) {
+      Logger.recordOutput(m_name+"/Mt1TagDistance", mt1_poses[mt1_poses.length-1].getDoubleArray()[idxTagDistance]);
+      Logger.recordOutput(m_name+"/Mt1TagCount", mt1_poses[mt1_poses.length-1].getDoubleArray()[idxTagCount]);
+    }
 
     // Parse MegaTag2 Info
     m_poseDataMT2.resize(mt2_poses.length);
@@ -508,6 +487,11 @@ protected void LoadNTQueueToVisionData() {
       m_poseDataMT2.tagCount[idx] = (int) data[idxTagCount];
       m_poseDataMT2.timestamps[idx] = timestampSeconds;
     } // End MT2
+    if (mt2_poses.length > 0) {
+      Logger.recordOutput(m_name+"/Mt2avgTagDistance", mt2_poses[mt2_poses.length-1].getDoubleArray()[idxTagDistance]);
+      Logger.recordOutput(m_name+"/Mt2TagCount", mt2_poses[mt2_poses.length-1].getDoubleArray()[idxTagCount]);
+    }
+
   }
 
   public static CameraSpecs LL3GSpecs() {