Add spline distance remaining functions
Change-Id: I0d87365a0f93dba33d50d11729b0e3485d3e8437
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 86349de..a71ee7d 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -350,6 +350,41 @@
}
}
+bool BaseAutonomousActor::SplineHandle::SplineDistanceRemaining(
+ double distance) {
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ return drivetrain_queue.status->trajectory_logging.is_executing &&
+ drivetrain_queue.status->trajectory_logging.distance_remaining <
+ distance;
+ }
+ return false;
+}
+bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceRemaining(
+ double distance) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (base_autonomous_actor_->ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ if (SplineDistanceRemaining(distance)) {
+ return true;
+ }
+ }
+}
+
+void BaseAutonomousActor::LineFollowAtVelocity(double velocity) {
+ auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ drivetrain_message->controller_type = 3;
+ // TODO(james): Currently the 4.0 is copied from the
+ // line_follow_drivetrain.cc, but it is somewhat year-specific, so we should
+ // factor it out in some way.
+ drivetrain_message->throttle = velocity / 4.0;
+ drivetrain_message.Send();
+}
+
BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
const ::frc971::MultiSpline &spline, SplineDirection direction) {
LOG(INFO, "Planning spline\n");
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index aeef38c..e060a2c 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -29,7 +29,10 @@
bool IsDone();
bool WaitForDone();
- // Wait for done, wait until X from the end, wait for distance from the end
+ // Whether there is less than a certain distance, in meters, remaining in
+ // the current spline.
+ bool SplineDistanceRemaining(double distance);
+ bool WaitForSplineDistanceRemaining(double distance);
private:
friend BaseAutonomousActor;
@@ -67,6 +70,8 @@
// Returns true if the drive has finished.
bool IsDriveDone();
+ void LineFollowAtVelocity(double velocity);
+
// Waits until the robot is pitched up above the specified angle, or the move
// finishes. Returns true on success, and false if it cancels.
bool WaitForAboveAngle(double angle);