[#297] - added accepted vision logging

frc-862 · Apr 15, 2023 · cee7da8 · cee7da8
1 parent 0b76865
commit cee7da8
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -130,6 +130,7 @@ public class Drivetrain extends SubsystemBase {
     private boolean doVisionUpdate = true;
     private double lastKnownGoodVisionX = 0;
     private double lastKnownGoodVisionY = 0;
+    private double lastKnownGoodVisionRotation = 0;
     private double lastTime = 0;
 
     // Heading compenstaion variables
@@ -392,10 +393,12 @@ public void updateVision() {
 
             lastKnownGoodVisionX = visionPose2d.getX();
             lastKnownGoodVisionY = visionPose2d.getY();
+            lastKnownGoodVisionRotation = visionPose2d.getRotation().getDegrees();
             lastTime = currTime;
 
             LightningShuffleboard.setDouble("Drivetrain", "Accepted vision X", lastKnownGoodVisionX);
             LightningShuffleboard.setDouble("Drivetrain", "Accepted vision Y", lastKnownGoodVisionY);
+            LightningShuffleboard.setDouble("Drivetrain", "Accepted vision Rotation", lastKnownGoodVisionRotation);
         }
     }