debug stuff

--Gabe
frc937 · Oct 27, 2023 · 292a2db · 292a2db
1 parent 043078e
commit 292a2db
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -147,6 +147,8 @@ private void configureBindings() {
 
     controller.rightBumper().whileTrue(balance);
 
+    controller.leftBumper().whileTrue(new InstantCommand(driveSubsystem::printPoses));
+
     controller.y().toggleOnTrue(driveFO);
 
     controller.b().whileTrue(deployPlunger);

diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -374,6 +374,16 @@ public boolean trajectoryDone() {
     return ramseteController.atReference();
   }
 
+  /**
+   * @deprecated debug method
+   */
+  @Deprecated
+  public void printPoses() {
+    /* KILL THIS IN PROD */
+    System.out.println("LL pose: " + limelightManager.getTargetedLimelight().getBotpose2d());
+    System.out.println("Odo pose: " + whereTheHeckAreWe.getEstimatedPosition());
+  }
+
   /**
    * Subsystem periodic, runs every scheduler loop.
    *
@@ -401,8 +411,5 @@ public void periodic() {
         whereTheHeckAreWe.addVisionMeasurement(limelight.getBotpose2d(), Timer.getFPGATimestamp());
       }
     }
-
-    /* TODO: REMOVE IN PROD */
-    //updatePIDValuesFromSmartDash();
   }
 }