Log some data from camera for debugging.

CDS-Cyclones · Jan 31, 2025 · 7d0d2c1 · 7d0d2c1
1 parent ef3ecd2
commit 7d0d2c1
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -7,6 +7,7 @@
 import java.io.File;
 import java.util.function.DoubleSupplier;
 import java.util.Optional;
+import java.util.List;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.PIDConstants;
@@ -24,11 +25,13 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import frc.robot.Constants;
 import frc.robot.subsystems.Vision.Cameras;
+import org.photonvision.targeting.PhotonTrackedTarget;
 import swervelib.SwerveController;
 import swervelib.SwerveDrive;
 import swervelib.math.SwerveMath;
@@ -147,6 +150,18 @@ public void periodic() {
       swerveDrive.updateOdometry();
       vision.updatePoseEstimation(swerveDrive);
     }
+
+    List<PhotonPipelineResult> pipelineResults = Cameras.CAM1.camera.getAllUnreadResults();
+    if(!pipelineResults.isEmpty()) {
+      PhotonPipelineResult LatestResult = pipelineResults.get(pipelineResults.size() - 1);
+      PhotonTrackedTarget LatestBestTarget = LatestResult.getBestTarget();
+      SmartDashboard.putNumber("April Tag ID", LatestBestTarget.getFiducialId());
+      SmartDashboard.putNumber("Area of Tag", LatestBestTarget.getArea());
+      SmartDashboard.putNumber("Photon Yaw", LatestBestTarget.getYaw());
+      SmartDashboard.putNumber("Photon Pitch", LatestBestTarget.getPitch());
+    }else{
+      SmartDashboard.putNumber("April Tag ID", -1);
+    }
   }
 
 

diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
@@ -192,15 +192,15 @@ public enum Cameras
         /**
          * Latency alert to use when high latency is detected.
          */
-        public final  Alert latencyAlert;
+        public final Alert latencyAlert;
         /**
          * Camera instance for comms.
          */
-        public final  PhotonCamera camera;
+        public final PhotonCamera camera;
         /**
          * Pose estimator for camera.
          */
-        public final  PhotonPoseEstimator poseEstimator;
+        public final PhotonPoseEstimator poseEstimator;
         /**
          * Standard Deviation for single tag readings for pose estimation.
          */
@@ -221,10 +221,6 @@ public enum Cameras
          * Estimated robot pose.
          */
         public Optional<EstimatedRobotPose> estimatedRobotPose;
-        /**
-         * Simulated camera instance which only exists during simulations.
-         */
-        public PhotonCameraSim cameraSim;
         /**
          * Results list to be updated periodically and cached to avoid unnecessary queries.
          */
@@ -315,7 +311,7 @@ private void updateUnreadResults() {
                 mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
             }
             if ((resultsList.isEmpty() || (currentTimestamp - mostRecentTimestamp >= debounceTime)) && (currentTimestamp - lastReadTimestamp) >= debounceTime) {
-                resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
+                resultsList = camera.getAllUnreadResults();
                 lastReadTimestamp = currentTimestamp;
                 resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
                     return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;