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.h

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 0170151..303701f 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -32,6 +32,8 @@
   // returns false.
   bool HasTrajectory(const fb::Trajectory *trajectory) const;
   void AddTrajectory(const fb::Trajectory *trajectory);
+  bool IsCurrentTrajectory(const fb::Trajectory *trajectory) const;
+  void DeleteTrajectory(const fb::Trajectory *trajectory);
 
   void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state,
               const ::Eigen::Matrix<double, 2, 1> &voltage_error);
@@ -58,6 +60,8 @@
                : true;
   }
 
+  size_t trajectory_count() const { return trajectories_.size(); }
+
   // Returns true if the splinedrivetrain is enabled.
   bool enable() const { return enable_; }
 
@@ -79,10 +83,7 @@
   void DeleteCurrentSpline();
 
   const FinishedTrajectory &current_trajectory() const {
-    CHECK(current_trajectory_index_);
-    CHECK_LE(0u, *current_trajectory_index_);
-    CHECK_LT(*current_trajectory_index_, trajectories_.size());
-    return *trajectories_[*current_trajectory_index_];
+    return *CHECK_NOTNULL(current_trajectory_);
   }
 
   const DrivetrainConfig<double> dt_config_;
@@ -92,7 +93,7 @@
   // TODO(james): Sort out construction to avoid so much dynamic memory
   // allocation...
   std::vector<std::unique_ptr<FinishedTrajectory>> trajectories_;
-  std::optional<size_t> current_trajectory_index_;
+  const FinishedTrajectory *current_trajectory_ = nullptr;
 
   std::optional<int> commanded_spline_;