commit 1a8437749be646a1562e7112990802dfefa6d787
author Austin Schuh <austin.linux@gmail.com> Sun Apr 09 22:18:05 2023 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Apr 12 20:56:07 2023 -0700
tree 36bebfeb2ffe996bc7bd0b0b6d7de32700d8d1cf
parent 803eead4a484fca695c5de5d5392c95ad4b60e9f

Add ability to wait for a distance along a spline in auto

Change-Id: I0e992ccffeca59a655b8b890942aa7aaadfb9fd7
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

frc971/autonomous/base_autonomous_actor.cc

diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 90c1454..22909be 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -450,6 +450,35 @@
   return false;
 }
 
+bool BaseAutonomousActor::SplineHandle::SplineDistanceTraveled(
+    double distance) {
+  base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
+  if (base_autonomous_actor_->drivetrain_status_fetcher_.get()) {
+    // Confirm that:
+    // (a) The spline has started executiong (is_executing remains true even
+    //     when we reach the end of the spline).
+    // (b) The spline that we are executing is the correct one.
+    // (c) There is less than distance distance remaining.
+    if (base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+            ->goal_spline_handle() != spline_handle_) {
+      // Never done if we aren't the active spline.
+      return false;
+    }
+
+    if (base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+            ->is_executed()) {
+      return true;
+    }
+    return base_autonomous_actor_->drivetrain_status_fetcher_
+               ->trajectory_logging()
+               ->is_executing() &&
+           base_autonomous_actor_->drivetrain_status_fetcher_
+                   ->trajectory_logging()
+                   ->distance_traveled() > distance;
+  }
+  return false;
+}
+
 bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceRemaining(
     double distance) {
   ::aos::time::PhasedLoop phased_loop(
@@ -467,6 +496,23 @@
   }
 }
 
+bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceTraveled(
+    double distance) {
+  ::aos::time::PhasedLoop phased_loop(
+      frc971::controls::kLoopFrequency,
+      base_autonomous_actor_->event_loop()->monotonic_now(),
+      ActorBase::kLoopOffset);
+  while (true) {
+    if (base_autonomous_actor_->ShouldCancel()) {
+      return false;
+    }
+    phased_loop.SleepUntilNext();
+    if (SplineDistanceTraveled(distance)) {
+      return true;
+    }
+  }
+}
+
 void BaseAutonomousActor::LineFollowAtVelocity(
     double velocity, y2019::control_loops::drivetrain::SelectionHint hint) {
   {

frc971/autonomous/base_autonomous_actor.h

diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 5562dde..403854f 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -41,7 +41,9 @@
     // Whether there is less than a certain distance, in meters, remaining in
     // the current spline.
     bool SplineDistanceRemaining(double distance);
+    bool SplineDistanceTraveled(double distance);
     bool WaitForSplineDistanceRemaining(double distance);
+    bool WaitForSplineDistanceTraveled(double distance);
 
     // Returns [x, y, theta] position of the start.
     const Eigen::Vector3d &starting_position() const { return spline_start_; }