Fix WaitForDistanceRemaining auto check
The previous check could return true prematurely if the drivetrain
status message still contained information about the previous spline.
Add a check to make sure we are checking the distance remaining for the
current spline.
Change-Id: I9ade75434dcf0d64ab69be3f74979fe3c345beef
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index bb81194..1b1ce36 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -391,11 +391,19 @@
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.
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;