Add dashboard variable to track camera tag detection percentage

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

@@ -47,6 +47,7 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
+    robotContainer.driveSubsystem.resetVisionStats();
     CommandScheduler.getInstance().cancelAll();
     CommandTimeline.cancelAll();
     FieldStatePublisher.setupElasticNotifications();
@@ -57,10 +58,13 @@ public void autonomousInit() {
   public void autonomousPeriodic() {}
 
   @Override
-  public void autonomousExit() {}
+  public void autonomousExit() {
+    robotContainer.driveSubsystem.logVisionStats("auto");
+  }
 
   @Override
   public void teleopInit() {
+    robotContainer.driveSubsystem.resetVisionStats();
     CommandScheduler.getInstance().cancelAll();
     robotContainer.scheduleTeleOp();
   }
@@ -69,7 +73,9 @@ public void teleopInit() {
   public void teleopPeriodic() {}
 
   @Override
-  public void teleopExit() {}
+  public void teleopExit() {
+    robotContainer.driveSubsystem.logVisionStats("teleop");
+  }
 
   @Override
   public void testInit() {

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -193,6 +193,41 @@ public void removeObstaclesSupplier(Supplier<List<Obstacle>> supplier) {
         obstaclesSuppliers.remove(supplier);
     }
 
+    /**
+     * Resets vision tag-detection statistics for all cameras.
+     * Should be called at the start of each enabled period (teleop/auto) so the
+     * reported percentages only cover the current run.
+     */
+    public void resetVisionStats() {
+        if (useVisionOdometry && visionOdometry != null) {
+            visionOdometry.resetAllCameraStats();
+        }
+    }
+
+    /**
+     * Appends each camera's tag-detection percentage to the on-robot log file.
+     * Should be called at the end of each enabled period (teleop/auto).
+     *
+     * @param runLabel Short label describing the mode that just ended (e.g. "teleop").
+     */
+    public void logVisionStats(String runLabel) {
+        if (useVisionOdometry && visionOdometry != null) {
+            visionOdometry.logVisionStats(runLabel);
+        }
+    }
+
+    /**
+     * Returns the percentage (0–100) of loop cycles in which at least one camera
+     * had an AprilTag visible, since the last {@link #resetVisionStats()} call.
+     * Returns 0 if vision odometry is not in use.
+     */
+    public double getAnyCameraTagDetectionPercentage() {
+        if (useVisionOdometry && visionOdometry != null) {
+            return visionOdometry.getAnyCameraTagDetectionPercentage();
+        }
+        return 0.0;
+    }
+
     /**
      * Sets a supplier that returns a speed scale factor applied to all drive output.
      * A value of 1.0 is full speed; 0.5 limits the robot to 50% of normal speed.

src/main/java/frc/robot/subsystems/vision/PhotonVisionOdometry.java

@@ -12,6 +12,11 @@
 import frc.robot.util.vision.Cameras;
 
 import java.awt.Desktop;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.StandardOpenOption;
+import java.time.OffsetDateTime;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
@@ -46,6 +51,14 @@ public class PhotonVisionOdometry {
    * Photon Vision Simulation
    */
   public VisionSystemSim visionSim;
+  /**
+   * Total loop cycles processed since the last reset (any-camera aggregate).
+   */
+  private long anyCamTotalCycles = 0;
+  /**
+   * Loop cycles in which at least one camera had a tag since the last reset.
+   */
+  private long anyCamCyclesWithTag = 0;
   /**
    * Count of times that the odom thinks we're more than 10meters away from the
    * april tag.
@@ -122,6 +135,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) {
        */
       visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
     }
+    boolean anyCameraHasTag = false;
     for (Cameras camera : Cameras.values()) {
       Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
       if (poseEst.isPresent()) {
@@ -135,8 +149,28 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) {
         SmartDashboard.putNumber("VisionX", pose.estimatedPose.getX());
         SmartDashboard.putNumber("VisionY", pose.estimatedPose.getY());
       }
+
+      // Publish per-camera tag detection percentage (0–100).
+      SmartDashboard.putNumber("Vision/TagDetectionPct/" + camera.name(),
+          camera.getTagDetectionPercentage());
+
+      // Check whether this camera saw a tag in its latest result.
+      if (!camera.resultsList.isEmpty() && camera.resultsList.get(camera.resultsList.size() - 1).hasTargets()) {
+        anyCameraHasTag = true;
+      }
     }
 
+    // Aggregate: track cycles where at least one camera had a tag.
+    anyCamTotalCycles++;
+    if (anyCameraHasTag) {
+      anyCamCyclesWithTag++;
+    }
+    double anyCamPct = anyCamTotalCycles > 0
+        ? (anyCamCyclesWithTag * 100.0 / anyCamTotalCycles)
+        : 0.0;
+    SmartDashboard.putNumber("Vision/TagDetectionPct/ANY_CAMERA", anyCamPct);
+    SmartDashboard.putBoolean("Vision/AnyCameraHasTag", anyCameraHasTag);
+
   }
 
   /**
@@ -165,6 +199,59 @@ public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera) {
     return poseEst;
   }
 
+  /**
+   * Resets tag-detection statistics for all cameras and the any-camera aggregate.
+   * Should be called whenever the robot is enabled (teleop or autonomous) so
+   * percentages reflect only the current run.
+   */
+  public void resetAllCameraStats() {
+    for (Cameras camera : Cameras.values()) {
+      camera.resetStats();
+    }
+    anyCamTotalCycles = 0;
+    anyCamCyclesWithTag = 0;
+  }
+
+  /**
+   * @return Percentage (0–100) of loop cycles in which at least one camera had an
+   *         AprilTag visible, since the last {@link #resetAllCameraStats()} call.
+   */
+  public double getAnyCameraTagDetectionPercentage() {
+    return anyCamTotalCycles > 0 ? (anyCamCyclesWithTag * 100.0 / anyCamTotalCycles) : 0.0;
+  }
+
+  /**
+   * Appends a summary of each camera's tag-detection percentage to
+   * {@code vision-stats.log} in the WPILib operating directory returned by
+   * {@code edu.wpi.first.wpilibj.Filesystem.getOperatingDirectory()}. Each entry
+   * is timestamped and labelled with the run type (e.g. "teleop" or "auto").
+   *
+   * @param runLabel Short label describing the mode that just ended.
+   */
+  public void logVisionStats(String runLabel) {
+    StringBuilder sb = new StringBuilder();
+    sb.append(OffsetDateTime.now()).append(" [").append(runLabel).append("]\n");
+    for (Cameras camera : Cameras.values()) {
+      sb.append(String.format("  %-15s %5.1f%%  (%d / %d frames)%n",
+          camera.name(),
+          camera.getTagDetectionPercentage(),
+          camera.getFramesWithTags(),
+          camera.getTotalFrames()));
+    }
+    sb.append("\n");
+
+    Path logFile = edu.wpi.first.wpilibj.Filesystem.getOperatingDirectory()
+        .toPath().resolve("vision-stats.log");
+    try {
+      Files.writeString(logFile, sb.toString(),
+          StandardOpenOption.CREATE, StandardOpenOption.APPEND);
+    } catch (IOException e) {
+      edu.wpi.first.wpilibj.DriverStation.reportError(
+          "[VisionStats] Failed to write log to " + logFile + ": " + e.getMessage(),
+          e.getStackTrace());
+    }
+  }
+
   /**
    * Get distance of the robot from the AprilTag pose.
    *

src/main/java/frc/robot/util/vision/Cameras.java

@@ -4,11 +4,6 @@
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
-import static edu.wpi.first.units.Units.Microsecond;
-import static edu.wpi.first.units.Units.Microseconds;
-import static edu.wpi.first.units.Units.Milliseconds;
-import static edu.wpi.first.units.Units.Seconds;
-
 import java.util.ArrayList;
 import java.util.Optional;
 
@@ -28,7 +23,6 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.networktables.NetworkTablesJNI;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import frc.robot.Constants.VisionOdometryConstants;
@@ -96,6 +90,45 @@ public enum Cameras {
      */
     public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
 
+    /**
+     * Total number of camera frames received since the last reset.
+     */
+    private long totalFrames = 0;
+    /**
+     * Number of camera frames in which at least one AprilTag was detected since the last reset.
+     */
+    private long framesWithTags = 0;
+    /**
+     * Percentage (0–100) of camera frames in which at least one AprilTag was detected.
+     * Updated each time new frames are processed. Reset to 0 when {@link #resetStats()} is called.
+     */
+    private double tagDetectionPercentage = 0.0;
+
+    /**
+     * Resets tag-detection statistics. Call this when the robot is enabled so
+     * percentages reflect only the current run.
+     */
+    public void resetStats() {
+        totalFrames = 0;
+        framesWithTags = 0;
+        tagDetectionPercentage = 0.0;
+    }
+
+    /** @return Percentage (0–100) of frames in which at least one AprilTag was detected. */
+    public double getTagDetectionPercentage() {
+        return tagDetectionPercentage;
+    }
+
+    /** @return Total camera frames received since the last reset. */
+    public long getTotalFrames() {
+        return totalFrames;
+    }
+
+    /** @return Camera frames in which at least one AprilTag was detected since the last reset. */
+    public long getFramesWithTags() {
+        return framesWithTags;
+    }
+
     /**
      * Simulated camera instance which only exists during simulations.
      */
@@ -105,10 +138,6 @@ public enum Cameras {
      * queries.
      */
     public List<PhotonPipelineResult> resultsList = new ArrayList<>();
-    /**
-     * Last read from the camera timestamp to prevent lag due to slow data fetches.
-     */
-    private double lastReadTimestamp = Microsecond.of(NetworkTablesJNI.now()).in(Seconds);
 
     /**
      * Construct a Photon Camera class with help. Standard deviations are fake
@@ -221,22 +250,25 @@ public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
     }
 
     /**
-     * Update the latest results, cached with a maximum refresh rate of 1req/15ms.
-     * Sorts the list by timestamp.
+     * Update the latest unread results from the camera. Sorts the list by timestamp.
      */
     private void updateUnreadResults() {
-        double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds();
-        double currentTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
-        double debounceTime = Milliseconds.of(15).in(Seconds);
-        for (PhotonPipelineResult result : resultsList) {
-            mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
-        }
-
         resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
-        lastReadTimestamp = currentTimestamp;
         resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
             return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
         });
+
+        // Track tag detection at the per-frame level.
+        for (PhotonPipelineResult frame : resultsList) {
+            totalFrames++;
+            if (frame.hasTargets()) {
+                framesWithTags++;
+            }
+        }
+        tagDetectionPercentage = (totalFrames > 0)
+                ? (framesWithTags * 100.0 / totalFrames)
+                : 0.0;
+
         if (!resultsList.isEmpty()) {
             updateEstimatedGlobalPose();
         }