diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index 91b9898..f445fa4 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -123,7 +123,7 @@
   // The spline we are converting to a distance.
   aos::SizedArray<Spline, kMaxSplines> splines_;
   // An interpolation table of distances evenly distributed in alpha.
-  const absl::Span<const float> distances_;
+  absl::Span<const float> distances_;
 };
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a06b965..0d100ed 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -146,9 +146,10 @@
 class DrivetrainLoop
     : public frc971::controls::ControlLoop<Goal, Position, Status, Output> {
  public:
-  // Note that we only actually store N - 1 splines, since we need to keep one
-  // fetcher free to check whether there are any new splines.
-  static constexpr size_t kNumSplineFetchers = 5;
+  // Note that we only actually store N - 1 splines consistently, since we need
+  // to keep one fetcher free to check whether there are any new splines.
+  static constexpr size_t kNumSplineFetchers =
+      SplineDrivetrain::kMaxTrajectories;
 
   // Constructs a control loop which can take a Drivetrain or defaults to the
   // drivetrain at frc971::control_loops::drivetrain
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 5a4e5da..0f9418c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -36,18 +36,16 @@
 
 void SplineDrivetrain::SetGoal(
     const ::frc971::control_loops::drivetrain::Goal *goal) {
-  if (goal->has_spline_handle()) {
-    commanded_spline_ = goal->spline_handle();
-  } else {
-    commanded_spline_.reset();
-  }
-  UpdateSplineHandles();
+  UpdateSplineHandles(goal->has_spline_handle()
+                          ? std::make_optional<int>(goal->spline_handle())
+                          : std::nullopt);
 }
 
 bool SplineDrivetrain::IsCurrentTrajectory(
     const fb::Trajectory *trajectory) const {
-  return (current_trajectory_ != nullptr &&
-          current_trajectory().spline_handle() == trajectory->handle());
+  const FinishedTrajectory *current = current_trajectory();
+  return (current != nullptr &&
+          current->spline_handle() == trajectory->handle());
 }
 
 bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
@@ -55,7 +53,7 @@
     return false;
   }
   for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
-    if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+    if (trajectories_[ii].spline_handle() == trajectory->handle()) {
       return true;
     }
   }
@@ -66,7 +64,7 @@
   CHECK(trajectory != nullptr);
 
   for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
-    if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+    if (trajectories_[ii].spline_handle() == trajectory->handle()) {
       trajectories_.erase(trajectories_.begin() + ii);
       return;
     }
@@ -76,49 +74,69 @@
 }
 
 void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
-  trajectories_.emplace_back(std::make_unique<FinishedTrajectory>(
-      dt_config_, trajectory, velocity_drivetrain_));
-  UpdateSplineHandles();
+  CHECK_LT(trajectories_.size(), trajectories_.capacity());
+  trajectories_.emplace_back(dt_config_, trajectory, velocity_drivetrain_);
+  UpdateSplineHandles(commanded_spline_);
 }
 
 void SplineDrivetrain::DeleteCurrentSpline() {
-  DeleteTrajectory(&CHECK_NOTNULL(current_trajectory_)->trajectory());
+  DeleteTrajectory(&CHECK_NOTNULL(current_trajectory())->trajectory());
   executing_spline_ = false;
-  current_trajectory_ = nullptr;
+  commanded_spline_.reset();
   current_xva_.setZero();
 }
 
-void SplineDrivetrain::UpdateSplineHandles() {
+void SplineDrivetrain::UpdateSplineHandles(
+    std::optional<int> commanded_spline) {
   // If we are currently executing a spline and have received a change
   if (executing_spline_) {
-    if (!commanded_spline_) {
+    if (!commanded_spline) {
       // We've been told to stop executing a spline; remove it from our queue,
       // and clean up.
       DeleteCurrentSpline();
       return;
     } else {
       if (executing_spline_ &&
-          current_trajectory().spline_handle() != *commanded_spline_) {
+          CHECK_NOTNULL(current_trajectory())->spline_handle() !=
+              *commanded_spline) {
         // If we are executing a spline, and the handle has changed, garbage
         // collect the old spline.
         DeleteCurrentSpline();
       }
     }
   }
+  commanded_spline_ = commanded_spline;
   // We've now cleaned up the previous state; handle any new commands.
   if (!commanded_spline_) {
     return;
   }
   for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
-    if (trajectories_[ii]->spline_handle() == *commanded_spline_) {
+    if (trajectories_[ii].spline_handle() == *commanded_spline_) {
       executing_spline_ = true;
-      current_trajectory_ = trajectories_[ii].get();
     }
   }
   // If we didn't find the commanded spline in the list of available splines,
   // that's fine; it just means, it hasn't been fully planned yet.
 }
 
+FinishedTrajectory *SplineDrivetrain::current_trajectory() {
+  for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+    if (trajectories_[ii].spline_handle() == *commanded_spline_) {
+      return &trajectories_[ii];
+    }
+  }
+  return nullptr;
+}
+
+const FinishedTrajectory *SplineDrivetrain::current_trajectory() const {
+  for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+    if (trajectories_[ii].spline_handle() == *commanded_spline_) {
+      return &trajectories_[ii];
+    }
+  }
+  return nullptr;
+}
+
 // TODO(alex): Hold position when done following the spline.
 void SplineDrivetrain::Update(
     bool enable, const ::Eigen::Matrix<double, 5, 1> &state,
@@ -127,17 +145,19 @@
   enable_ = enable;
   if (enable && executing_spline_) {
     ::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
+    const FinishedTrajectory *const trajectory =
+        CHECK_NOTNULL(current_trajectory());
     if (!IsAtEnd() && executing_spline_) {
       // TODO(alex): It takes about a cycle for the outputs to propagate to the
       // motors. Consider delaying the output by a cycle.
-      U_ff = current_trajectory().FFVoltage(current_xva_(0));
+      U_ff = trajectory->FFVoltage(current_xva_(0));
     }
 
     const double current_distance = current_xva_(0);
     ::Eigen::Matrix<double, 2, 5> K =
-        current_trajectory().GainForDistance(current_distance);
+        trajectory->GainForDistance(current_distance);
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
-    const bool backwards = current_trajectory().drive_spline_backwards();
+    const bool backwards = trajectory->drive_spline_backwards();
     if (backwards) {
       ::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
       U_ff = -swapU;
@@ -148,11 +168,11 @@
       goal_state(4, 0) = -left_goal;
     }
     const Eigen::Matrix<double, 5, 1> relative_goal =
-        current_trajectory().StateToPathRelativeState(current_distance,
-                                                      goal_state, backwards);
+        trajectory->StateToPathRelativeState(current_distance, goal_state,
+                                             backwards);
     const Eigen::Matrix<double, 5, 1> relative_state =
-        current_trajectory().StateToPathRelativeState(current_distance, state,
-                                                      backwards);
+        trajectory->StateToPathRelativeState(current_distance, state,
+                                             backwards);
     Eigen::Matrix<double, 5, 1> state_error = relative_goal - relative_state;
     state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
     ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
@@ -163,7 +183,7 @@
     }
 
     ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2, 1>(0, 0);
-    next_xva_ = current_trajectory().GetNextXVA(dt_config_.dt, &xv_state);
+    next_xva_ = trajectory->GetNextXVA(dt_config_.dt, &xv_state);
     next_U_ = U_ff + U_fb - voltage_error;
     uncapped_U_ = next_U_;
     ScaleCapU(&next_U_);
@@ -203,7 +223,7 @@
       builder->CreateUninitializedVector(trajectories_.size(), &spline_handles);
 
   for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
-    spline_handles[ii] = trajectories_[ii]->spline_handle();
+    spline_handles[ii] = trajectories_[ii].spline_handle();
   }
 
   drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
@@ -211,7 +231,7 @@
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
     trajectory_logging_builder.add_x(goal_state(0));
     trajectory_logging_builder.add_y(goal_state(1));
-    if (current_trajectory().drive_spline_backwards()) {
+    if (CHECK_NOTNULL(current_trajectory())->drive_spline_backwards()) {
       trajectory_logging_builder.add_left_velocity(-goal_state(4));
       trajectory_logging_builder.add_right_velocity(-goal_state(3));
       trajectory_logging_builder.add_theta(
@@ -223,8 +243,7 @@
       trajectory_logging_builder.add_right_velocity(goal_state(4));
     }
   }
-  trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
-                                              executing_spline_);
+  trajectory_logging_builder.add_is_executing(!IsAtEnd() && executing_spline_);
   trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
   if (commanded_spline_) {
     trajectory_logging_builder.add_goal_spline_handle(*commanded_spline_);
@@ -233,8 +252,9 @@
     }
   }
   trajectory_logging_builder.add_distance_remaining(
-      executing_spline_ ? current_trajectory().length() - current_xva_.x()
-                        : 0.0);
+      executing_spline_
+          ? CHECK_NOTNULL(current_trajectory())->length() - current_xva_.x()
+          : 0.0);
   trajectory_logging_builder.add_available_splines(handles_vector);
 
   return trajectory_logging_builder.Finish();
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 390236b..cd712bc 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -5,7 +5,6 @@
 #include <thread>
 
 #include "Eigen/Dense"
-
 #include "aos/condition.h"
 #include "aos/mutex/mutex.h"
 #include "frc971/control_loops/control_loops_generated.h"
@@ -23,6 +22,7 @@
 
 class SplineDrivetrain {
  public:
+  static constexpr size_t kMaxTrajectories = 5;
   SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
 
   void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
@@ -49,15 +49,15 @@
   // Accessor for the current goal state, pretty much only present for debugging
   // purposes.
   ::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
-    return executing_spline_ ? current_trajectory().GoalState(current_xva_(0),
-                                                              current_xva_(1))
+    return executing_spline_ ? CHECK_NOTNULL(current_trajectory())
+                                   ->GoalState(current_xva_(0), current_xva_(1))
                              : ::Eigen::Matrix<double, 5, 1>::Zero();
   }
 
   bool IsAtEnd() const {
-    return executing_spline_
-               ? current_trajectory().is_at_end(current_xva_.block<2, 1>(0, 0))
-               : true;
+    return executing_spline_ ? CHECK_NOTNULL(current_trajectory())
+                                   ->is_at_end(current_xva_.block<2, 1>(0, 0))
+                             : true;
   }
 
   size_t trajectory_count() const { return trajectories_.size(); }
@@ -70,21 +70,20 @@
 
   // This is called to update the internal state for managing all the splines.
   // Calling it redundantly does not cause any issues. It checks the value of
-  // commanded_spline_ to determine whether we are being commanded to run a
+  // commanded_spline to determine whether we are being commanded to run a
   // spline, and if there is any trajectory in the list of trajectories matching
-  // the command, we begin/continue executing that spline. If commanded_spline_
+  // the command, we begin/continue executing that spline. If commanded_spline
   // is empty or has changed, we stop executing the previous trajectory and
   // remove it from trajectories_. Then, when the drivetrain code checks
   // HasTrajectory() for the old trajectory, it will return false and the
   // drivetrain can free up the fetcher to get the next trajectory.
-  void UpdateSplineHandles();
+  void UpdateSplineHandles(std::optional<int> commanded_spline);
 
   // Deletes the currently executing trajectory.
   void DeleteCurrentSpline();
 
-  const FinishedTrajectory &current_trajectory() const {
-    return *CHECK_NOTNULL(current_trajectory_);
-  }
+  FinishedTrajectory *current_trajectory();
+  const FinishedTrajectory *current_trajectory() const;
 
   const DrivetrainConfig<double> dt_config_;
 
@@ -97,8 +96,7 @@
 
   // TODO(james): Sort out construction to avoid so much dynamic memory
   // allocation...
-  std::vector<std::unique_ptr<FinishedTrajectory>> trajectories_;
-  const FinishedTrajectory *current_trajectory_ = nullptr;
+  aos::SizedArray<FinishedTrajectory, kMaxTrajectories> trajectories_;
 
   std::optional<int> commanded_spline_;
 
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 6bccc74..ab1c55f 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -3,12 +3,12 @@
 
 #include <chrono>
 
-#include "aos/flatbuffers.h"
 #include "Eigen/Dense"
+#include "aos/flatbuffers.h"
 #include "frc971/control_loops/drivetrain/distance_spline.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/drivetrain/trajectory_generated.h"
 #include "frc971/control_loops/drivetrain/spline_goal_generated.h"
+#include "frc971/control_loops/drivetrain/trajectory_generated.h"
 #include "frc971/control_loops/hybrid_state_feedback_loop.h"
 #include "frc971/control_loops/runge_kutta.h"
 #include "frc971/control_loops/state_feedback_loop.h"
@@ -193,11 +193,11 @@
                         HybridKalman<2, 2, 2>>>
       velocity_drivetrain_;
 
-  const DrivetrainConfig<double> config_;
+  DrivetrainConfig<double> config_;
 
   // Robot radiuses.
-  const double robot_radius_l_;
-  const double robot_radius_r_;
+  double robot_radius_l_;
+  double robot_radius_r_;
   float lateral_acceleration_ = 3.0;
   float longitudinal_acceleration_ = 2.0;
   float voltage_limit_ = 12.0;
@@ -205,7 +205,7 @@
 
 // A wrapper around the Trajectory flatbuffer to allow for controlling to a
 // spline using a pre-generated trajectory.
-class FinishedTrajectory  : public BaseTrajectory {
+class FinishedTrajectory : public BaseTrajectory {
  public:
   // Note: The lifetime of the supplied buffer is assumed to be greater than
   // that of this object.
@@ -225,6 +225,11 @@
                 HybridKalman<2, 2, 2>>>(
                 config.make_hybrid_drivetrain_velocity_loop())) {}
 
+  FinishedTrajectory(const FinishedTrajectory &) = delete;
+  FinishedTrajectory &operator=(const FinishedTrajectory &) = delete;
+  FinishedTrajectory(FinishedTrajectory &&) = default;
+  FinishedTrajectory &operator=(FinishedTrajectory &&) = default;
+
   virtual ~FinishedTrajectory() = default;
 
   // Takes the 5-element state that is [x, y, theta, v_left, v_right] and
@@ -252,7 +257,7 @@
  private:
   const DistanceSplineBase &spline() const override { return spline_; }
   const fb::Trajectory *buffer_;
-  const FinishedDistanceSpline spline_;
+  FinishedDistanceSpline spline_;
 };
 
 // Class to handle plannign a trajectory and producing a flatbuffer containing
@@ -268,8 +273,7 @@
 
   virtual ~Trajectory() = default;
 
-  std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(
-      std::chrono::nanoseconds dt);
+  std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(std::chrono::nanoseconds dt);
 
   enum class VoltageLimit {
     kConservative,
@@ -375,10 +379,7 @@
   const DistanceSpline &spline() const override { return spline_; }
 
  private:
-
-  float plan_velocity(size_t index) const override {
-    return plan_[index];
-  }
+  float plan_velocity(size_t index) const override { return plan_[index]; }
   size_t distance_plan_size() const override { return plan_.size(); }
 
   fb::SegmentConstraint plan_constraint(size_t index) const override {
@@ -410,8 +411,7 @@
 inline Eigen::Matrix<double, 5, 1> ContinuousDynamics(
     const StateFeedbackHybridPlant<2, 2, 2> &velocity_drivetrain,
     const Eigen::Matrix<double, 2, 2> &Tlr_to_la,
-    const Eigen::Matrix<double, 5, 1> X,
-    const Eigen::Matrix<double, 2, 1> U) {
+    const Eigen::Matrix<double, 5, 1> X, const Eigen::Matrix<double, 2, 1> U) {
   const auto &velocity = X.block<2, 1>(3, 0);
   const double theta = X(2);
   Eigen::Matrix<double, 2, 1> la = Tlr_to_la * velocity;