commit 99af8b5ea54aaea7a33b56c2c21c4d5cfd9976bf
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Mar 28 10:50:15 2021 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Mar 28 10:50:15 2021 -0700
tree f02fe4004cc3e9e6f1525afddbb1abd48f96f62d
parent 04e8d6ca3b557adf5a8b8cfa35c6889c700ccaaf

Pre-plan auto splines

(a) Make it so that the drivetrain automatically evicts old splines
(b) Set up auto to preplan splines at construction and after every auto.

Change-Id: I96ddb3a38947da02ad9ddc6fe933b7e85727dc18

frc971/control_loops/drivetrain/splinedrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index cdedce0..1ebda6c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -41,6 +41,12 @@
   UpdateSplineHandles();
 }
 
+bool SplineDrivetrain::IsCurrentTrajectory(
+    const fb::Trajectory *trajectory) const {
+  return (current_trajectory_ != nullptr &&
+          current_trajectory().spline_handle() == trajectory->handle());
+}
+
 bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
   if (trajectory == nullptr) {
     return false;
@@ -53,6 +59,19 @@
   return false;
 }
 
+void SplineDrivetrain::DeleteTrajectory(const fb::Trajectory *trajectory) {
+  CHECK(trajectory != nullptr);
+
+  for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+    if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+      trajectories_.erase(trajectories_.begin() + ii);
+      return;
+    }
+  }
+
+  LOG(FATAL) << "Trying to remove unknown trajectory " << trajectory->handle();
+}
+
 void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
   trajectories_.emplace_back(
       std::make_unique<FinishedTrajectory>(dt_config_, trajectory));
@@ -60,11 +79,9 @@
 }
 
 void SplineDrivetrain::DeleteCurrentSpline() {
-  CHECK(current_trajectory_index_);
-  CHECK_LT(*current_trajectory_index_, trajectories_.size());
-  trajectories_.erase(trajectories_.begin() + *current_trajectory_index_);
+  DeleteTrajectory(&CHECK_NOTNULL(current_trajectory_)->trajectory());
   executing_spline_ = false;
-  current_trajectory_index_.reset();
+  current_trajectory_ = nullptr;
   current_xva_.setZero();
 }
 
@@ -92,7 +109,7 @@
   for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
     if (trajectories_[ii]->spline_handle() == *commanded_spline_) {
       executing_spline_ = true;
-      current_trajectory_index_ = ii;
+      current_trajectory_ = trajectories_[ii].get();
     }
   }
   // If we didn't find the commanded spline in the list of available splines,