tested working auto

bit jittery
4201VitruvianBots · Jan 18, 2025 · d31a015 · d31a015
1 parent f13971d
commit d31a015
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java
@@ -28,11 +28,14 @@ public DriveForward(CommandSwerveDrivetrain swerveDrive) {
         var point = new SwerveRequest.PointWheelsAt();
         var stopRequest = new SwerveRequest.ApplyRobotSpeeds();
 
+        // Will throw an exception if the starting pose is not present
+        var starting_pose = path.getStartingHolonomicPose().orElseThrow();
+
         // Add your commands in the addCommands() call, e.g.
         // addCommands(new FooCommand(), new BarCommand());
         addCommands(
             new InstantCommand( // Reset the pose of the robot to the starting pose of the path
-                () -> swerveDrive.resetPose(path.getStartingHolonomicPose().isPresent() ? path.getStartingHolonomicPose().get() : null)),
+                () -> swerveDrive.resetPose(starting_pose)),
             new InstantCommand(
                     () -> swerveDrive.applyRequest(() -> point.withModuleDirection(new Rotation2d())),
                     swerveDrive)