Add ability to wait for a distance along a spline in auto
Change-Id: I0e992ccffeca59a655b8b890942aa7aaadfb9fd7
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 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) {
{
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_; }
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 89dc3d5..8158e75 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -77,6 +77,7 @@
left_velocity:float (id: 9);
right_velocity:float (id: 10);
distance_remaining:float (id: 11);
+ distance_traveled:float (id: 13);
// Splines that we have full plans for.
available_splines:[int] (id: 12);
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0f9418c..ddcf63d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -256,6 +256,8 @@
? CHECK_NOTNULL(current_trajectory())->length() - current_xva_.x()
: 0.0);
trajectory_logging_builder.add_available_splines(handles_vector);
+ trajectory_logging_builder.add_distance_traveled(
+ executing_spline_ ? current_xva_.x() : 0.0);
return trajectory_logging_builder.Finish();
}