Added debugging logging

Author: PotatoBoyH4

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

@@ -2,6 +2,7 @@
 
 import choreo.trajectory.SwerveSample;
 import choreo.trajectory.Trajectory;
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.controller.HolonomicDriveController;
 import edu.wpi.first.math.controller.PIDController;
@@ -168,10 +169,21 @@ public Trajectory<SwerveSample> getAutoTrajectory() {
   }
 
   public void addVisionMeasurement(Pose2d pose, double timestamp) {
+    org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Time Since Vision Update", MathSharedStore.getTimestamp() - timestamp);
     io.addVisionMeasurement(pose, timestamp);
   }
 
   public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix<N3, N1> stdDevvs) {
+    org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose);
+    org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/stdDevvs", stdDevvs);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Time Since Vision Update", MathSharedStore.getTimestamp() - timestamp);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Vision Timestamp", timestamp);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Vision MathShared Time", MathSharedStore.getTimestamp());
     io.addVisionMeasurement(pose, timestamp, stdDevvs);
   }
 
@@ -334,6 +346,8 @@ public PIDController getomegaControllerNonProfiled() {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs(getName(), inputs);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Swerve Pose", inputs.swervePose);
     if (Math.abs(inputs.gyroRotation2d.minus(inputs.navxRotation2d).getDegrees())
         > DriveConstants.kGyroDifferentThreshold) {
       gyroDifferentCount++;

src/main/java/frc/robot/subsystems/drive/Swerve.java

@@ -113,6 +113,8 @@ public Swerve() {
             getSwerveModulePositions());
 
     swerveDrive.setOdometry(odometryStrategy);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "Swerve/OdometryStratgey Pose", odometryStrategy.getPoseMeters());
   }
 
   // Getters/Setter
@@ -249,6 +251,7 @@ public void updateInputs(SwerveIOInputs inputs) {
 
     inputs.odometryX = swerveDrive.getPoseMeters().getX();
     inputs.odometryY = swerveDrive.getPoseMeters().getY();
+    inputs.swervePose = odometryStrategy.getPoseMeters();
     inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees();
     inputs.gyroRotation2d = swerveDrive.getHeading();
     inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset);

src/main/java/frc/robot/subsystems/drive/SwerveIO.java

@@ -22,6 +22,7 @@ public interface SwerveIO {
   public static class SwerveIOInputs {
     public double odometryX = 0.0;
     public double odometryY = 0.0;
+    public Pose2d swervePose;
     public double odometryRotation2D = 0.0;
     public Rotation2d gyroRotation2d = new Rotation2d();
     public Rotation2d navxRotation2d = new Rotation2d();

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

@@ -7,6 +7,7 @@
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -308,12 +309,13 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn
     double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ());
     double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ());
     // This filters out results by seeing if they are close to the right height
-    if (dist1 < dist2 && dist1 < 0.5) {
+    // If you use height ot gyro rotation should be determined experimentally
+    /*if (dist1 < dist2 && dist1 < 0.5) {
       return pose1;
     }
     if (dist2 < dist1 && dist2 < 0.5) {
       return pose2;
-    }
+    }*/
     // If we don't have enough data in the gyro buffer we default to returning a pose
     if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) return pose1;
 
@@ -369,7 +371,7 @@ public void periodic() {
         WallEyePoseResult result = (WallEyePoseResult) res.getFirst();
         int idx = res.getSecond();
 
-        logger.recordOutput("Vision", result.getTimeStamp());
+        logger.recordOutput("Vision/resultTime", result.getTimeStamp());
         for (int i = 0; i < 2; i++) {
           stdMatrix.set(
               i,
@@ -421,8 +423,11 @@ public void periodic() {
           logger.recordOutput("Vision/Accepted Cam " + camNames[idx], robotPose);
           // However we do have to be accepting the poses to use them
           if (visionUpdating) {
-            driveSubsystem.addVisionMeasurement(robotPose, result.getTimeStamp(), stdMatrix);
+            Matrix<N3, N1> testingMatrix = VecBuilder.fill(0.0001, 0.0001, 0.0001);
+            driveSubsystem.addVisionMeasurement(
+                robotPose, result.getTimeStamp() / 1_000_000, testingMatrix);
             logger.recordOutput("Vision/Vision Updating", visionUpdating);
+            logger.recordOutput("Vision/std", stdMatrix.get(0, 0));
           }
         } else {
           logger.recordOutput("Vision/Rejected Cam " + camNames[idx], robotPose);