From f18a68474eec90cf220f2eaa2de5a1ee3cb06780 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Mon, 18 Nov 2024 19:48:09 -0800
Subject: [PATCH] Add end conditions to following points

---
 .../java/frc/robot/autos/trailblazer/Trailblazer.java | 11 ++++++-----
 .../trailblazer/trackers/HeuristicPathTracker.java    |  9 +++++++--
 .../robot/autos/trailblazer/trackers/PathTracker.java |  8 ++++++++
 .../trailblazer/trackers/PurePursuitPathTracker.java  |  5 +++++
 4 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java b/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java
index 0a8a83b..6f1349b 100644
--- a/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java
+++ b/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java
@@ -57,11 +57,12 @@ public Command followSegment(AutoSegment segment) {
   private Command followPoint(AutoPoint point, List<AutoPointConstraint> segmentConstraints) {
     return point.command.alongWith(
         Commands.run(
-            () -> {
-              var constrainedVelocityGoal = getSwerveSetpoint(point, segmentConstraints);
-              swerve.setFieldRelativeAutoSpeeds(constrainedVelocityGoal);
-            },
-            swerve));
+                () -> {
+                  var constrainedVelocityGoal = getSwerveSetpoint(point, segmentConstraints);
+                  swerve.setFieldRelativeAutoSpeeds(constrainedVelocityGoal);
+                },
+                swerve)
+            .until(() -> pathTracker.isFinished(point)));
   }
 
   private ChassisSpeeds getSwerveSetpoint(
diff --git a/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java b/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java
index 79db50e..6cfa268 100644
--- a/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java
+++ b/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java
@@ -5,6 +5,7 @@
 import frc.robot.autos.trailblazer.AutoPoint;
 import java.util.List;
 
+// TODO: Implement https://github.com/team581/2024-offseason-bot/issues/94
 public class HeuristicPathTracker implements PathTracker {
   private List<AutoPoint> points = List.of();
 
@@ -15,7 +16,11 @@ public void resetAndSetPoints(List<AutoPoint> points) {
 
   @Override
   public Pose2d getTargetPose(Pose2d currentPose, ChassisSpeeds currentFieldRelativeRobotSpeeds) {
-    // TODO: Implement https://github.com/team581/2024-offseason-bot/issues/94
-    return currentPose;
+    return null;
+  }
+
+  @Override
+  public boolean isFinished(AutoPoint point) {
+    return false;
   }
 }
diff --git a/src/main/java/frc/robot/autos/trailblazer/trackers/PathTracker.java b/src/main/java/frc/robot/autos/trailblazer/trackers/PathTracker.java
index c4a35ba..284441b 100644
--- a/src/main/java/frc/robot/autos/trailblazer/trackers/PathTracker.java
+++ b/src/main/java/frc/robot/autos/trailblazer/trackers/PathTracker.java
@@ -25,4 +25,12 @@ public interface PathTracker {
    * @return
    */
   public Pose2d getTargetPose(Pose2d currentPose, ChassisSpeeds currentFieldRelativeRobotSpeeds);
+
+  /**
+   * Check whether the robot has finished following the given point.
+   *
+   * @param point The point to check.
+   * @return Whether the robot has finished following the given point.
+   */
+  public boolean isFinished(AutoPoint point);
 }
diff --git a/src/main/java/frc/robot/autos/trailblazer/trackers/PurePursuitPathTracker.java b/src/main/java/frc/robot/autos/trailblazer/trackers/PurePursuitPathTracker.java
index ec6b55b..742c637 100644
--- a/src/main/java/frc/robot/autos/trailblazer/trackers/PurePursuitPathTracker.java
+++ b/src/main/java/frc/robot/autos/trailblazer/trackers/PurePursuitPathTracker.java
@@ -14,4 +14,9 @@ public void resetAndSetPoints(List<AutoPoint> points) {}
   public Pose2d getTargetPose(Pose2d currentPose, ChassisSpeeds currentFieldRelativeRobotSpeeds) {
     return null;
   }
+
+  @Override
+  public boolean isFinished(AutoPoint point) {
+    return false;
+  }
 }