new imports

Author: 527dj

.vscode/settings.json

@@ -18,7 +18,9 @@
     {
       "name": "WPIlibUnitTests",
       "workingDirectory": "${workspaceFolder}/build/jni/release",
-      "vmargs": ["-Djava.library.path=${workspaceFolder}/build/jni/release"],
+      "vmargs": [
+        "-Djava.library.path=${workspaceFolder}/build/jni/release"
+      ],
       "env": {
         "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release",
         "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"

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

@@ -39,6 +39,9 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.collections.*;
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.units.mutable.*;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -186,6 +189,27 @@ public void periodic() {
                 module.stop();
             }
         }
+        // Retrieve the current pose from odometry
+        Pose2d currentPose = poseEstimator.getEstimatedPosition();
+        double xMeters = currentPose.getX();
+        double yMeters = currentPose.getY();
+        double headingRadians = currentPose.getRotation().getRadians();
+
+        // Convert to feet, inches, and degrees using WPILib Units library
+        double xFeet = Meters.of(xMeters).in(Feet);
+        double yFeet = Meters.of(yMeters).in(Feet);
+        double xInches = Meters.of(xMeters).in(Inches);
+        double yInches = Meters.of(yMeters).in(Inches);
+        double headingDegrees = Math.toDegrees(headingRadians);
+
+        // Log the current pose in various units
+        Logger.recordOutput("Pose/X (meters)", xMeters);
+        Logger.recordOutput("Pose/Y (meters)", yMeters);
+        Logger.recordOutput("Pose/X (feet)", xFeet);
+        Logger.recordOutput("Pose/Y (feet)", yFeet);
+        Logger.recordOutput("Pose/X (inches)", xInches);
+        Logger.recordOutput("Pose/Y (inches)", yInches);
+        Logger.recordOutput("Pose/Heading (degrees)", headingDegrees);
 
         // Log empty setpoint states when disabled
         if (DriverStation.isDisabled()) {