From d79dc4e4088f10006ad286da0d8c9a6c6a413f67 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Thu, 29 Jun 2023 01:15:04 -0700 Subject: [PATCH] fix Create a `RobotPath.HeadingSupplier.stop()` Method #53 --- .../org/carlmontrobotics/lib199/path/RobotPath.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/path/RobotPath.java b/src/main/java/org/carlmontrobotics/lib199/path/RobotPath.java index 8d47428e..be65a34b 100644 --- a/src/main/java/org/carlmontrobotics/lib199/path/RobotPath.java +++ b/src/main/java/org/carlmontrobotics/lib199/path/RobotPath.java @@ -85,12 +85,12 @@ public Command getPathCommand(boolean faceInPathDirection, boolean stopAtEnd) { if (trajectory == null) { generateTrajectory(); } - hs.reset(); // We want the robot to stay facing the same direction (in this case), so save // the current heading (make sure to update at the start of the command) AtomicReference headingRef = new AtomicReference<>(dt.getPose().getRotation()); Supplier desiredHeading = (!faceInPathDirection) ? () -> headingRef.get() : () -> hs.sample(); Command command = dt.createAutoCommand(trajectory, desiredHeading); + command = new InstantCommand(hs::reset).andThen(command, new InstantCommand(hs::stop)); if (stopAtEnd) { command = command.andThen(new InstantCommand(dt::stop, dt)); } @@ -310,5 +310,13 @@ public void reset() { timerStarted = false; timer.reset(); } + + /** + * Stops the timer + */ + public void stop() { + timer.stop(); + reset(); + } } }