more pose tests that seem to work

HighlanderRobotics · Nov 17, 2023 · 38c2d8c · 38c2d8c
1 parent f7caaf3
commit 38c2d8c
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -277,6 +277,10 @@ private void configureBindings() {
         .a()
         .whileTrue(
             swerveSubsystem.poseLockDriveCommand(() -> 3, () -> 1, () -> Math.PI, true, false));
+    controller
+        .b()
+        .whileTrue(
+            swerveSubsystem.poseLockDriveCommand(() -> 4, () -> 0.5, () -> 0, true, false));
     operator
         .y()
         .whileTrue(

diff --git a/src/main/java/frc/robot/subsystems/Swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/Swerve/SwerveSubsystem.java
@@ -269,14 +269,25 @@ public CommandBase poseLockDriveCommand(
             () -> deadband(yController.calculate(pose.getY(), y.getAsDouble()), 0),
             () ->
                 deadband(
-                    thetaController.calculate(pose.getRotation().getRadians(), theta.getAsDouble()), 0),
+                    thetaController.calculate(pose.getRotation().getRadians(), theta.getAsDouble()),
+                    0),
             fieldRelative,
             isOpenLoop,
             false)
         .alongWith(
             new PrintCommand(pose.getX() + " x"),
             new PrintCommand(pose.getY() + " y"),
             new PrintCommand(thetaController.getPositionError() + " heading error"))
+        .alongWith(
+            new RunCommand(
+                () -> {
+                  Logger.getInstance()
+                      .recordOutput("pose lock x error", xController.getPositionError());
+                  Logger.getInstance()
+                      .recordOutput("pose lock y error", yController.getPositionError());
+                  Logger.getInstance()
+                      .recordOutput("pose lock heading error", thetaController.getPositionError());
+                }))
         .alongWith(
             new RunCommand(
                 () ->