Fix logic for SplineDistanceRemaining
It was getting stuck if a spline skipped executing and was just
executed.
Change-Id: If240e8135ce1e5c650bd8238be0349b01487c235
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 492302f..90c1454 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -430,18 +430,26 @@
// 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()
- ->goal_spline_handle() == spline_handle_ &&
- base_autonomous_actor_->drivetrain_status_fetcher_
- ->trajectory_logging()
->distance_remaining() < distance;
}
return false;
}
+
bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceRemaining(
double distance) {
::aos::time::PhasedLoop phased_loop(