Merge pull request #54 from DeepBlueRobotics/fix-heading-supplier

Create a `RobotPath.HeadingSupplier.stop()` Method

Author: CoolSpy3

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<Rotation2d> headingRef = new AtomicReference<>(dt.getPose().getRotation());
         Supplier<Rotation2d> 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();
+        }
     }
 }