diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 1da9db3..b08e96f 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -6,10 +6,20 @@
 load("@npm_bazel_typescript//:defs.bzl", "ts_library")
 
 flatbuffer_cc_library(
+    name = "spline_goal_fbs",
+    srcs = ["spline_goal.fbs"],
+    gen_reflections = 1,
+    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+flatbuffer_cc_library(
     name = "drivetrain_goal_fbs",
     srcs = ["drivetrain_goal.fbs"],
     gen_reflections = 1,
-    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+    includes = [
+        ":spline_goal_fbs_includes",
+        "//frc971/control_loops:control_loops_fbs_includes",
+    ],
 )
 
 flatbuffer_cc_library(
@@ -31,6 +41,13 @@
     includes = ["//frc971/control_loops:control_loops_fbs_includes"],
 )
 
+flatbuffer_cc_library(
+    name = "trajectory_fbs",
+    srcs = ["trajectory.fbs"],
+    gen_reflections = 1,
+    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
 flatbuffer_ts_library(
     name = "drivetrain_status_ts_fbs",
     srcs = ["drivetrain_status.fbs"],
@@ -70,7 +87,10 @@
     name = "drivetrain_goal_float_fbs",
     srcs = ["drivetrain_goal_float.fbs"],
     gen_reflections = 1,
-    includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+    includes = [
+        ":spline_goal_fbs_includes",
+        "//frc971/control_loops:control_loops_fbs_includes",
+    ],
 )
 
 flatbuffer_cc_library(
@@ -118,6 +138,8 @@
     src = "drivetrain_config.json",
     flatbuffers = [
         ":drivetrain_goal_fbs",
+        ":trajectory_fbs",
+        ":spline_goal_fbs",
         ":drivetrain_output_fbs",
         ":drivetrain_status_fbs",
         ":drivetrain_position_fbs",
@@ -220,6 +242,7 @@
         ":drivetrain_output_fbs",
         ":drivetrain_status_fbs",
         ":spline",
+        ":spline_goal_fbs",
         ":trajectory",
         "//aos:condition",
         "//aos:init",
@@ -243,6 +266,7 @@
         ":drivetrain_output_fbs",
         ":drivetrain_status_fbs",
         ":localizer",
+        ":spline_goal_fbs",
         "//aos:math",
         "//aos/util:math",
         "//frc971/control_loops:c2d",
@@ -289,6 +313,7 @@
         ":drivetrain_status_fbs",
         ":gear",
         ":localizer",
+        ":spline_goal_fbs",
         "//aos:math",
         "//aos/controls:control_loop",
         "//aos/controls:polytope",
@@ -322,6 +347,7 @@
         "//aos/controls:polytope",
         "//frc971/control_loops:coerce_goal",
         "//frc971/control_loops:control_loops_fbs",
+        ":spline_goal_fbs",
         "//frc971/control_loops:state_feedback_loop",
     ] + select({
         "@platforms//os:linux": [
@@ -396,6 +422,7 @@
         ":localizer",
         ":localizer_fbs",
         ":polydrivetrain",
+        ":spline_goal_fbs",
         ":splinedrivetrain",
         ":ssdrivetrain",
         "//aos/controls:control_loop",
@@ -462,6 +489,7 @@
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
         ":drivetrain_config",
+        ":trajectory_generator",
         ":drivetrain_lib",
         ":localizer_fbs",
         ":drivetrain_goal_fbs",
@@ -568,8 +596,11 @@
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
         ":spline",
+        ":trajectory_fbs",
         "//aos/logging",
+        "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:fixed_quadrature",
+        "@com_github_google_glog//:glog",
         "@org_tuxfamily_eigen//:eigen",
     ],
 )
@@ -591,6 +622,7 @@
     deps = [
         ":distance_spline",
         "//aos/testing:googletest",
+        "//aos:flatbuffers",
         "//aos/testing:test_shm",
         "@com_github_gflags_gflags//:gflags",
     ] + cpu_select({
@@ -609,6 +641,8 @@
     deps = [
         ":distance_spline",
         ":drivetrain_config",
+        ":spline_goal_fbs",
+        ":trajectory_fbs",
         "//frc971/control_loops:c2d",
         "//frc971/control_loops:dlqr",
         "//frc971/control_loops:hybrid_state_feedback_loop",
@@ -618,6 +652,20 @@
     ],
 )
 
+cc_library(
+    name = "trajectory_generator",
+    srcs = ["trajectory_generator.cc"],
+    hdrs = ["trajectory_generator.h"],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":distance_spline",
+        ":drivetrain_config",
+        ":spline_goal_fbs",
+        ":trajectory",
+        ":trajectory_fbs",
+    ],
+)
+
 cc_binary(
     name = "trajectory_plot",
     srcs = [
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index 113eb27..5c05fe4 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -2,14 +2,15 @@
 
 #include "aos/logging/logging.h"
 #include "frc971/control_loops/drivetrain/spline.h"
+#include "glog/logging.h"
 
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
 
-::std::vector<double> DistanceSpline::BuildDistances(size_t num_alpha) {
+::std::vector<float> DistanceSpline::BuildDistances(size_t num_alpha) {
   num_alpha = num_alpha == 0 ? 100 * splines_.size() : num_alpha;
-  ::std::vector<double> distances;
+  ::std::vector<float> distances;
   distances.push_back(0.0);
 
   if (splines_.size() > 1) {
@@ -76,12 +77,82 @@
   return distances;
 }
 
+std::vector<Spline> FlatbufferToSplines(const MultiSpline *fb) {
+  CHECK_NOTNULL(fb);
+  const size_t spline_count = fb->spline_count();
+  CHECK_EQ(fb->spline_x()->size(), static_cast<size_t>(spline_count * 5 + 1));
+  CHECK_EQ(fb->spline_y()->size(), static_cast<size_t>(spline_count * 5 + 1));
+  std::vector<Spline> splines;
+  for (size_t ii = 0; ii < spline_count; ++ii) {
+    Eigen::Matrix<double, 2, 6> points;
+    for (int jj = 0; jj < 6; ++jj) {
+      points(0, jj) = fb->spline_x()->Get(ii * 5 + jj);
+      points(1, jj) = fb->spline_y()->Get(ii * 5 + jj);
+    }
+    splines.emplace_back(Spline(points));
+  }
+  return splines;
+}
+
 DistanceSpline::DistanceSpline(::std::vector<Spline> &&splines, int num_alpha)
     : splines_(::std::move(splines)), distances_(BuildDistances(num_alpha)) {}
 
 DistanceSpline::DistanceSpline(const Spline &spline, int num_alpha)
     : splines_({spline}), distances_(BuildDistances(num_alpha)) {}
 
+DistanceSpline::DistanceSpline(const MultiSpline *fb, int num_alpha)
+    : splines_(FlatbufferToSplines(fb)),
+      distances_(BuildDistances(num_alpha)) {}
+
+// TODO(james): Directly use the flatbuffer vector for accessing distances,
+// rather than doing this redundant copy.
+DistanceSpline::DistanceSpline(const fb::DistanceSpline &fb)
+    : splines_(FlatbufferToSplines(fb.spline())),
+      distances_(CHECK_NOTNULL(fb.distances())->begin(),
+                 fb.distances()->end()) {}
+
+flatbuffers::Offset<fb::DistanceSpline> DistanceSpline::Serialize(
+    flatbuffers::FlatBufferBuilder *fbb,
+    flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Constraint>>>
+        constraints) const {
+  if (splines_.empty()) {
+    return {};
+  }
+  const size_t num_points = splines_.size() * 5 + 1;
+  float *spline_x_vector = nullptr;
+  float *spline_y_vector = nullptr;
+  const flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+      fbb->CreateUninitializedVector(num_points, &spline_x_vector);
+  const flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+      fbb->CreateUninitializedVector(num_points, &spline_y_vector);
+  CHECK_NOTNULL(spline_x_vector);
+  CHECK_NOTNULL(spline_y_vector);
+  spline_x_vector[0] = splines_[0].control_points()(0, 0);
+  spline_y_vector[0] = splines_[0].control_points()(1, 0);
+  for (size_t spline_index = 0; spline_index < splines_.size();
+       ++spline_index) {
+    for (size_t point = 1; point < 6u; ++point) {
+      spline_x_vector[spline_index * 5 + point] =
+          splines_[spline_index].control_points()(0, point);
+      spline_y_vector[spline_index * 5 + point] =
+          splines_[spline_index].control_points()(1, point);
+    }
+  }
+  MultiSpline::Builder multi_spline_builder(*fbb);
+  multi_spline_builder.add_spline_count(splines_.size());
+  multi_spline_builder.add_spline_x(spline_x_offset);
+  multi_spline_builder.add_spline_y(spline_y_offset);
+  multi_spline_builder.add_constraints(constraints);
+  const flatbuffers::Offset<MultiSpline> multi_spline_offset =
+      multi_spline_builder.Finish();
+  const flatbuffers::Offset<flatbuffers::Vector<float>> distances_offset =
+      fbb->CreateVector(distances_);
+  fb::DistanceSpline::Builder spline_builder(*fbb);
+  spline_builder.add_spline(multi_spline_offset);
+  spline_builder.add_distances(distances_offset);
+  return spline_builder.Finish();
+}
+
 ::Eigen::Matrix<double, 2, 1> DistanceSpline::DDXY(double distance) const {
   const AlphaAndIndex a = DistanceToAlpha(distance);
   const ::Eigen::Matrix<double, 2, 1> dspline_point =
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index ab88560..0feb902 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -5,12 +5,15 @@
 
 #include "Eigen/Dense"
 #include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory_generated.h"
 #include "frc971/control_loops/fixed_quadrature.h"
 
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
 
+std::vector<Spline> FlatbufferToSplines(const MultiSpline *fb);
+
 // Class to hold a spline as a function of distance.
 class DistanceSpline {
  public:
@@ -18,6 +21,15 @@
 
   DistanceSpline(const Spline &spline, int num_alpha = 0);
   DistanceSpline(::std::vector<Spline> &&splines, int num_alpha = 0);
+  DistanceSpline(const MultiSpline *fb, int num_alpha = 0);
+  // Copies the distances for the spline directly out of the provided buffer,
+  // rather than constructing the distances from the original splines.
+  DistanceSpline(const fb::DistanceSpline &fb);
+
+  flatbuffers::Offset<fb::DistanceSpline> Serialize(
+      flatbuffers::FlatBufferBuilder *fbb,
+      flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Constraint>>>
+          constraints) const;
 
   // Returns a point on the spline as a function of distance.
   ::Eigen::Matrix<double, 2, 1> XY(double distance) const {
@@ -61,6 +73,9 @@
   // Returns the length of the path in meters.
   double length() const { return distances_.back(); }
 
+  const std::vector<float> &distances() const { return distances_; }
+  const std::vector<Spline> &splines() const { return splines_; }
+
  private:
   struct AlphaAndIndex {
     size_t index;
@@ -70,12 +85,12 @@
   // Computes alpha for a distance
   AlphaAndIndex DistanceToAlpha(double distance) const;
 
-  ::std::vector<double> BuildDistances(size_t num_alpha);
+  ::std::vector<float> BuildDistances(size_t num_alpha);
 
   // The spline we are converting to a distance.
   const ::std::vector<Spline> splines_;
   // An interpolation table of distances evenly distributed in alpha.
-  const ::std::vector<double> distances_;
+  const ::std::vector<float> distances_;
 };
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index 8189fe9..1ed756a 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -3,6 +3,7 @@
 #include <vector>
 
 #include "aos/testing/test_shm.h"
+#include "aos/flatbuffers.h"
 #include "gflags/gflags.h"
 #include "gtest/gtest.h"
 #if defined(SUPPORT_PLOT)
@@ -152,6 +153,24 @@
 #endif
 }
 
+TEST_P(ParameterizedDistanceSplineTest, Serialization) {
+  flatbuffers::FlatBufferBuilder fbb;
+  fbb.Finish(distance_spline_.Serialize(&fbb, {}));
+  const aos::FlatbufferDetachedBuffer<fb::DistanceSpline> spline(fbb.Release());
+  DistanceSpline reread_spline(spline.message());
+  ASSERT_EQ(reread_spline.distances().size(),
+            distance_spline_.distances().size());
+  for (size_t ii = 0; ii < distance_spline_.distances().size(); ++ii) {
+    const float orig_distance = distance_spline_.distances()[ii];
+    const float new_distance = reread_spline.distances()[ii];
+    EXPECT_EQ(orig_distance, new_distance);
+    EXPECT_FLOAT_EQ(distance_spline_.XY(orig_distance).x(),
+                    reread_spline.XY(new_distance).x());
+    EXPECT_FLOAT_EQ(distance_spline_.XY(orig_distance).y(),
+                    reread_spline.XY(new_distance).y());
+  }
+}
+
 INSTANTIATE_TEST_CASE_P(
     DistanceSplineTest, ParameterizedDistanceSplineTest,
     ::testing::Values(
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 6336386..1395e5d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -309,6 +309,41 @@
       dt_spline_(dt_config_),
       dt_line_follow_(dt_config_, localizer->target_selector()) {
   event_loop->SetRuntimeRealtimePriority(30);
+  for (size_t ii = 0; ii < trajectory_fetchers_.size(); ++ii) {
+    trajectory_fetchers_[ii].fetcher =
+        event_loop->MakeFetcher<fb::Trajectory>("/drivetrain");
+  }
+}
+
+void DrivetrainLoop::UpdateTrajectoryFetchers() {
+  for (auto &fetcher : trajectory_fetchers_) {
+    const fb::Trajectory *trajectory = fetcher.fetcher.get();
+    // If the current fetcher is already being used by the SplineDrivetrain,
+    // don't touch it.
+    // We have to check both in_use and HasTrajectory because if
+    // in_use is true and HasTrajectory() is false, that implies that the
+    // SplineDrivetrain has finished executing the trajectory and disposed of
+    // it; if in_use is false and HasTrajectory() is true, that implies that
+    // this fetcher is at the same point in the queue as another fetcher, and
+    // that the other fetcher is the one that we are using to keep the message
+    // pinned.
+    // TODO(james): Consider garbage-collecting splines once we run out of
+    // fetchers.
+    if (fetcher.in_use && dt_spline_.HasTrajectory(trajectory)) {
+      continue;
+    }
+    fetcher.in_use = false;
+    // Go through and find the next Trajectory that isn't already held by the
+    // SplineDrivetrain, and add it.
+    while (fetcher.fetcher.FetchNext()) {
+      trajectory = fetcher.fetcher.get();
+      if (!dt_spline_.HasTrajectory(trajectory)) {
+        fetcher.in_use = true;
+        dt_spline_.AddTrajectory(trajectory);
+        break;
+      }
+    }
+  }
 }
 
 void DrivetrainLoop::RunIteration(
@@ -326,6 +361,8 @@
     filters_.Reset(monotonic_now, position);
   }
 
+  UpdateTrajectoryFetchers();
+
   filters_.Correct(monotonic_now, position);
 
   // Set the gear-logging parts of the status
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9a23edd..070cd03 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -144,6 +144,8 @@
 class DrivetrainLoop
     : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
  public:
+  static constexpr size_t kNumSplineFetchers = 4;
+
   // Constructs a control loop which can take a Drivetrain or defaults to the
   // drivetrain at frc971::control_loops::drivetrain
   explicit DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
@@ -154,6 +156,11 @@
   virtual ~DrivetrainLoop() {}
 
  protected:
+  struct TrajectoryFetcherState {
+    aos::Fetcher<fb::Trajectory> fetcher;
+    bool in_use = false;
+  };
+
   // Executes one cycle of the control loop.
   void RunIteration(
       const ::frc971::control_loops::drivetrain::Goal *goal,
@@ -165,8 +172,11 @@
   flatbuffers::Offset<drivetrain::Output> Zero(
       aos::Sender<drivetrain::Output>::Builder *builder) override;
 
+  void UpdateTrajectoryFetchers();
+
   const DrivetrainConfig<double> dt_config_;
   DrivetrainFilters filters_;
+  std::array<TrajectoryFetcherState, kNumSplineFetchers> trajectory_fetchers_;
 
   PolyDrivetrain<double> dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index e505c82..86318ca 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -7,6 +7,20 @@
       "max_size": 2000,
       "frequency": 200
     },
+    /* TODO(james): Experiment with size limitations. */
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.fb.Trajectory",
+      "max_size": 200000,
+      "frequency": 10,
+      "read_method": "PIN",
+      "num_readers": 6
+    },
+    {
+      "name": "/drivetrain",
+      "type": "frc971.control_loops.drivetrain.SplineGoal",
+      "frequency": 10
+    },
     {
       "name": "/drivetrain",
       "type": "frc971.sensors.GyroReading",
diff --git a/frc971/control_loops/drivetrain/drivetrain_goal.fbs b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
index 3df5210..25fe93a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_goal.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
@@ -1,4 +1,4 @@
-include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/drivetrain/spline_goal.fbs";
 
 namespace frc971.control_loops.drivetrain;
 
@@ -9,17 +9,6 @@
   LINE_FOLLOWER,
 }
 
-table SplineGoal {
-  // index of the spline. Zero indicates the spline should not be computed.
-  spline_idx:int (id: 0);
-
-  // Acutal spline.
-  spline:frc971.MultiSpline (id: 1);
-
-  // Whether to follow the spline driving forwards or backwards.
-  drive_spline_backwards:bool (id: 2);
-}
-
 table Goal {
   // Position of the steering wheel (positive = turning left when going
   // forwards).
@@ -56,9 +45,11 @@
   // Parameters for a spline to follow. This just contains info on a spline to
   // compute. Each time this is sent, spline drivetrain will compute a new
   // spline.
-  spline:SplineGoal (id: 14);
+  // TODO(james): Is this actually how we want to manage deprecation?
+  spline:SplineGoal (id: 14, deprecated);
 
-  // Which spline to follow.
+  // Which spline to follow; if this is not populated, any running spline will
+  // be cancelled.
   spline_handle:int (id: 15);
 }
 
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index b3097ec..7dd5a52 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -10,6 +10,7 @@
 #include "aos/time/time.h"
 #include "gflags/gflags.h"
 #include "gtest/gtest.h"
+#include "gmock/gmock.h"
 
 #include "frc971/control_loops/coerce_goal.h"
 #include "frc971/control_loops/drivetrain/drivetrain.h"
@@ -19,6 +20,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
 #include "frc971/control_loops/drivetrain/localizer_generated.h"
 #include "frc971/queues/gyro_generated.h"
 
@@ -43,6 +45,8 @@
         test_event_loop_(MakeEventLoop("test")),
         drivetrain_goal_sender_(
             test_event_loop_->MakeSender<Goal>("/drivetrain")),
+        trajectory_goal_sender_(
+            test_event_loop_->MakeSender<SplineGoal>("/drivetrain")),
         drivetrain_goal_fetcher_(
             test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
         drivetrain_status_fetcher_(
@@ -52,9 +56,12 @@
         localizer_control_sender_(
             test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
         drivetrain_event_loop_(MakeEventLoop("drivetrain")),
+        trajectory_generator_event_loop_(MakeEventLoop("trajectory_generator")),
         dt_config_(GetTestDrivetrainConfig()),
         localizer_(drivetrain_event_loop_.get(), dt_config_),
         drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+        trajectory_generator_(trajectory_generator_event_loop_.get(),
+                              dt_config_),
         drivetrain_plant_event_loop_(MakeEventLoop("drivetrain_plant")),
         drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_) {
     // Too many tests care...
@@ -106,16 +113,6 @@
     EXPECT_NEAR(actual(1), estimated_y, spline_estimate_tolerance_);
   }
 
-  void WaitForTrajectoryPlan() {
-    do {
-      // Run for fewer iterations while the worker thread computes.
-      ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
-      RunFor(dt());
-      EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
-    } while (CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
-                 ->planning_state() != PlanningState::PLANNED);
-  }
-
   void WaitForTrajectoryExecution() {
     do {
       RunFor(dt());
@@ -150,6 +147,7 @@
   ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
   ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
       drivetrain_goal_sender_;
+  ::aos::Sender<SplineGoal> trajectory_goal_sender_;
   ::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
       drivetrain_goal_fetcher_;
   ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
@@ -159,10 +157,12 @@
   ::aos::Sender<LocalizerControl> localizer_control_sender_;
 
   ::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
+  ::std::unique_ptr<::aos::EventLoop> trajectory_generator_event_loop_;
   const DrivetrainConfig<double> dt_config_;
   DeadReckonEkf localizer_;
   // Create a loop and simulation plant.
   DrivetrainLoop drivetrain_;
+  TrajectoryGenerator trajectory_generator_;
 
   ::std::unique_ptr<::aos::EventLoop> drivetrain_plant_event_loop_;
   DrivetrainSimulation drivetrain_plant_;
@@ -543,7 +543,7 @@
 TEST_F(DrivetrainTest, SplineSimple) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -564,13 +564,7 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
   RunFor(dt());
 
@@ -582,17 +576,17 @@
     goal_builder.add_spline_handle(1);
     EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(2000));
   VerifyNearSplineGoal();
 }
 
+
 // Tests that we can drive a spline backwards.
 TEST_F(DrivetrainTest, SplineSimpleBackwards) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>(
@@ -615,13 +609,8 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(true);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
 
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
   RunFor(dt());
 
@@ -632,7 +621,6 @@
     goal_builder.add_spline_handle(1);
     EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   // Check that we are right on the spline at the start (otherwise the feedback
   // will tend to correct for going the wrong direction).
@@ -659,7 +647,7 @@
 TEST_F(DrivetrainTest, SplineSingleGoal) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -680,16 +668,16 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
+    EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
+  }
 
+  {
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
     goal_builder.add_spline_handle(1);
     EXPECT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(2000));
   VerifyNearSplineGoal();
@@ -699,7 +687,7 @@
 TEST_F(DrivetrainTest, SplineStop) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -720,23 +708,18 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+  }
+  {
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(500));
   drivetrain_status_fetcher_.Fetch();
-  const double goal_x =
-      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
-  const double goal_y =
-      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
 
   // Now stop.
   {
@@ -744,25 +727,28 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(0);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  RunFor(chrono::milliseconds(2000));
+  for (int i = 0; i < 100; ++i) {
+    RunFor(dt());
+    ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+    EXPECT_EQ(0.0, drivetrain_output_fetcher_->left_voltage());
+    EXPECT_EQ(0.0, drivetrain_output_fetcher_->right_voltage());
+    // The goal should be null after stopping.
+    ASSERT_TRUE(drivetrain_status_fetcher_.Fetch());
+    EXPECT_FALSE(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+                     ->has_x());
+    EXPECT_FALSE(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+                     ->has_y());
+  }
 
-  // The goal shouldn't change after being stopped.
-  drivetrain_status_fetcher_.Fetch();
-  EXPECT_NEAR(
-      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
-      goal_x, 1e-9);
-  EXPECT_NEAR(
-      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
-      goal_y, 1e-9);
 }
 
 // Tests that a spline can't be restarted.
 TEST_F(DrivetrainTest, SplineRestart) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -783,23 +769,19 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+  }
+  {
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
 
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(500));
   drivetrain_status_fetcher_.Fetch();
-  const double goal_x =
-      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
-  const double goal_y =
-      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
 
   // Send a stop goal.
   {
@@ -807,7 +789,7 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(0);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::milliseconds(500));
 
@@ -817,25 +799,23 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::milliseconds(2000));
 
-  // The goal shouldn't change after being stopped and restarted.
+  // The goal should be empty.
   drivetrain_status_fetcher_.Fetch();
-  EXPECT_NEAR(
-      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
-      goal_x, 1e-9);
-  EXPECT_NEAR(
-      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
-      goal_y, 1e-9);
+  EXPECT_FALSE(
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->has_x());
+  EXPECT_FALSE(
+      CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->has_y());
 }
 
 // Tests that simple spline converges when it doesn't start where it thinks.
 TEST_F(DrivetrainTest, SplineOffset) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.2, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -856,13 +836,7 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
   RunFor(dt());
 
@@ -871,9 +845,8 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(5000));
   VerifyNearSplineGoal();
@@ -884,7 +857,7 @@
 TEST_F(DrivetrainTest, SplineSideOffset) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -905,13 +878,7 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
   RunFor(dt());
 
@@ -920,9 +887,8 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(5000));
 
@@ -940,7 +906,7 @@
   drivetrain_plant_.set_left_voltage_offset(1.0);
   drivetrain_plant_.set_right_voltage_offset(0.5);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -961,13 +927,7 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
   RunFor(dt());
 
@@ -976,9 +936,8 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(5000));
   // Since the voltage error compensation is disabled, expect that we will have
@@ -1004,7 +963,7 @@
 TEST_F(DrivetrainTest, MultiSpline) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>(
@@ -1027,13 +986,7 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
   RunFor(dt());
 
@@ -1042,9 +995,8 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(4000));
   VerifyNearSplineGoal();
@@ -1054,7 +1006,7 @@
 TEST_F(DrivetrainTest, SequentialSplines) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1075,13 +1027,7 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
   RunFor(dt());
 
@@ -1090,9 +1036,8 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   WaitForTrajectoryExecution();
 
@@ -1100,7 +1045,7 @@
 
   // Second spline.
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
@@ -1121,13 +1066,7 @@
     spline_goal_builder.add_spline_idx(2);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
   RunFor(dt());
 
@@ -1137,18 +1076,18 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(2);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(2000));
   VerifyNearSplineGoal();
 }
+
 // Tests that a second spline will run if the first is stopped.
 TEST_F(DrivetrainTest, SplineStopFirst) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1169,18 +1108,24 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+  }
+  {
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(1);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
-  RunFor(chrono::milliseconds(1000));
+  RunFor(chrono::milliseconds(2000));
+  {
+    drivetrain_status_fetcher_.Fetch();
+    EXPECT_TRUE(
+        drivetrain_status_fetcher_->trajectory_logging()->is_executing());
+    EXPECT_FALSE(
+        drivetrain_status_fetcher_->trajectory_logging()->is_executed());
+  }
 
   // Stop goal
   {
@@ -1188,13 +1133,13 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(0);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
   RunFor(chrono::milliseconds(500));
 
   // Second spline goal.
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
@@ -1215,16 +1160,15 @@
     spline_goal_builder.add_spline_idx(2);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+  }
+  {
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
     goal_builder.add_spline_handle(2);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   WaitForTrajectoryExecution();
   spline_control_tolerance_ = 0.1;
@@ -1237,7 +1181,7 @@
 TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1258,22 +1202,14 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    goal_builder.add_spline_handle(0);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   RunFor(chrono::milliseconds(1000));
 
   // Plan another spline, but don't start it yet:
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.75, 1.25, 1.5, 1.25, 1.0});
@@ -1294,16 +1230,8 @@
     spline_goal_builder.add_spline_idx(2);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    goal_builder.add_spline_handle(0);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   // Now execute it.
   {
@@ -1311,7 +1239,7 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(2);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   WaitForTrajectoryExecution();
@@ -1323,7 +1251,7 @@
 TEST_F(DrivetrainTest, ParallelSplines) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1344,19 +1272,12 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   // Second spline goal
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
@@ -1377,14 +1298,14 @@
     spline_goal_builder.add_spline_idx(2);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+  }
+  {
+    auto builder = drivetrain_goal_sender_.MakeBuilder();
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
   WaitForTrajectoryExecution();
 
@@ -1394,7 +1315,7 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(2);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   RunFor(chrono::milliseconds(4000));
@@ -1405,7 +1326,7 @@
 TEST_F(DrivetrainTest, OnlyPlanSpline) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1426,23 +1347,12 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
 
   for (int i = 0; i < 100; ++i) {
     RunFor(dt());
     drivetrain_status_fetcher_.Fetch();
-    EXPECT_EQ(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
-                  ->planning_state(),
-              PlanningState::PLANNED);
-    ::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
   }
   VerifyNearSplineGoal();
 }
@@ -1451,7 +1361,7 @@
 TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
   SetEnabled(true);
   {
-    auto builder = drivetrain_goal_sender_.MakeBuilder();
+    auto builder = trajectory_goal_sender_.MakeBuilder();
 
     flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
         builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1472,15 +1382,8 @@
     spline_goal_builder.add_spline_idx(1);
     spline_goal_builder.add_drive_spline_backwards(false);
     spline_goal_builder.add_spline(multispline_offset);
-    flatbuffers::Offset<SplineGoal> spline_goal_offset =
-        spline_goal_builder.Finish();
-
-    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-    goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
-    goal_builder.add_spline(spline_goal_offset);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
   }
-  WaitForTrajectoryPlan();
   RunFor(chrono::milliseconds(2000));
 
   // Start goal
@@ -1489,13 +1392,107 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
     goal_builder.add_spline_handle(1);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
-  RunFor(chrono::milliseconds(2000));
+  WaitForTrajectoryExecution();
 
   VerifyNearPosition(1.0, 1.0);
 }
 
+// Tests that when we send a bunch of splines we can fill up the internal
+// buffers and that we handle that case sanely.
+TEST_F(DrivetrainTest, FillSplineBuffer) {
+  SetEnabled(true);
+  std::vector<int> sent_spline_indices;
+  constexpr size_t kExtraSplines = 4;
+  for (size_t spline_index = 1;
+       spline_index < DrivetrainLoop::kNumSplineFetchers + 1 + kExtraSplines;
+       ++spline_index) {
+    auto builder = trajectory_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+    flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+        builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+    MultiSpline::Builder multispline_builder =
+        builder.MakeBuilder<MultiSpline>();
+
+    multispline_builder.add_spline_count(1);
+    multispline_builder.add_spline_x(spline_x_offset);
+    multispline_builder.add_spline_y(spline_y_offset);
+
+    flatbuffers::Offset<MultiSpline> multispline_offset =
+        multispline_builder.Finish();
+
+    SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+    spline_goal_builder.add_spline_idx(spline_index);
+    spline_goal_builder.add_drive_spline_backwards(false);
+    spline_goal_builder.add_spline(multispline_offset);
+    ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+    RunFor(dt());
+
+    sent_spline_indices.push_back(spline_index);
+  }
+
+  drivetrain_status_fetcher_.Fetch();
+
+  ASSERT_EQ(DrivetrainLoop::kNumSplineFetchers,
+            CHECK_NOTNULL(drivetrain_status_fetcher_.get()
+                              ->trajectory_logging()
+                              ->available_splines())
+                ->size());
+  for (size_t ii = 0; ii < DrivetrainLoop::kNumSplineFetchers; ++ii) {
+    EXPECT_EQ(sent_spline_indices[ii],
+              CHECK_NOTNULL(drivetrain_status_fetcher_.get()
+                                ->trajectory_logging()
+                                ->available_splines())
+                  ->Get(ii));
+  }
+
+  // Next, start going through and executing all the splines; we should
+  // gradually work through the splines.
+  for (size_t ii = 0; ii < sent_spline_indices.size(); ++ii) {
+    const int current_spline = sent_spline_indices[ii];
+    {
+      auto builder = drivetrain_goal_sender_.MakeBuilder();
+      Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+      goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
+      goal_builder.add_spline_handle(current_spline);
+      ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+    }
+
+    // Run for two iterations to give the drivetrain time to resolve all of its
+    // internal state for handling splines (coordinating the fetchers and goal
+    // message to get this two all happen in 1 message is entirely possible, but
+    // has no practical need).
+    RunFor(2 * dt());
+    drivetrain_status_fetcher_.Fetch();
+
+    ASSERT_EQ(current_spline, drivetrain_status_fetcher_.get()
+                                  ->trajectory_logging()
+                                  ->current_spline_idx())
+        << aos::FlatbufferToJson(drivetrain_status_fetcher_.get());
+
+    const int num_available_splines = std::min(
+        sent_spline_indices.size() - ii, DrivetrainLoop::kNumSplineFetchers);
+
+    ASSERT_EQ(num_available_splines,
+              CHECK_NOTNULL(drivetrain_status_fetcher_.get()
+                                ->trajectory_logging()
+                                ->available_splines())
+                  ->size())
+        << aos::FlatbufferToJson(drivetrain_status_fetcher_.get());
+    for (int jj = 0; jj < num_available_splines; ++jj) {
+      EXPECT_EQ(sent_spline_indices[ii + jj],
+                CHECK_NOTNULL(drivetrain_status_fetcher_.get()
+                                  ->trajectory_logging()
+                                  ->available_splines())
+                    ->Get(jj));
+    }
+  }
+}
+
 // The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
 // tests that the integration with drivetrain_lib worked properly.
 TEST_F(DrivetrainTest, BasicLineFollow) {
@@ -1507,7 +1504,7 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
     goal_builder.add_throttle(0.5);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   RunFor(chrono::seconds(5));
@@ -1544,7 +1541,7 @@
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
     goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
     goal_builder.add_throttle(0.5);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   RunFor(chrono::seconds(5));
@@ -1589,7 +1586,7 @@
     goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
     goal_builder.add_left_goal(4.0);
     goal_builder.add_right_goal(4.0);
-    EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
 
   RunFor(chrono::seconds(2));
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 5ce14e6..fe691ed 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -54,21 +54,21 @@
 // For logging information about the state of the trajectory planning.
 table TrajectoryLogging {
   // state of planning the trajectory.
-  planning_state:PlanningState (id: 0);
+  planning_state:PlanningState (id: 0, deprecated);
 
   // State of the spline execution.
   is_executing:bool (id: 1);
   // Whether we have finished the spline specified by current_spline_idx.
   is_executed:bool (id: 2);
 
-  // The handle of the goal spline.  0 means stop requested.
+  // The handle of the goal spline. Empty means no goal/stop requested.
   goal_spline_handle:int (id: 3);
-  // Handle of the executing spline.  -1 means none requested.  If there was no
-  // spline executing when a spline finished optimizing, it will become the
-  // current spline even if we aren't ready to start yet.
+  // Handle of the executing spline. Will generally be identical to
+  // goal_spline_handle if the spline is available; however, if the commanded
+  // spline has not yet been planned, this will be empty.
   current_spline_idx:int (id: 4);
   // Handle of the spline that is being optimized and staged.
-  planning_spline_idx:int (id: 5);
+  planning_spline_idx:int (id: 5, deprecated);
 
   // Expected position and velocity on the spline
   x:float (id: 6);
@@ -77,6 +77,9 @@
   left_velocity:float (id: 9);
   right_velocity:float (id: 10);
   distance_remaining:float (id: 11);
+
+  // Splines that we have full plans for.
+  available_splines:[int] (id: 12);
 }
 
 // For logging state of the line follower.
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 6ea23c6..f275c17 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -121,8 +121,9 @@
 Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
                           int num_distance) {
   return new Trajectory(
-      spline, ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
-      num_distance);
+      DistanceSpline(*spline),
+      ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), nullptr, -1,
+      vmax, num_distance);
 }
 
 void deleteTrajectory(Trajectory *t) { delete t; }
diff --git a/frc971/control_loops/drivetrain/spline_goal.fbs b/frc971/control_loops/drivetrain/spline_goal.fbs
new file mode 100644
index 0000000..3561a18
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline_goal.fbs
@@ -0,0 +1,16 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain;
+
+table SplineGoal {
+  // index of the spline.
+  spline_idx:int (id: 0);
+
+  // Acutal spline.
+  spline:frc971.MultiSpline (id: 1);
+
+  // Whether to follow the spline driving forwards or backwards.
+  drive_spline_backwards:bool (id: 2);
+}
+
+root_type SplineGoal;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 423465a..cdedce0 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -2,6 +2,8 @@
 
 #include "Eigen/Dense"
 
+#include "aos/json_to_flatbuffer.h"
+
 #include "aos/realtime.h"
 #include "aos/util/math.h"
 #include "frc971/control_loops/control_loops_generated.h"
@@ -15,9 +17,10 @@
 namespace drivetrain {
 
 SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
-    : dt_config_(dt_config), new_goal_(&mutex_) {
-  worker_thread_ = std::thread(&SplineDrivetrain::ComputeTrajectory, this);
-}
+    : dt_config_(dt_config),
+      current_xva_(Eigen::Vector3d::Zero()),
+      next_xva_(Eigen::Vector3d::Zero()),
+      next_U_(Eigen::Vector2d::Zero()) {}
 
 void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
   output_was_capped_ =
@@ -28,151 +31,72 @@
   }
 }
 
-void SplineDrivetrain::ComputeTrajectory() {
-  ::aos::SetCurrentThreadRealtimePriority(1);
-
-  ::aos::MutexLocker locker(&mutex_);
-  while (run_) {
-    while (goal_.spline_idx == future_spline_idx_) {
-      AOS_CHECK(!new_goal_.Wait());
-      if (!run_) {
-        return;
-      }
-    }
-    past_distance_spline_.reset();
-    past_trajectory_.reset();
-
-    plan_state_ = PlanningState::BUILDING_TRAJECTORY;
-    future_spline_idx_ = goal_.spline_idx;
-    planning_spline_idx_ = future_spline_idx_;
-    const MultiSpline &multispline = goal_.spline;
-    auto x = multispline.spline_x;
-    auto y = multispline.spline_y;
-    ::std::vector<Spline> splines = ::std::vector<Spline>();
-    for (int i = 0; i < multispline.spline_count; ++i) {
-      ::Eigen::Matrix<double, 2, 6> points =
-          ::Eigen::Matrix<double, 2, 6>::Zero();
-      for (int j = 0; j < 6; ++j) {
-        points(0, j) = x[i * 5 + j];
-        points(1, j) = y[i * 5 + j];
-      }
-      splines.emplace_back(Spline(points));
-    }
-
-    future_drive_spline_backwards_ = goal_.drive_spline_backwards;
-
-    future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
-        new DistanceSpline(::std::move(splines)));
-
-    future_trajectory_ = ::std::unique_ptr<Trajectory>(
-        new Trajectory(future_distance_spline_.get(), dt_config_));
-
-    for (size_t i = 0; i < multispline.constraints.size(); ++i) {
-      const ::frc971::ConstraintT &constraint = multispline.constraints[i];
-      switch (constraint.constraint_type) {
-        case frc971::ConstraintType::CONSTRAINT_TYPE_UNDEFINED:
-          break;
-        case frc971::ConstraintType::LONGITUDINAL_ACCELERATION:
-          future_trajectory_->set_longitudinal_acceleration(constraint.value);
-          break;
-        case frc971::ConstraintType::LATERAL_ACCELERATION:
-          future_trajectory_->set_lateral_acceleration(constraint.value);
-          break;
-        case frc971::ConstraintType::VOLTAGE:
-          future_trajectory_->set_voltage_limit(constraint.value);
-          break;
-        case frc971::ConstraintType::VELOCITY:
-          future_trajectory_->LimitVelocity(constraint.start_distance,
-                                            constraint.end_distance,
-                                            constraint.value);
-          break;
-      }
-    }
-    plan_state_ = PlanningState::PLANNING_TRAJECTORY;
-
-    future_trajectory_->Plan();
-    plan_state_ = PlanningState::PLANNED;
-  }
-}
-
 void SplineDrivetrain::SetGoal(
     const ::frc971::control_loops::drivetrain::Goal *goal) {
-  current_spline_handle_ = goal->spline_handle();
-
-  // If told to stop, set the executing spline to an invalid index and clear out
-  // its plan:
-  if (current_spline_handle_ == 0 &&
-      (goal->spline() == nullptr ||
-       current_spline_idx_ != CHECK_NOTNULL(goal->spline())->spline_idx())) {
-    current_spline_idx_ = -1;
-  }
-
-  ::aos::Mutex::State mutex_state = mutex_.TryLock();
-  if (mutex_state == ::aos::Mutex::State::kLocked) {
-    if (goal->spline() != nullptr && goal->spline()->spline_idx() &&
-        future_spline_idx_ != goal->spline()->spline_idx()) {
-      CHECK_NOTNULL(goal->spline());
-      goal_.spline_idx = goal->spline()->spline_idx();
-      goal_.drive_spline_backwards = goal->spline()->drive_spline_backwards();
-
-      const frc971::MultiSpline *multispline = goal->spline()->spline();
-      CHECK_NOTNULL(multispline);
-
-      goal_.spline.spline_count = multispline->spline_count();
-
-      CHECK_EQ(multispline->spline_x()->size(),
-               static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
-      CHECK_EQ(multispline->spline_y()->size(),
-               static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
-
-      std::copy(multispline->spline_x()->begin(),
-                multispline->spline_x()->end(), goal_.spline.spline_x.begin());
-      std::copy(multispline->spline_y()->begin(),
-                multispline->spline_y()->end(), goal_.spline.spline_y.begin());
-
-      for (size_t i = 0; i < 6; ++i) {
-        if (multispline->constraints() != nullptr &&
-            i < multispline->constraints()->size()) {
-          multispline->constraints()->Get(i)->UnPackTo(
-              &goal_.spline.constraints[i]);
-        } else {
-          goal_.spline.constraints[i].constraint_type =
-              ConstraintType::CONSTRAINT_TYPE_UNDEFINED;
-        }
-      }
-
-      new_goal_.Broadcast();
-      if (current_spline_handle_ != current_spline_idx_) {
-        // If we aren't going to actively execute the current spline, evict it's
-        // plan.
-        past_trajectory_ = std::move(current_trajectory_);
-        past_distance_spline_ = std::move(current_distance_spline_);
-      }
-    }
-    // If you never started executing the previous spline, you're screwed.
-    if (future_trajectory_ &&
-        (!current_trajectory_ ||
-         current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) ||
-         current_spline_idx_ == -1)) {
-      // Move current data to other variables to be cleared by worker.
-      past_trajectory_ = std::move(current_trajectory_);
-      past_distance_spline_ = std::move(current_distance_spline_);
-
-      // Move the computed data to be executed.
-      current_trajectory_ = std::move(future_trajectory_);
-      current_distance_spline_ = std::move(future_distance_spline_);
-      current_drive_spline_backwards_ = future_drive_spline_backwards_;
-      current_spline_idx_ = future_spline_idx_;
-
-      // Reset internal state to a trajectory start position.
-      current_xva_ = current_trajectory_->FFAcceleration(0);
-      current_xva_(1) = 0.0;
-      has_started_execution_ = false;
-    }
-    mutex_.Unlock();
+  if (goal->has_spline_handle()) {
+    commanded_spline_ = goal->spline_handle();
   } else {
-    VLOG(1) << "Failed to acquire trajectory lock.";
+    commanded_spline_.reset();
   }
+  UpdateSplineHandles();
+}
+
+bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
+  if (trajectory == nullptr) {
+    return false;
+  }
+  for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+    if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
+  trajectories_.emplace_back(
+      std::make_unique<FinishedTrajectory>(dt_config_, trajectory));
+  UpdateSplineHandles();
+}
+
+void SplineDrivetrain::DeleteCurrentSpline() {
+  CHECK(current_trajectory_index_);
+  CHECK_LT(*current_trajectory_index_, trajectories_.size());
+  trajectories_.erase(trajectories_.begin() + *current_trajectory_index_);
+  executing_spline_ = false;
+  current_trajectory_index_.reset();
+  current_xva_.setZero();
+}
+
+void SplineDrivetrain::UpdateSplineHandles() {
+  // If we are currently executing a spline and have received a change
+  if (executing_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_) {
+        // If we are executing a spline, and the handle has changed, garbage
+        // collect the old spline.
+        DeleteCurrentSpline();
+      }
+    }
+  }
+  // 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_) {
+      executing_spline_ = true;
+      current_trajectory_index_ = ii;
+    }
+  }
+  // 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.
 }
 
 // TODO(alex): Hold position when done following the spline.
@@ -181,20 +105,19 @@
     const ::Eigen::Matrix<double, 2, 1> &voltage_error) {
   next_U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
   enable_ = enable;
-  if (enable && current_trajectory_) {
+  if (enable && executing_spline_) {
     ::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
-    if (!IsAtEnd() && current_spline_handle_ == current_spline_idx_) {
-      has_started_execution_ = true;
+    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 = current_trajectory().FFVoltage(current_xva_(0));
     }
 
     const double current_distance = current_xva_(0);
     ::Eigen::Matrix<double, 2, 5> K =
-        current_trajectory_->GainForDistance(current_distance);
+        current_trajectory().GainForDistance(current_distance);
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
-    if (current_drive_spline_backwards_) {
+    if (current_trajectory().drive_spline_backwards()) {
       ::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
       U_ff = -swapU;
       goal_state(2, 0) += M_PI;
@@ -204,16 +127,16 @@
       goal_state(4, 0) = -left_goal;
     }
     const Eigen::Matrix<double, 5, 1> relative_goal =
-        current_trajectory_->StateToPathRelativeState(current_distance,
+        current_trajectory().StateToPathRelativeState(current_distance,
                                                       goal_state);
     const Eigen::Matrix<double, 5, 1> relative_state =
-        current_trajectory_->StateToPathRelativeState(current_distance, state);
+        current_trajectory().StateToPathRelativeState(current_distance, state);
     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;
 
     ::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_ = current_trajectory().GetNextXVA(dt_config_.dt, &xv_state);
     next_U_ = U_ff + U_fb - voltage_error;
     uncapped_U_ = next_U_;
     ScaleCapU(&next_U_);
@@ -225,7 +148,7 @@
   if (!output) {
     return;
   }
-  if (current_spline_handle_ == current_spline_idx_) {
+  if (executing_spline_) {
     if (!IsAtEnd()) {
       output->left_voltage = next_U_(0);
       output->right_voltage = next_U_(1);
@@ -248,33 +171,39 @@
 
 flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
     flatbuffers::FlatBufferBuilder *builder) const {
+  int *spline_handles;
+  const flatbuffers::Offset<flatbuffers::Vector<int>> handles_vector =
+      builder->CreateUninitializedVector(trajectories_.size(), &spline_handles);
+
+  for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+    spline_handles[ii] = trajectories_[ii]->spline_handle();
+  }
+
   drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
-  if (current_distance_spline_) {
+  if (executing_spline_) {
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
     trajectory_logging_builder.add_x(goal_state(0));
     trajectory_logging_builder.add_y(goal_state(1));
     trajectory_logging_builder.add_theta(::aos::math::NormalizeAngle(
-        goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0)));
+        goal_state(2) +
+        (current_trajectory().drive_spline_backwards() ? M_PI : 0.0)));
     trajectory_logging_builder.add_left_velocity(goal_state(3));
     trajectory_logging_builder.add_right_velocity(goal_state(4));
   }
-  trajectory_logging_builder.add_planning_state(plan_state_.load());
   trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
-                                              has_started_execution_);
-  trajectory_logging_builder.add_is_executed((current_spline_idx_ != -1) &&
-                                             IsAtEnd());
-  trajectory_logging_builder.add_goal_spline_handle(current_spline_handle_);
-  trajectory_logging_builder.add_current_spline_idx(current_spline_idx_);
-  trajectory_logging_builder.add_distance_remaining(
-      current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
-                          : 0.0);
-
-  int32_t planning_spline_idx = planning_spline_idx_;
-  if (current_spline_idx_ == planning_spline_idx) {
-    trajectory_logging_builder.add_planning_spline_idx(0);
-  } else {
-    trajectory_logging_builder.add_planning_spline_idx(planning_spline_idx_);
+                                              executing_spline_);
+  trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
+  if (commanded_spline_) {
+    trajectory_logging_builder.add_goal_spline_handle(*commanded_spline_);
+    if (executing_spline_) {
+      trajectory_logging_builder.add_current_spline_idx(*commanded_spline_);
+    }
   }
+  trajectory_logging_builder.add_distance_remaining(
+      executing_spline_ ? 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 2ceaac6..0170151 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -25,16 +25,13 @@
  public:
   SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
 
-  ~SplineDrivetrain() {
-    {
-      ::aos::MutexLocker locker(&mutex_);
-      run_ = false;
-      new_goal_.Signal();
-    }
-    worker_thread_.join();
-  }
-
   void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
+  // Note that the user maintains ownership of the trajectory flatbuffer for the
+  // entire time; once AddTrajectory() is called, the trajectory pointer must
+  // stay valid until the spline has finished executing and HasTrajectory()
+  // returns false.
+  bool HasTrajectory(const fb::Trajectory *trajectory) const;
+  void AddTrajectory(const fb::Trajectory *trajectory);
 
   void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state,
               const ::Eigen::Matrix<double, 2, 1> &voltage_error);
@@ -50,40 +47,54 @@
   // Accessor for the current goal state, pretty much only present for debugging
   // purposes.
   ::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
-    return current_trajectory_ ? current_trajectory_->GoalState(current_xva_(0),
-                                                                current_xva_(1))
-                               : ::Eigen::Matrix<double, 5, 1>::Zero();
+    return executing_spline_ ? current_trajectory().GoalState(current_xva_(0),
+                                                              current_xva_(1))
+                             : ::Eigen::Matrix<double, 5, 1>::Zero();
   }
 
   bool IsAtEnd() const {
-    return current_trajectory_
-               ? current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0))
+    return executing_spline_
+               ? current_trajectory().is_at_end(current_xva_.block<2, 1>(0, 0))
                : true;
   }
 
   // Returns true if the splinedrivetrain is enabled.
   bool enable() const { return enable_; }
 
-  enum class PlanState : int8_t {
-    kNoPlan = 0,
-    kBuildingTrajectory = 1,
-    kPlanningTrajectory = 2,
-    kPlannedTrajectory = 3,
-  };
-
  private:
-  void ComputeTrajectory();
   void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
 
+  // 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
+  // spline, and if there is any trajectory in the list of trajectories matching
+  // 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();
+
+  // Deletes the currently executing trajectory.
+  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_];
+  }
+
   const DrivetrainConfig<double> dt_config_;
 
-  int32_t current_spline_handle_ = 0;  // Current spline told to excecute.
-  int32_t current_spline_idx_ = 0;     // Current executing spline.
-  bool has_started_execution_ = false;
+  bool executing_spline_ = false;
 
-  ::std::unique_ptr<DistanceSpline> current_distance_spline_;
-  ::std::unique_ptr<Trajectory> current_trajectory_;
-  bool current_drive_spline_backwards_ = false;
+  // 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_;
+
+  std::optional<int> commanded_spline_;
 
   // State required to compute the next iteration's output.
   ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
@@ -93,44 +104,6 @@
   ::Eigen::Matrix<double, 2, 1> uncapped_U_;
   bool enable_ = false;
   bool output_was_capped_ = false;
-
-  std::atomic<PlanningState> plan_state_ = {PlanningState::NO_PLAN};
-
-  ::std::thread worker_thread_;
-  // mutex_ is held by the worker thread while it is doing work or by the main
-  // thread when it is sending work to the worker thread.
-  ::aos::Mutex mutex_;
-  // new_goal_ is used to signal to the worker thread that ther is work to do.
-  ::aos::Condition new_goal_;
-  // The following variables are guarded by mutex_.
-  bool run_ = true;
-
-  // These two structures mirror the flatbuffer Multispline.
-  // TODO(austin): copy the goal flatbuffer directly instead of recreating it
-  // like this...
-  struct MultiSpline {
-    int32_t spline_count;
-    std::array<float, 36> spline_x;
-    std::array<float, 36> spline_y;
-    std::array<ConstraintT, 6> constraints;
-  };
-
-  struct SplineGoal {
-    int32_t spline_idx = 0;
-
-    bool drive_spline_backwards;
-
-    MultiSpline spline;
-  };
-
-  SplineGoal goal_;
-  ::std::unique_ptr<DistanceSpline> past_distance_spline_;
-  ::std::unique_ptr<DistanceSpline> future_distance_spline_;
-  ::std::unique_ptr<Trajectory> past_trajectory_;
-  ::std::unique_ptr<Trajectory> future_trajectory_;
-  bool future_drive_spline_backwards_ = false;
-  int32_t future_spline_idx_ = 0;  // Current spline being computed.
-  ::std::atomic<int32_t> planning_spline_idx_{-1};
 };
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 9c32483..08dd3fd 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -14,14 +14,73 @@
 namespace control_loops {
 namespace drivetrain {
 
-Trajectory::Trajectory(const DistanceSpline *spline,
-                       const DrivetrainConfig<double> &config, double vmax,
-                       int num_distance)
-    : spline_(spline),
-      velocity_drivetrain_(
-          ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
-                                              StateFeedbackHybridPlant<2, 2, 2>,
-                                              HybridKalman<2, 2, 2>>>(
+namespace {
+float DefaultConstraint(ConstraintType type) {
+  switch (type) {
+    case ConstraintType::LONGITUDINAL_ACCELERATION:
+      return 2.0;
+    case ConstraintType::LATERAL_ACCELERATION:
+      return 3.0;
+    case ConstraintType::VOLTAGE:
+      return 12.0;
+    case ConstraintType::VELOCITY:
+    case ConstraintType::CONSTRAINT_TYPE_UNDEFINED:
+      LOG(FATAL) << "No default constraint value for "
+                 << EnumNameConstraintType(type);
+  }
+  LOG(FATAL) << "Invalid ConstraintType " << static_cast<int>(type);
+}
+}  // namespace
+
+FinishedTrajectory::FinishedTrajectory(const DrivetrainConfig<double> &config,
+                                       const fb::Trajectory *buffer)
+    : BaseTrajectory(CHECK_NOTNULL(CHECK_NOTNULL(buffer->spline())->spline())
+                         ->constraints(),
+                     config),
+      buffer_(buffer),
+      spline_(*buffer_->spline()) {}
+
+const Eigen::Matrix<double, 2, 1> BaseTrajectory::K1(
+    double current_ddtheta) const {
+  return (Eigen::Matrix<double, 2, 1>() << -robot_radius_l_ * current_ddtheta,
+          robot_radius_r_ * current_ddtheta)
+      .finished();
+}
+
+const Eigen::Matrix<double, 2, 1> BaseTrajectory::K2(
+    double current_dtheta) const {
+  return (Eigen::Matrix<double, 2, 1>()
+              << 1.0 - robot_radius_l_ * current_dtheta,
+          1.0 + robot_radius_r_ * current_dtheta)
+      .finished();
+}
+
+void BaseTrajectory::K345(const double x, Eigen::Matrix<double, 2, 1> *K3,
+                          Eigen::Matrix<double, 2, 1> *K4,
+                          Eigen::Matrix<double, 2, 1> *K5) const {
+  const double current_ddtheta = spline().DDTheta(x);
+  const double current_dtheta = spline().DTheta(x);
+  // We've now got the equation:
+  //     K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
+  const Eigen::Matrix<double, 2, 1> my_K2 = K2(current_dtheta);
+
+  const Eigen::Matrix<double, 2, 2> B_inverse =
+      velocity_drivetrain_->plant().coefficients().B_continuous.inverse();
+
+  // Now, rephrase it as K5 a + K3 v^2 + K4 v = U
+  *K3 = B_inverse * K1(current_ddtheta);
+  *K4 = -B_inverse * velocity_drivetrain_->plant().coefficients().A_continuous *
+        my_K2;
+  *K5 = B_inverse * my_K2;
+}
+
+BaseTrajectory::BaseTrajectory(
+    const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+    const DrivetrainConfig<double> &config)
+    : velocity_drivetrain_(
+          std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
+                                            StateFeedbackHybridPlant<2, 2, 2>,
+                                            HybridKalman<2, 2, 2>>>(
               new StateFeedbackLoop<2, 2, 2, double,
                                     StateFeedbackHybridPlant<2, 2, 2>,
                                     HybridKalman<2, 2, 2>>(
@@ -29,18 +88,43 @@
       config_(config),
       robot_radius_l_(config.robot_radius),
       robot_radius_r_(config.robot_radius),
-      longitudinal_acceleration_(3.0),
-      lateral_acceleration_(2.0),
-      Tlr_to_la_((::Eigen::Matrix<double, 2, 2>() << 0.5, 0.5,
-                  -1.0 / (robot_radius_l_ + robot_radius_r_),
-                  1.0 / (robot_radius_l_ + robot_radius_r_))
-                     .finished()),
-      Tla_to_lr_(Tlr_to_la_.inverse()),
+      lateral_acceleration_(
+          ConstraintValue(constraints, ConstraintType::LATERAL_ACCELERATION)),
+      longitudinal_acceleration_(ConstraintValue(
+          constraints, ConstraintType::LONGITUDINAL_ACCELERATION)),
+      voltage_limit_(ConstraintValue(constraints, ConstraintType::VOLTAGE)) {}
+
+Trajectory::Trajectory(const SplineGoal &spline_goal,
+                       const DrivetrainConfig<double> &config)
+    : Trajectory(DistanceSpline{spline_goal.spline()}, config,
+                 spline_goal.spline()->constraints(),
+                 spline_goal.spline_idx()) {
+  drive_spline_backwards_ = spline_goal.drive_spline_backwards();
+}
+
+Trajectory::Trajectory(
+    DistanceSpline &&input_spline, const DrivetrainConfig<double> &config,
+    const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+    int spline_idx, double vmax, int num_distance)
+    : BaseTrajectory(constraints, config),
+      spline_idx_(spline_idx),
+      spline_(std::move(input_spline)),
+      config_(config),
       plan_(num_distance == 0
-                ? ::std::max(100, static_cast<int>(spline_->length() / 0.0025))
+                ? std::max(100, static_cast<int>(spline_.length() / 0.0025))
                 : num_distance,
             vmax),
-      plan_segment_type_(plan_.size(), SegmentType::VELOCITY_LIMITED) {}
+      plan_segment_type_(plan_.size(),
+                         fb::SegmentConstraint::VELOCITY_LIMITED) {
+  if (constraints != nullptr) {
+    for (const Constraint *constraint : *constraints) {
+      if (constraint->constraint_type() == ConstraintType::VELOCITY) {
+        LimitVelocity(constraint->start_distance(), constraint->end_distance(),
+                      constraint->value());
+      }
+    }
+  }
+}
 
 void Trajectory::LateralAccelPass() {
   for (size_t i = 0; i < plan_.size(); ++i) {
@@ -48,7 +132,7 @@
     const double velocity_limit = LateralVelocityCurvature(distance);
     if (velocity_limit < plan_[i]) {
       plan_[i] = velocity_limit;
-      plan_segment_type_[i] = CURVATURE_LIMITED;
+      plan_segment_type_[i] = fb::SegmentConstraint::CURVATURE_LIMITED;
     }
   }
 }
@@ -59,29 +143,30 @@
     const double velocity_limit = VoltageVelocityLimit(distance, limit_type);
     if (velocity_limit < plan_[i]) {
       plan_[i] = velocity_limit;
-      plan_segment_type_[i] = VOLTAGE_LIMITED;
+      plan_segment_type_[i] = fb::SegmentConstraint::VOLTAGE_LIMITED;
     }
   }
 }
 
-double Trajectory::BestAcceleration(double x, double v, bool backwards) {
-  ::Eigen::Matrix<double, 2, 1> K3;
-  ::Eigen::Matrix<double, 2, 1> K4;
-  ::Eigen::Matrix<double, 2, 1> K5;
+double BaseTrajectory::BestAcceleration(double x, double v,
+                                        bool backwards) const {
+  Eigen::Matrix<double, 2, 1> K3;
+  Eigen::Matrix<double, 2, 1> K4;
+  Eigen::Matrix<double, 2, 1> K5;
   K345(x, &K3, &K4, &K5);
 
   // Now, solve for all a's and find the best one which meets our criteria.
-  const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
-  double min_voltage_accel = ::std::numeric_limits<double>::infinity();
+  const Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
+  double min_voltage_accel = std::numeric_limits<double>::infinity();
   double max_voltage_accel = -min_voltage_accel;
-  for (const double a : {(voltage_limit_ - C(0, 0)) / K5(0, 0),
-                         (voltage_limit_ - C(1, 0)) / K5(1, 0),
-                         (-voltage_limit_ - C(0, 0)) / K5(0, 0),
-                         (-voltage_limit_ - C(1, 0)) / K5(1, 0)}) {
-    const ::Eigen::Matrix<double, 2, 1> U = K5 * a + K3 * v * v + K4 * v;
-    if ((U.array().abs() < voltage_limit_ + 1e-6).all()) {
-      min_voltage_accel = ::std::min(a, min_voltage_accel);
-      max_voltage_accel = ::std::max(a, max_voltage_accel);
+  for (const double a : {(max_voltage() - C(0, 0)) / K5(0, 0),
+                         (max_voltage() - C(1, 0)) / K5(1, 0),
+                         (-max_voltage() - C(0, 0)) / K5(0, 0),
+                         (-max_voltage() - C(1, 0)) / K5(1, 0)}) {
+    const Eigen::Matrix<double, 2, 1> U = K5 * a + K3 * v * v + K4 * v;
+    if ((U.array().abs() < max_voltage() + 1e-6).all()) {
+      min_voltage_accel = std::min(a, min_voltage_accel);
+      max_voltage_accel = std::max(a, max_voltage_accel);
     }
   }
   double best_accel = backwards ? min_voltage_accel : max_voltage_accel;
@@ -89,9 +174,9 @@
   double min_friction_accel, max_friction_accel;
   FrictionLngAccelLimits(x, v, &min_friction_accel, &max_friction_accel);
   if (backwards) {
-    best_accel = ::std::max(best_accel, min_friction_accel);
+    best_accel = std::max(best_accel, min_friction_accel);
   } else {
-    best_accel = ::std::min(best_accel, max_friction_accel);
+    best_accel = std::min(best_accel, max_friction_accel);
   }
 
   // Ideally, the max would never be less than the min, but due to the way that
@@ -117,7 +202,7 @@
   return best_accel;
 }
 
-double Trajectory::LateralVelocityCurvature(double distance) const {
+double BaseTrajectory::LateralVelocityCurvature(double distance) const {
   // To calculate these constraints, we first note that:
   // wheel accels = K2 * v_robot' + K1 * v_robot^2
   // All that this logic does is solve for v_robot, leaving v_robot' free,
@@ -139,37 +224,37 @@
   // 3) Solving the relatively tractable remaining equation, which is
   // basically just grouping all the terms together in one spot and taking the
   // 4th root of everything.
-  const double dtheta = spline_->DTheta(distance);
-  const ::Eigen::Matrix<double, 1, 2> K2prime =
+  const double dtheta = spline().DTheta(distance);
+  const Eigen::Matrix<double, 1, 2> K2prime =
       K2(dtheta).transpose() *
-      (::Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
+      (Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
   // Calculate whether the wheels are spinning in opposite directions.
   const bool opposites = K2prime(0) * K2prime(1) < 0;
-  const ::Eigen::Matrix<double, 2, 1> K1calc = K1(spline_->DDTheta(distance));
-  const double lat_accel_squared =
-      ::std::pow(dtheta / lateral_acceleration_, 2);
+  const Eigen::Matrix<double, 2, 1> K1calc = K1(spline().DDTheta(distance));
+  const double lat_accel_squared = std::pow(dtheta / max_lateral_accel(), 2);
   const double curvature_change_term =
       (K2prime * K1calc).value() /
       (K2prime *
-       (::Eigen::Matrix<double, 2, 1>() << 1.0, (opposites ? -1.0 : 1.0))
+       (Eigen::Matrix<double, 2, 1>() << 1.0, (opposites ? -1.0 : 1.0))
            .finished() *
-       longitudinal_acceleration_)
+       max_longitudinal_accel())
           .value();
-  const double vel_inv = ::std::sqrt(
-      ::std::sqrt(::std::pow(curvature_change_term, 2) + lat_accel_squared));
+  const double vel_inv = std::sqrt(
+      std::sqrt(std::pow(curvature_change_term, 2) + lat_accel_squared));
   if (vel_inv == 0.0) {
-    return ::std::numeric_limits<double>::infinity();
+    return std::numeric_limits<double>::infinity();
   }
   return 1.0 / vel_inv;
 }
 
-void Trajectory::FrictionLngAccelLimits(double x, double v, double *min_accel,
-                                        double *max_accel) const {
+void BaseTrajectory::FrictionLngAccelLimits(double x, double v,
+                                            double *min_accel,
+                                            double *max_accel) const {
   // First, calculate the max longitudinal acceleration that can be achieved
   // by either wheel given the friction elliipse that we have.
-  const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
+  const double lateral_acceleration = v * v * spline().DDXY(x).norm();
   const double max_wheel_lng_accel_squared =
-      1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_, 2.0);
+      1.0 - std::pow(lateral_acceleration / max_lateral_accel(), 2.0);
   if (max_wheel_lng_accel_squared < 0.0) {
     VLOG(1) << "Something (probably Runge-Kutta) queried invalid velocity " << v
             << " at distance " << x;
@@ -179,45 +264,45 @@
     // how min_accel > max_accel if we reach this case) to avoid causing any
     // numerical issues.
     *min_accel =
-        ::std::sqrt(-max_wheel_lng_accel_squared) * longitudinal_acceleration_;
+        std::sqrt(-max_wheel_lng_accel_squared) * max_longitudinal_accel();
     *max_accel = -*min_accel;
     return;
   }
-  *min_accel = -::std::numeric_limits<double>::infinity();
-  *max_accel = ::std::numeric_limits<double>::infinity();
+  *min_accel = -std::numeric_limits<double>::infinity();
+  *max_accel = std::numeric_limits<double>::infinity();
 
   // Calculate max/min accelerations by calculating what the robots overall
   // longitudinal acceleration would be if each wheel were running at the max
   // forwards/backwards longitudinal acceleration.
   const double max_wheel_lng_accel =
-      longitudinal_acceleration_ * ::std::sqrt(max_wheel_lng_accel_squared);
-  const ::Eigen::Matrix<double, 2, 1> K1v2 = K1(spline_->DDTheta(x)) * v * v;
-  const ::Eigen::Matrix<double, 2, 1> K2inv =
-      K2(spline_->DTheta(x)).cwiseInverse();
+      max_longitudinal_accel() * std::sqrt(max_wheel_lng_accel_squared);
+  const Eigen::Matrix<double, 2, 1> K1v2 = K1(spline().DDTheta(x)) * v * v;
+  const Eigen::Matrix<double, 2, 1> K2inv =
+      K2(spline().DTheta(x)).cwiseInverse();
   // Store the accelerations of the robot corresponding to each wheel being at
   // the max/min acceleration. The first coefficient in each vector
   // corresponds to the left wheel, the second to the right wheel.
-  const ::Eigen::Matrix<double, 2, 1> accels1 =
+  const Eigen::Matrix<double, 2, 1> accels1 =
       K2inv.array() * (-K1v2.array() + max_wheel_lng_accel);
-  const ::Eigen::Matrix<double, 2, 1> accels2 =
+  const Eigen::Matrix<double, 2, 1> accels2 =
       K2inv.array() * (-K1v2.array() - max_wheel_lng_accel);
 
   // If either term is non-finite, that suggests that a term of K2 is zero
   // (which is physically possible when turning such that one wheel is
   // stationary), so just ignore that side of the drivetrain.
-  if (::std::isfinite(accels1(0))) {
+  if (std::isfinite(accels1(0))) {
     // The inner max/min in this case determines which of the two cases (+ or
     // - acceleration on the left wheel) we care about--in a sufficiently
     // tight turning radius, the left hweel may be accelerating backwards when
     // the robot as a whole accelerates forwards. We then use that
     // acceleration to bound the min/max accel.
-    *min_accel = ::std::max(*min_accel, ::std::min(accels1(0), accels2(0)));
-    *max_accel = ::std::min(*max_accel, ::std::max(accels1(0), accels2(0)));
+    *min_accel = std::max(*min_accel, std::min(accels1(0), accels2(0)));
+    *max_accel = std::min(*max_accel, std::max(accels1(0), accels2(0)));
   }
   // Same logic as previous if-statement, but for the right wheel.
-  if (::std::isfinite(accels1(1))) {
-    *min_accel = ::std::max(*min_accel, ::std::min(accels1(1), accels2(1)));
-    *max_accel = ::std::min(*max_accel, ::std::max(accels1(1), accels2(1)));
+  if (std::isfinite(accels1(1))) {
+    *min_accel = std::max(*min_accel, std::min(accels1(1), accels2(1)));
+    *max_accel = std::min(*max_accel, std::max(accels1(1), accels2(1)));
   }
 }
 
@@ -254,28 +339,28 @@
   // K2 * v_robot' + K1 * v_robot^2 = A * K2 * v_robot +/- B * U_max
   // K2prime * K1 * v_robot^2 = K2prime * (A * K2 * v_robot +/- B * U_max)
   // a v_robot^2 + b v_robot +/- c = 0
-  const ::Eigen::Matrix<double, 2, 2> B =
-      velocity_drivetrain_->plant().coefficients().B_continuous;
-  const double dtheta = spline_->DTheta(distance);
-  const ::Eigen::Matrix<double, 2, 1> BinvK2 = B.inverse() * K2(dtheta);
+  const Eigen::Matrix<double, 2, 2> B =
+      velocity_drivetrain().plant().coefficients().B_continuous;
+  const double dtheta = spline().DTheta(distance);
+  const Eigen::Matrix<double, 2, 1> BinvK2 = B.inverse() * K2(dtheta);
   // Because voltages can actually impact *both* wheels, in order to determine
   // whether the voltages will have opposite signs, we need to use B^-1 * K2.
   const bool opposite_voltages = BinvK2(0) * BinvK2(1) > 0.0;
-  const ::Eigen::Matrix<double, 1, 2> K2prime =
+  const Eigen::Matrix<double, 1, 2> K2prime =
       K2(dtheta).transpose() *
-      (::Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
-  const double a = K2prime * K1(spline_->DDTheta(distance));
+      (Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
+  const double a = K2prime * K1(spline().DDTheta(distance));
   const double b = -K2prime *
-                   velocity_drivetrain_->plant().coefficients().A_continuous *
+                   velocity_drivetrain().plant().coefficients().A_continuous *
                    K2(dtheta);
-  const ::Eigen::Matrix<double, 1, 2> c_coeff = -K2prime * B;
+  const Eigen::Matrix<double, 1, 2> c_coeff = -K2prime * B;
   // Calculate the "positive" version of the voltage limits we will use.
-  const ::Eigen::Matrix<double, 2, 1> abs_volts =
-      voltage_limit_ *
-      (::Eigen::Matrix<double, 2, 1>() << 1.0, (opposite_voltages ? -1.0 : 1.0))
+  const Eigen::Matrix<double, 2, 1> abs_volts =
+      max_voltage() *
+      (Eigen::Matrix<double, 2, 1>() << 1.0, (opposite_voltages ? -1.0 : 1.0))
           .finished();
 
-  double min_valid_vel = ::std::numeric_limits<double>::infinity();
+  double min_valid_vel = std::numeric_limits<double>::infinity();
   if (limit_type == VoltageLimit::kAggressive) {
     min_valid_vel = 0.0;
   }
@@ -283,7 +368,7 @@
   // formula. For every positive solution, adjust the velocity limit
   // appropriately.
   for (const double sign : {1.0, -1.0}) {
-    const ::Eigen::Matrix<double, 2, 1> U = sign * abs_volts;
+    const Eigen::Matrix<double, 2, 1> U = sign * abs_volts;
     const double prev_vel = min_valid_vel;
     const double c = c_coeff * U;
     const double determinant = b * b - 4 * a * c;
@@ -295,40 +380,40 @@
       if (-b * c > 0.0) {
         const double vel = -c / b;
         if (limit_type == VoltageLimit::kConservative) {
-          min_valid_vel = ::std::min(min_valid_vel, vel);
+          min_valid_vel = std::min(min_valid_vel, vel);
         } else {
-          min_valid_vel = ::std::max(min_valid_vel, vel);
+          min_valid_vel = std::max(min_valid_vel, vel);
         }
       } else if (b == 0) {
         // If a and b are zero, then we are travelling in a straight line and
         // have no voltage-based velocity constraints.
-        min_valid_vel = ::std::numeric_limits<double>::infinity();
+        min_valid_vel = std::numeric_limits<double>::infinity();
       }
     } else if (determinant > 0) {
-      const double sqrt_determinant = ::std::sqrt(determinant);
+      const double sqrt_determinant = std::sqrt(determinant);
       const double high_vel = (-b + sqrt_determinant) / (2.0 * a);
       const double low_vel = (-b - sqrt_determinant) / (2.0 * a);
       if (low_vel > 0) {
         if (limit_type == VoltageLimit::kConservative) {
-          min_valid_vel = ::std::min(min_valid_vel, low_vel);
+          min_valid_vel = std::min(min_valid_vel, low_vel);
         } else {
-          min_valid_vel = ::std::max(min_valid_vel, low_vel);
+          min_valid_vel = std::max(min_valid_vel, low_vel);
         }
       }
       if (high_vel > 0) {
         if (limit_type == VoltageLimit::kConservative) {
-          min_valid_vel = ::std::min(min_valid_vel, high_vel);
+          min_valid_vel = std::min(min_valid_vel, high_vel);
         } else {
-          min_valid_vel = ::std::max(min_valid_vel, high_vel);
+          min_valid_vel = std::max(min_valid_vel, high_vel);
         }
       }
     } else if (determinant == 0 && -b * a > 0) {
       const double vel = -b / (2.0 * a);
       if (vel > 0.0) {
         if (limit_type == VoltageLimit::kConservative) {
-          min_valid_vel = ::std::min(min_valid_vel, vel);
+          min_valid_vel = std::min(min_valid_vel, vel);
         } else {
-          min_valid_vel = ::std::max(min_valid_vel, vel);
+          min_valid_vel = std::max(min_valid_vel, vel);
         }
       }
     }
@@ -352,7 +437,7 @@
 
     if (new_plan_velocity <= plan_[i + 1]) {
       plan_[i + 1] = new_plan_velocity;
-      plan_segment_type_[i] = SegmentType::ACCELERATION_LIMITED;
+      plan_segment_type_[i] = fb::SegmentConstraint::ACCELERATION_LIMITED;
     }
   }
 }
@@ -370,12 +455,13 @@
 
     if (new_plan_velocity <= plan_[i - 1]) {
       plan_[i - 1] = new_plan_velocity;
-      plan_segment_type_[i - 1] = SegmentType::DECELERATION_LIMITED;
+      plan_segment_type_[i - 1] = fb::SegmentConstraint::DECELERATION_LIMITED;
     }
   }
 }
 
-::Eigen::Matrix<double, 3, 1> Trajectory::FFAcceleration(double distance) {
+Eigen::Matrix<double, 3, 1> BaseTrajectory::FFAcceleration(
+    double distance) const {
   if (distance < 0.0) {
     // Make sure we don't end up off the beginning of the curve.
     distance = 0.0;
@@ -384,7 +470,8 @@
     distance = length();
   }
   const size_t before_index = DistanceToSegment(distance);
-  const size_t after_index = std::min(before_index + 1, plan_.size() - 1);
+  const size_t after_index =
+      std::min(before_index + 1, distance_plan_size() - 1);
 
   const double before_distance = Distance(before_index);
   const double after_distance = Distance(after_index);
@@ -397,67 +484,85 @@
   // TODO(james): While technically correct for sufficiently small segment
   // steps, this method of switching between limits has a tendency to produce
   // sudden jumps in acceelrations, which is undesirable.
-  switch (plan_segment_type_[DistanceToSegment(distance)]) {
-    case SegmentType::VELOCITY_LIMITED:
+  switch (plan_constraint(DistanceToSegment(distance))) {
+    case fb::SegmentConstraint::VELOCITY_LIMITED:
       acceleration = 0.0;
-      velocity = (plan_[before_index] + plan_[after_index]) / 2.0;
+      velocity =
+          (plan_velocity(before_index) + plan_velocity(after_index)) / 2.0;
       // TODO(austin): Accelerate or decelerate until we hit the limit in the
       // time slice.  Otherwise our acceleration will be lying for this slice.
       // Do note, we've got small slices so the effect will be small.
       break;
-    case SegmentType::CURVATURE_LIMITED:
+    case fb::SegmentConstraint::CURVATURE_LIMITED:
       velocity = vcurvature;
       FrictionLngAccelLimits(distance, velocity, &acceleration, &acceleration);
       break;
-    case SegmentType::VOLTAGE_LIMITED:
+    case fb::SegmentConstraint::VOLTAGE_LIMITED:
       // Normally, we expect that voltage limited plans will all get dominated
       // by the acceleration/deceleration limits. This may not always be true;
       // if we ever encounter this error, we just need to back out what the
       // accelerations would be in this case.
       LOG(FATAL) << "Unexpectedly got VOLTAGE_LIMITED plan.";
       break;
-    case SegmentType::ACCELERATION_LIMITED:
+    case fb::SegmentConstraint::ACCELERATION_LIMITED:
       // TODO(james): The integration done here and in the DECELERATION_LIMITED
       // can technically cause us to violate friction constraints. We currently
       // don't do anything about it to avoid causing sudden jumps in voltage,
       // but we probably *should* at some point.
       velocity = IntegrateAccelForDistance(
           [this](double x, double v) { return ForwardAcceleration(x, v); },
-          plan_[before_index], before_distance, distance - before_distance);
+          plan_velocity(before_index), before_distance,
+          distance - before_distance);
       acceleration = ForwardAcceleration(distance, velocity);
       break;
-    case SegmentType::DECELERATION_LIMITED:
+    case fb::SegmentConstraint::DECELERATION_LIMITED:
       velocity = IntegrateAccelForDistance(
           [this](double x, double v) { return BackwardAcceleration(x, v); },
-          plan_[after_index], after_distance, distance - after_distance);
+          plan_velocity(after_index), after_distance,
+          distance - after_distance);
       acceleration = BackwardAcceleration(distance, velocity);
       break;
     default:
-      AOS_LOG(
-          FATAL, "Unknown segment type %d\n",
-          static_cast<int>(plan_segment_type_[DistanceToSegment(distance)]));
+      AOS_LOG(FATAL, "Unknown segment type %d\n",
+              static_cast<int>(plan_constraint(DistanceToSegment(distance))));
       break;
   }
 
-  return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
+  return (Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
       .finished();
 }
 
-::Eigen::Matrix<double, 2, 1> Trajectory::FFVoltage(double distance) {
+size_t FinishedTrajectory::distance_plan_size() const {
+  return trajectory().has_distance_based_plan()
+             ? trajectory().distance_based_plan()->size()
+             : 0u;
+}
+
+fb::SegmentConstraint FinishedTrajectory::plan_constraint(size_t index) const {
+  CHECK_LT(index, distance_plan_size());
+  return trajectory().distance_based_plan()->Get(index)->segment_constraint();
+}
+
+float FinishedTrajectory::plan_velocity(size_t index) const {
+  CHECK_LT(index, distance_plan_size());
+  return trajectory().distance_based_plan()->Get(index)->velocity();
+}
+
+Eigen::Matrix<double, 2, 1> BaseTrajectory::FFVoltage(double distance) const {
   const Eigen::Matrix<double, 3, 1> xva = FFAcceleration(distance);
   const double velocity = xva(1);
   const double acceleration = xva(2);
 
-  ::Eigen::Matrix<double, 2, 1> K3;
-  ::Eigen::Matrix<double, 2, 1> K4;
-  ::Eigen::Matrix<double, 2, 1> K5;
+  Eigen::Matrix<double, 2, 1> K3;
+  Eigen::Matrix<double, 2, 1> K4;
+  Eigen::Matrix<double, 2, 1> K5;
   K345(distance, &K3, &K4, &K5);
 
   return K5 * acceleration + K3 * velocity * velocity + K4 * velocity;
 }
 
-const ::std::vector<double> Trajectory::Distances() const {
-  ::std::vector<double> d;
+const std::vector<double> Trajectory::Distances() const {
+  std::vector<double> d;
   d.reserve(plan_.size());
   for (size_t i = 0; i < plan_.size(); ++i) {
     d.push_back(Distance(i));
@@ -465,114 +570,17 @@
   return d;
 }
 
-::Eigen::Matrix<double, 5, 5> Trajectory::ALinearizedContinuous(
-    const ::Eigen::Matrix<double, 5, 1> &state) const {
-  const double sintheta = ::std::sin(state(2));
-  const double costheta = ::std::cos(state(2));
-  const ::Eigen::Matrix<double, 2, 1> linear_angular =
-      Tlr_to_la_ * state.block<2, 1>(3, 0);
-
-  // When stopped, just roll with a min velocity.
-  double linear_velocity = 0.0;
-  constexpr double kMinVelocity = 0.1;
-  if (::std::abs(linear_angular(0)) < kMinVelocity / 100.0) {
-    linear_velocity = 0.1;
-  } else if (::std::abs(linear_angular(0)) > kMinVelocity) {
-    linear_velocity = linear_angular(0);
-  } else if (linear_angular(0) > 0) {
-    linear_velocity = kMinVelocity;
-  } else if (linear_angular(0) < 0) {
-    linear_velocity = -kMinVelocity;
-  }
-
-  ::Eigen::Matrix<double, 5, 5> result = ::Eigen::Matrix<double, 5, 5>::Zero();
-  result(0, 2) = -sintheta * linear_velocity;
-  result(0, 3) = 0.5 * costheta;
-  result(0, 4) = 0.5 * costheta;
-
-  result(1, 2) = costheta * linear_velocity;
-  result(1, 3) = 0.5 * sintheta;
-  result(1, 4) = 0.5 * sintheta;
-
-  result(2, 3) = Tlr_to_la_(1, 0);
-  result(2, 4) = Tlr_to_la_(1, 1);
-
-  result.block<2, 2>(3, 3) =
-      velocity_drivetrain_->plant().coefficients().A_continuous;
-  return result;
-}
-
-::Eigen::Matrix<double, 5, 2> Trajectory::BLinearizedContinuous() const {
-  ::Eigen::Matrix<double, 5, 2> result = ::Eigen::Matrix<double, 5, 2>::Zero();
-  result.block<2, 2>(3, 0) =
-      velocity_drivetrain_->plant().coefficients().B_continuous;
-  return result;
-}
-
-void Trajectory::AB(const ::Eigen::Matrix<double, 5, 1> &state,
-                    ::std::chrono::nanoseconds dt,
-                    ::Eigen::Matrix<double, 5, 5> *A,
-                    ::Eigen::Matrix<double, 5, 2> *B) const {
-  ::Eigen::Matrix<double, 5, 5> A_linearized_continuous =
-      ALinearizedContinuous(state);
-  ::Eigen::Matrix<double, 5, 2> B_linearized_continuous =
-      BLinearizedContinuous();
-
-  // Now, convert it to discrete.
-  controls::C2D(A_linearized_continuous, B_linearized_continuous, dt, A, B);
-}
-
-::Eigen::Matrix<double, 2, 5> Trajectory::KForState(
-    const ::Eigen::Matrix<double, 5, 1> &state, ::std::chrono::nanoseconds dt,
-    const ::Eigen::DiagonalMatrix<double, 5> &Q,
-    const ::Eigen::DiagonalMatrix<double, 2> &R) const {
-  ::Eigen::Matrix<double, 5, 5> A;
-  ::Eigen::Matrix<double, 5, 2> B;
-  AB(state, dt, &A, &B);
-
-  ::Eigen::Matrix<double, 5, 5> S = ::Eigen::Matrix<double, 5, 5>::Zero();
-  ::Eigen::Matrix<double, 2, 5> K = ::Eigen::Matrix<double, 2, 5>::Zero();
-
-  int info = ::frc971::controls::dlqr<5, 2>(A, B, Q, R, &K, &S);
-  if (info != 0) {
-    AOS_LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
-            controls::Controllability(A, B));
-    // TODO(austin): Can we be more clever here?  Use the last one?  We should
-    // collect more info about when this breaks down from logs.
-    K = ::Eigen::Matrix<double, 2, 5>::Zero();
-  }
-  if (VLOG_IS_ON(1)) {
-    ::Eigen::EigenSolver<::Eigen::Matrix<double, 5, 5>> eigensolver(A - B * K);
-    const auto eigenvalues = eigensolver.eigenvalues();
-    LOG(INFO) << "Eigenvalues: " << eigenvalues;
-  }
-  return K;
-}
-
-const ::Eigen::Matrix<double, 5, 1> Trajectory::GoalState(double distance,
-                                                          double velocity) {
-  ::Eigen::Matrix<double, 5, 1> result;
-  result.block<2, 1>(0, 0) = spline_->XY(distance);
-  result(2, 0) = spline_->Theta(distance);
-
-  result.block<2, 1>(3, 0) =
-      Tla_to_lr_ * (::Eigen::Matrix<double, 2, 1>() << velocity,
-                    spline_->DThetaDt(distance, velocity))
-                       .finished();
-  return result;
-}
-
-::Eigen::Matrix<double, 3, 1> Trajectory::GetNextXVA(
-    ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
+Eigen::Matrix<double, 3, 1> BaseTrajectory::GetNextXVA(
+    std::chrono::nanoseconds dt, Eigen::Matrix<double, 2, 1> *state) const {
   double dt_float = ::aos::time::DurationInSeconds(dt);
 
   const double last_distance = (*state)(0);
   // TODO(austin): This feels like something that should be pulled out into
   // a library for re-use.
   *state = RungeKutta(
-      [this](const ::Eigen::Matrix<double, 2, 1> x) {
-        ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
-        return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
+      [this](const Eigen::Matrix<double, 2, 1> x) {
+        Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
+        return (Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
       },
       *state, dt_float);
   // Force the distance to move forwards, to guarantee that we actually finish
@@ -582,15 +590,15 @@
     (*state)(0) = last_distance + kMinDistanceIncrease;
   }
 
-  ::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
+  Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
   (*state)(1) = result(1);
   return result;
 }
 
-::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
-    ::std::chrono::nanoseconds dt) {
-  ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
-  ::std::vector<::Eigen::Matrix<double, 3, 1>> result;
+std::vector<Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
+    std::chrono::nanoseconds dt) {
+  Eigen::Matrix<double, 2, 1> state = Eigen::Matrix<double, 2, 1>::Zero();
+  std::vector<Eigen::Matrix<double, 3, 1>> result;
   result.emplace_back(FFAcceleration(0));
   result.back()(1) = 0.0;
 
@@ -616,11 +624,11 @@
     AOS_LOG(FATAL, "End before start: %f > %f\n", starting_distance,
             ending_distance);
   }
-  starting_distance = ::std::min(length(), ::std::max(0.0, starting_distance));
-  ending_distance = ::std::min(length(), ::std::max(0.0, ending_distance));
+  starting_distance = std::min(length(), std::max(0.0, starting_distance));
+  ending_distance = std::min(length(), std::max(0.0, ending_distance));
   if (segment_length < min_length) {
     const size_t plan_index = static_cast<size_t>(
-        ::std::round((starting_distance + ending_distance) / 2.0 / min_length));
+        std::round((starting_distance + ending_distance) / 2.0 / min_length));
     if (max_velocity < plan_[plan_index]) {
       plan_[plan_index] = max_velocity;
     }
@@ -630,7 +638,7 @@
       if (max_velocity < plan_[i]) {
         plan_[i] = max_velocity;
         if (i < DistanceToSegment(ending_distance)) {
-          plan_segment_type_[i] = SegmentType::VELOCITY_LIMITED;
+          plan_segment_type_[i] = fb::SegmentConstraint::VELOCITY_LIMITED;
         }
       }
     }
@@ -641,7 +649,7 @@
                                               Eigen::Matrix<double, 5, 5> *A,
                                               Eigen::Matrix<double, 5, 2> *B) {
   const double nominal_velocity = FFAcceleration(distance)(1);
-  const double dtheta_dt = spline_->DThetaDt(distance, nominal_velocity);
+  const double dtheta_dt = spline().DThetaDt(distance, nominal_velocity);
   // Calculate the "path-relative" coordinates, which are:
   // [[distance along the path],
   //  [lateral position along path],
@@ -650,8 +658,8 @@
   //  [right wheel velocity]]
   Eigen::Matrix<double, 5, 1> nominal_X;
   nominal_X << distance, 0.0, 0.0,
-      nominal_velocity - dtheta_dt * robot_radius_l_,
-      nominal_velocity + dtheta_dt * robot_radius_r_;
+      nominal_velocity - dtheta_dt * robot_radius_l(),
+      nominal_velocity + dtheta_dt * robot_radius_r();
   PathRelativeContinuousSystem(nominal_X, A, B);
 }
 
@@ -663,9 +671,9 @@
   const double theta = X(2);
   const double ctheta = std::cos(theta);
   const double stheta = std::sin(theta);
-  const double curvature = spline_->DTheta(X(0));
+  const double curvature = spline().DTheta(X(0));
   const double longitudinal_velocity = (X(3) + X(4)) / 2.0;
-  const double diameter = robot_radius_l_ + robot_radius_r_;
+  const double diameter = robot_radius_l() + robot_radius_r();
   // d_dpath / dt = (v_left + v_right) / 2.0 * cos(theta)
   // (d_dpath / dt) / dv_left = cos(theta) / 2.0
   (*A)(0, 3) = ctheta / 2.0;
@@ -688,24 +696,24 @@
   (*A)(2, 4) = 1.0 / diameter - curvature / 2.0;
   // v_{left,right} / dt = the normal LTI system.
   A->block<2, 2>(3, 3) =
-      velocity_drivetrain_->plant().coefficients().A_continuous;
+      velocity_drivetrain().plant().coefficients().A_continuous;
   B->block<2, 2>(3, 0) =
-      velocity_drivetrain_->plant().coefficients().B_continuous;
+      velocity_drivetrain().plant().coefficients().B_continuous;
 }
 
 double Trajectory::EstimateDistanceAlongPath(
     double nominal_distance, const Eigen::Matrix<double, 5, 1> &state) {
-  const double nominal_theta = spline_->Theta(nominal_distance);
+  const double nominal_theta = spline().Theta(nominal_distance);
   const Eigen::Matrix<double, 2, 1> xy_err =
-      state.block<2, 1>(0, 0) - spline_->XY(nominal_distance);
+      state.block<2, 1>(0, 0) - spline().XY(nominal_distance);
   return nominal_distance + xy_err.x() * std::cos(nominal_theta) +
          xy_err.y() * std::sin(nominal_theta);
 }
 
-Eigen::Matrix<double, 5, 1> Trajectory::StateToPathRelativeState(
-    double distance, const Eigen::Matrix<double, 5, 1> &state) {
-  const double nominal_theta = spline_->Theta(distance);
-  const Eigen::Matrix<double, 2, 1> nominal_xy = spline_->XY(distance);
+Eigen::Matrix<double, 5, 1> FinishedTrajectory::StateToPathRelativeState(
+    double distance, const Eigen::Matrix<double, 5, 1> &state) const {
+  const double nominal_theta = spline().Theta(distance);
+  const Eigen::Matrix<double, 2, 1> nominal_xy = spline().XY(distance);
   const Eigen::Matrix<double, 2, 1> xy_err =
       state.block<2, 1>(0, 0) - nominal_xy;
   const double ctheta = std::cos(nominal_theta);
@@ -792,20 +800,99 @@
     const Eigen::Matrix<double, 2, 2> RBPBinv =
         (R + B_discrete.transpose() * P * B_discrete).inverse();
     plan_gains_[i].first = distance;
-    plan_gains_[i].second = RBPBinv * APB.transpose();
-    P = AP * A_discrete - APB * plan_gains_[i].second + Q;
+    const Eigen::Matrix<double, 2, 5> K = RBPBinv * APB.transpose();
+    plan_gains_[i].second = K.cast<float>();
+    P = AP * A_discrete - APB * K + Q;
   }
 }
 
-Eigen::Matrix<double, 2, 5> Trajectory::GainForDistance(double distance) {
-  CHECK(!plan_gains_.empty());
+Eigen::Matrix<double, 2, 5> FinishedTrajectory::GainForDistance(
+    double distance) const {
+  const flatbuffers::Vector<flatbuffers::Offset<fb::GainPoint>> &gains =
+      *CHECK_NOTNULL(trajectory().gains());
+  CHECK_LT(0u, gains.size());
   size_t index = 0;
-  for (index = 0; index < plan_gains_.size() - 1; ++index) {
-    if (plan_gains_[index + 1].first > distance) {
+  for (index = 0; index < gains.size() - 1; ++index) {
+    if (gains[index + 1]->distance() > distance) {
       break;
     }
   }
-  return plan_gains_[index].second;
+  // ColMajor is the default storage order, but call it out explicitly here.
+  return Eigen::Matrix<float, 2, 5, Eigen::ColMajor>{
+      gains[index]->gains()->data()}
+      .cast<double>();
+}
+
+namespace {
+flatbuffers::Offset<Constraint> MakeWholeLengthConstraint(
+    flatbuffers::FlatBufferBuilder *fbb, ConstraintType constraint_type,
+    float value) {
+  Constraint::Builder builder(*fbb);
+  builder.add_constraint_type(constraint_type);
+  builder.add_value(value);
+  return builder.Finish();
+}
+}  // namespace
+
+flatbuffers::Offset<fb::Trajectory> Trajectory::Serialize(
+    flatbuffers::FlatBufferBuilder *fbb) const {
+  std::array<flatbuffers::Offset<Constraint>, 3> constraints_offsets = {
+      MakeWholeLengthConstraint(fbb, ConstraintType::LONGITUDINAL_ACCELERATION,
+                                max_longitudinal_accel()),
+      MakeWholeLengthConstraint(fbb, ConstraintType::LATERAL_ACCELERATION,
+                                max_lateral_accel()),
+      MakeWholeLengthConstraint(fbb, ConstraintType::VOLTAGE, max_voltage())};
+  const auto constraints = fbb->CreateVector<Constraint>(
+      constraints_offsets.data(), constraints_offsets.size());
+  const flatbuffers::Offset<fb::DistanceSpline> spline_offset =
+      spline().Serialize(fbb, constraints);
+
+  std::vector<flatbuffers::Offset<fb::PlanPoint>> plan_points;
+  for (size_t ii = 0; ii < distance_plan_size(); ++ii) {
+    plan_points.push_back(fb::CreatePlanPoint(
+        *fbb, Distance(ii), plan_velocity(ii), plan_constraint(ii)));
+  }
+
+  // TODO(james): What is an appropriate cap?
+  CHECK_LT(plan_gains_.size(), 5000u);
+  CHECK_LT(0u, plan_gains_.size());
+  std::vector<flatbuffers::Offset<fb::GainPoint>> gain_points;
+  const size_t matrix_size = plan_gains_[0].second.size();
+  for (size_t ii = 0; ii < plan_gains_.size(); ++ii) {
+    gain_points.push_back(fb::CreateGainPoint(
+        *fbb, plan_gains_[ii].first,
+        fbb->CreateVector(plan_gains_[ii].second.data(), matrix_size)));
+  }
+
+  return fb::CreateTrajectory(*fbb, spline_idx_, fbb->CreateVector(plan_points),
+                              fbb->CreateVector(gain_points), spline_offset,
+                              drive_spline_backwards_);
+}
+
+float BaseTrajectory::ConstraintValue(
+    const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+    ConstraintType type) {
+  if (constraints != nullptr) {
+    for (const Constraint *constraint : *constraints) {
+      if (constraint->constraint_type() == type) {
+        return constraint->value();
+      }
+    }
+  }
+  return DefaultConstraint(type);
+}
+
+const Eigen::Matrix<double, 5, 1> BaseTrajectory::GoalState(
+    double distance, double velocity) const {
+  Eigen::Matrix<double, 5, 1> result;
+  result.block<2, 1>(0, 0) = spline().XY(distance);
+  result(2, 0) = spline().Theta(distance);
+
+  result.block<2, 1>(3, 0) =
+      config_.Tla_to_lr() * (Eigen::Matrix<double, 2, 1>() << velocity,
+                             spline().DThetaDt(distance, velocity))
+                                .finished();
+  return result;
 }
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/trajectory.fbs b/frc971/control_loops/drivetrain/trajectory.fbs
new file mode 100644
index 0000000..ab14238
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory.fbs
@@ -0,0 +1,44 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain.fb;
+
+enum SegmentConstraint : byte {
+  VELOCITY_LIMITED,
+  CURVATURE_LIMITED,
+  ACCELERATION_LIMITED,
+  DECELERATION_LIMITED,
+  VOLTAGE_LIMITED,
+}
+
+table PlanPoint {
+  // Distance along the path of this point.
+  distance: float (id: 0);
+  velocity: float (id: 1);
+  segment_constraint: SegmentConstraint (id: 2);
+}
+
+table GainPoint {
+  distance: float (id: 0);
+  // Column-major matrix (should be 2 rows x 5 columns).
+  gains: [float] (id: 1);
+}
+
+table DistanceSpline {
+  spline:frc971.MultiSpline (id: 0);
+  distances:[float] (id: 1);
+}
+
+table Trajectory {
+  // Unique ID of the trajectory, same as that defined in the SplineGoal.
+  handle:int (id: 0);
+  // Spline plan, indexed on distance.
+  distance_based_plan : [PlanPoint] (id : 1);
+  // Gains, indexed on time.
+  gains: [GainPoint] (id: 2);
+  spline:DistanceSpline (id: 3);
+
+  // Whether to follow the spline driving forwards or backwards.
+  drive_spline_backwards:bool (id: 4);
+}
+
+root_type Trajectory;
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index ef02db5..00b37f6 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -3,9 +3,12 @@
 
 #include <chrono>
 
+#include "aos/flatbuffers.h"
 #include "Eigen/Dense"
 #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/hybrid_state_feedback_loop.h"
 #include "frc971/control_loops/runge_kutta.h"
 #include "frc971/control_loops/state_feedback_loop.h"
@@ -27,34 +30,23 @@
                 // true because when starting from a stop, under sane
                 // accelerations, we can assume that we will start with a
                 // constant acceleration.  So, hard-code it.
-                if (::std::abs(y) < 1e-6) {
+                if (std::abs(y) < 1e-6) {
                   return 0.0;
                 }
                 return (fn(t, y) - a0) / y;
               },
               v, x, dx) -
           v) +
-         ::std::sqrt(2.0 * a0 * dx + v * v);
+         std::sqrt(2.0 * a0 * dx + v * v);
 }
 
-// Class to plan and hold the velocity plan for a spline.
-class Trajectory {
+class BaseTrajectory {
  public:
-  Trajectory(const DistanceSpline *spline,
-             const DrivetrainConfig<double> &config, double vmax = 10.0,
-             int num_distance = 0);
-  // Sets the plan longitudinal acceleration limit
-  void set_longitudinal_acceleration(double longitudinal_acceleration) {
-    longitudinal_acceleration_ = longitudinal_acceleration;
-  }
-  // Sets the plan lateral acceleration limit
-  void set_lateral_acceleration(double lateral_acceleration) {
-    lateral_acceleration_ = lateral_acceleration;
-  }
-  // Sets the voltage limit
-  void set_voltage_limit(double voltage_limit) {
-    voltage_limit_ = voltage_limit;
-  }
+  BaseTrajectory(
+      const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+      const DrivetrainConfig<double> &config);
+
+  virtual ~BaseTrajectory() = default;
 
   // Returns the friction-constrained velocity limit at a given distance along
   // the path. At the returned velocity, one or both wheels will be on the edge
@@ -74,6 +66,184 @@
   void FrictionLngAccelLimits(double x, double v, double *min_accel,
                               double *max_accel) const;
 
+  // Returns the forwards/backwards acceleration for a distance along the spline
+  // taking into account the lateral acceleration, longitudinal acceleration,
+  // and voltage limits.
+  double BestAcceleration(double x, double v, bool backwards) const;
+  double BackwardAcceleration(double x, double v) const {
+    return BestAcceleration(x, v, true);
+  }
+  double ForwardAcceleration(double x, double v) const {
+    return BestAcceleration(x, v, false);
+  }
+
+  const StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+                          HybridKalman<2, 2, 2>>
+      &velocity_drivetrain() const {
+    return *velocity_drivetrain_;
+  }
+
+  // Returns K1 and K2.
+  // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
+  const Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const;
+  const Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const;
+
+  // Computes K3, K4, and K5 for the provided distance.
+  // K5 a + K3 v^2 + K4 v = U
+  void K345(const double x, Eigen::Matrix<double, 2, 1> *K3,
+            Eigen::Matrix<double, 2, 1> *K4,
+            Eigen::Matrix<double, 2, 1> *K5) const;
+
+  virtual const DistanceSpline &spline() const = 0;
+
+  // Returns the length of the path in meters.
+  double length() const { return spline().length(); }
+
+  // Returns whether a state represents a state at the end of the spline.
+  bool is_at_end(Eigen::Matrix<double, 2, 1> state) const {
+    return state(0) > length() - 1e-4;
+  }
+
+  // Returns true if the state is invalid or unreasonable in some way.
+  bool state_is_faulted(Eigen::Matrix<double, 2, 1> state) const {
+    // Consider things faulted if the current velocity implies we are going
+    // backwards or if any infinities/NaNs have crept in.
+    return state(1) < 0 || !state.allFinite();
+  }
+
+  virtual float plan_velocity(size_t index) const = 0;
+  virtual size_t distance_plan_size() const = 0;
+
+  // Sets the plan longitudinal acceleration limit
+  void set_longitudinal_acceleration(double longitudinal_acceleration) {
+    longitudinal_acceleration_ = longitudinal_acceleration;
+  }
+  // Sets the plan lateral acceleration limit
+  void set_lateral_acceleration(double lateral_acceleration) {
+    lateral_acceleration_ = lateral_acceleration;
+  }
+  // Sets the voltage limit
+  void set_voltage_limit(double voltage_limit) {
+    voltage_limit_ = voltage_limit;
+  }
+
+  float max_lateral_accel() const { return lateral_acceleration_; }
+
+  float max_longitudinal_accel() const { return longitudinal_acceleration_; }
+
+  float max_voltage() const { return voltage_limit_; }
+
+  // Return the next position, velocity, acceleration based on the current
+  // state. Updates the passed in state for the next iteration.
+  Eigen::Matrix<double, 3, 1> GetNextXVA(
+      std::chrono::nanoseconds dt, Eigen::Matrix<double, 2, 1> *state) const;
+
+  // Returns the distance for an index in the plan.
+  double Distance(int index) const {
+    return static_cast<double>(index) * length() /
+           static_cast<double>(distance_plan_size() - 1);
+  }
+
+  virtual fb::SegmentConstraint plan_constraint(size_t index) const = 0;
+
+  // Returns the feed forwards position, velocity, acceleration for an explicit
+  // distance.
+  Eigen::Matrix<double, 3, 1> FFAcceleration(double distance) const;
+
+  // Returns the feed forwards voltage for an explicit distance.
+  Eigen::Matrix<double, 2, 1> FFVoltage(double distance) const;
+
+  // Computes alpha for a distance.
+  size_t DistanceToSegment(double distance) const {
+    return std::max(
+        static_cast<size_t>(0),
+        std::min(distance_plan_size() - 1,
+                 static_cast<size_t>(std::floor(distance / length() *
+                                                (distance_plan_size() - 1)))));
+  }
+
+  // Returns the goal state as a function of path distance, velocity.
+  const ::Eigen::Matrix<double, 5, 1> GoalState(double distance,
+                                                double velocity) const;
+
+ protected:
+  double robot_radius_l() const { return robot_radius_l_; }
+  double robot_radius_r() const { return robot_radius_r_; }
+
+ private:
+  static float ConstraintValue(
+      const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+      ConstraintType type);
+
+  std::unique_ptr<
+      StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+                        HybridKalman<2, 2, 2>>>
+      velocity_drivetrain_;
+
+  const DrivetrainConfig<double> config_;
+
+  // Robot radiuses.
+  const double robot_radius_l_;
+  const double robot_radius_r_;
+  float lateral_acceleration_ = 3.0;
+  float longitudinal_acceleration_ = 2.0;
+  float voltage_limit_ = 12.0;
+};
+
+// A wrapper around the Trajectory flatbuffer to allow for controlling to a
+// spline using a pre-generated trajectory.
+class FinishedTrajectory  : public BaseTrajectory {
+ public:
+  // Note: The lifetime of the supplied buffer is assumed to be greater than
+  // that of this object.
+  explicit FinishedTrajectory(const DrivetrainConfig<double> &config,
+                              const fb::Trajectory *buffer);
+
+  virtual ~FinishedTrajectory() = default;
+
+  // Takes the 5-element state that is [x, y, theta, v_left, v_right] and
+  // converts it to a path-relative state, using distance as a linearization
+  // point (i.e., distance should be roughly equal to the actual distance along
+  // the path).
+  Eigen::Matrix<double, 5, 1> StateToPathRelativeState(
+      double distance, const Eigen::Matrix<double, 5, 1> &state) const;
+
+  // Retrieves the gain matrix K for a given distance along the path.
+  Eigen::Matrix<double, 2, 5> GainForDistance(double distance) const;
+
+  size_t distance_plan_size() const override;
+  float plan_velocity(size_t index) const override;
+  fb::SegmentConstraint plan_constraint(size_t index) const override;
+
+  bool drive_spline_backwards() const {
+    return trajectory().drive_spline_backwards();
+  }
+
+  int spline_handle() const { return trajectory().handle(); }
+  const fb::Trajectory &trajectory() const { return *buffer_; }
+
+ private:
+  const DistanceSpline &spline() const override { return spline_; }
+  const fb::Trajectory *buffer_;
+  const DistanceSpline spline_;
+};
+
+// Class to handle plannign a trajectory and producing a flatbuffer containing
+// all the information required to create a FinishedTrajectory;
+class Trajectory : public BaseTrajectory {
+ public:
+  Trajectory(const SplineGoal &spline_goal,
+             const DrivetrainConfig<double> &config);
+  Trajectory(
+      DistanceSpline &&spline, const DrivetrainConfig<double> &config,
+      const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+      int spline_idx = 0, double vmax = 10.0, int num_distance = 0);
+
+  virtual ~Trajectory() = default;
+
+  std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(
+      std::chrono::nanoseconds dt);
+
   enum class VoltageLimit {
     kConservative,
     kAggressive,
@@ -104,17 +274,6 @@
   // Runs the forwards pass, setting the starting velocity to 0 m/s
   void ForwardPass();
 
-  // Returns the forwards/backwards acceleration for a distance along the spline
-  // taking into account the lateral acceleration, longitudinal acceleration,
-  // and voltage limits.
-  double BestAcceleration(double x, double v, bool backwards);
-  double BackwardAcceleration(double x, double v) {
-    return BestAcceleration(x, v, true);
-  }
-  double ForwardAcceleration(double x, double v) {
-    return BestAcceleration(x, v, false);
-  }
-
   // Runs the forwards pass, setting the ending velocity to 0 m/s
   void BackwardPass();
 
@@ -127,115 +286,18 @@
     CalculatePathGains();
   }
 
-  // Returns the feed forwards position, velocity, acceleration for an explicit
-  // distance.
-  ::Eigen::Matrix<double, 3, 1> FFAcceleration(double distance);
-
-  // Returns the feed forwards voltage for an explicit distance.
-  ::Eigen::Matrix<double, 2, 1> FFVoltage(double distance);
-
-  // Returns whether a state represents a state at the end of the spline.
-  bool is_at_end(::Eigen::Matrix<double, 2, 1> state) const {
-    return state(0) > length() - 1e-4;
-  }
-
-  // Returns true if the state is invalid or unreasonable in some way.
-  bool state_is_faulted(::Eigen::Matrix<double, 2, 1> state) const {
-    // Consider things faulted if the current velocity implies we are going
-    // backwards or if any infinities/NaNs have crept in.
-    return state(1) < 0 || !state.allFinite();
-  }
-
-  // Returns the length of the path in meters.
-  double length() const { return spline_->length(); }
-
   // Returns a list of the distances.  Mostly useful for plotting.
-  const ::std::vector<double> Distances() const;
+  const std::vector<double> Distances() const;
   // Returns the distance for an index in the plan.
   double Distance(int index) const {
     return static_cast<double>(index) * length() /
            static_cast<double>(plan_.size() - 1);
   }
 
-  // Returns the plan.  This is the pathwise velocity as a function of distance.
-  // To get the distance for an index, use the Distance(index) function provided
-  // with the index.
-  const ::std::vector<double> plan() const { return plan_; }
-
-  // Returns the left, right to linear, angular transformation matrix.
-  const ::Eigen::Matrix<double, 2, 2> &Tlr_to_la() const { return Tlr_to_la_; }
-  // Returns the linear, angular to left, right transformation matrix.
-  const ::Eigen::Matrix<double, 2, 2> &Tla_to_lr() const { return Tla_to_lr_; }
-
-  // Returns the goal state as a function of path distance, velocity.
-  const ::Eigen::Matrix<double, 5, 1> GoalState(double distance,
-                                                double velocity);
-
-  // Returns the velocity drivetrain in use.
-  const StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
-                          HybridKalman<2, 2, 2>>
-      &velocity_drivetrain() const {
-    return *velocity_drivetrain_;
-  }
-
-  // Returns the continuous statespace A and B matricies for [x, y, theta, vl,
-  // vr] for the linearized system (around the provided state).
-  ::Eigen::Matrix<double, 5, 5> ALinearizedContinuous(
-      const ::Eigen::Matrix<double, 5, 1> &state) const;
-  ::Eigen::Matrix<double, 5, 2> BLinearizedContinuous() const;
-
-  // Returns the discrete time A and B matricies for the provided state,
-  // assuming the provided timestep.
-  void AB(const ::Eigen::Matrix<double, 5, 1> &state,
-          ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 5, 5> *A,
-          ::Eigen::Matrix<double, 5, 2> *B) const;
-
-  // Returns the lqr controller for the current state, timestep, and Q and R
-  // gains. This controller is currently not used--we instead are experimenting
-  // with the path-relative LQR (and potentially MPC, in the future) controller,
-  // which uses the methods defined below.
-  // TODO(austin): This feels like it should live somewhere else, but I'm not
-  // sure where.  So, throw it here...
-  ::Eigen::Matrix<double, 2, 5> KForState(
-      const ::Eigen::Matrix<double, 5, 1> &state, ::std::chrono::nanoseconds dt,
-      const ::Eigen::DiagonalMatrix<double, 5> &Q,
-      const ::Eigen::DiagonalMatrix<double, 2> &R) const;
-
-  // Return the next position, velocity, acceleration based on the current
-  // state. Updates the passed in state for the next iteration.
-  ::Eigen::Matrix<double, 3, 1> GetNextXVA(
-      ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state);
-  ::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
-      ::std::chrono::nanoseconds dt);
-
-  enum SegmentType : uint8_t {
-    VELOCITY_LIMITED,
-    CURVATURE_LIMITED,
-    ACCELERATION_LIMITED,
-    DECELERATION_LIMITED,
-    VOLTAGE_LIMITED,
-  };
-
-  const ::std::vector<SegmentType> &plan_segment_type() const {
+  const std::vector<fb::SegmentConstraint> &plan_segment_type() const {
     return plan_segment_type_;
   }
 
-  // Returns K1 and K2.
-  // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
-  const ::Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const {
-    return (::Eigen::Matrix<double, 2, 1>()
-                << -robot_radius_l_ * current_ddtheta,
-            robot_radius_r_ * current_ddtheta)
-        .finished();
-  }
-
-  const ::Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const {
-    return (::Eigen::Matrix<double, 2, 1>()
-                << 1.0 - robot_radius_l_ * current_dtheta,
-            1.0 + robot_radius_r_ * current_dtheta)
-        .finished();
-  }
-
   // The controller represented by these functions uses a discrete-time,
   // finite-horizon LQR with states that are relative to the predicted path
   // of the robot to produce a gain to be used on the error.
@@ -278,60 +340,31 @@
   // phase, and is a relatively expensive operation.
   void CalculatePathGains();
 
-  // Retrieves the gain matrix K for a given distance along the path.
-  Eigen::Matrix<double, 2, 5> GainForDistance(double distance);
+  flatbuffers::Offset<fb::Trajectory> Serialize(
+      flatbuffers::FlatBufferBuilder *fbb) const;
+
+  const std::vector<double> plan() const { return plan_; }
+
+  const DistanceSpline &spline() const override { return spline_; }
 
  private:
-  // Computes alpha for a distance.
-  size_t DistanceToSegment(double distance) const {
-    return ::std::max(
-        static_cast<size_t>(0),
-        ::std::min(plan_segment_type_.size() - 1,
-                   static_cast<size_t>(::std::floor(distance / length() *
-                                                    (plan_.size() - 1)))));
+
+  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 {
+    return plan_segment_type_[index];
   }
 
-  // Computes K3, K4, and K5 for the provided distance.
-  // K5 a + K3 v^2 + K4 v = U
-  void K345(const double x, ::Eigen::Matrix<double, 2, 1> *K3,
-            ::Eigen::Matrix<double, 2, 1> *K4,
-            ::Eigen::Matrix<double, 2, 1> *K5) {
-    const double current_ddtheta = spline_->DDTheta(x);
-    const double current_dtheta = spline_->DTheta(x);
-    // We've now got the equation:
-    //     K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
-    const ::Eigen::Matrix<double, 2, 1> my_K2 = K2(current_dtheta);
-
-    const ::Eigen::Matrix<double, 2, 2> B_inverse =
-        velocity_drivetrain_->plant().coefficients().B_continuous.inverse();
-
-    // Now, rephrase it as K5 a + K3 v^2 + K4 v = U
-    *K3 = B_inverse * K1(current_ddtheta);
-    *K4 = -B_inverse *
-          velocity_drivetrain_->plant().coefficients().A_continuous * my_K2;
-    *K5 = B_inverse * my_K2;
-  }
+  const int spline_idx_;
 
   // The spline we are planning for.
-  const DistanceSpline *spline_;
-  // The drivetrain we are controlling.
-  ::std::unique_ptr<
-      StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
-                        HybridKalman<2, 2, 2>>>
-      velocity_drivetrain_;
+  const DistanceSpline spline_;
 
   const DrivetrainConfig<double> config_;
 
-  // Robot radiuses.
-  const double robot_radius_l_;
-  const double robot_radius_r_;
-  // Acceleration limits.
-  double longitudinal_acceleration_;
-  double lateral_acceleration_;
-  // Transformation matrix from left, right to linear, angular
-  const ::Eigen::Matrix<double, 2, 2> Tlr_to_la_;
-  // Transformation matrix from linear, angular to left, right
-  const ::Eigen::Matrix<double, 2, 2> Tla_to_lr_;
   // Velocities in the plan (distance for each index is defined by Distance())
   std::vector<double> plan_;
   // Gain matrices to use for the path-relative state error at each stage in the
@@ -339,24 +372,24 @@
   // config_.dt in time.
   // The first value in the pair is the distance along the path corresponding to
   // the gain matrix; the second value is the gain itself.
-  std::vector<std::pair<double, Eigen::Matrix<double, 2, 5>>> plan_gains_;
-  std::vector<SegmentType> plan_segment_type_;
-  // Plan voltage limit.
-  double voltage_limit_ = 12.0;
+  std::vector<std::pair<double, Eigen::Matrix<float, 2, 5>>> plan_gains_;
+  std::vector<fb::SegmentConstraint> plan_segment_type_;
+
+  bool drive_spline_backwards_ = false;
 };
 
 // Returns the continuous time dynamics given the [x, y, theta, vl, vr] state
 // and the [vl, vr] input voltage.
-inline ::Eigen::Matrix<double, 5, 1> ContinuousDynamics(
+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, 2, 2> &Tlr_to_la,
+    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;
-  return (::Eigen::Matrix<double, 5, 1>() << ::std::cos(theta) * la(0),
-          ::std::sin(theta) * la(0), la(1),
+  Eigen::Matrix<double, 2, 1> la = Tlr_to_la * velocity;
+  return (Eigen::Matrix<double, 5, 1>() << std::cos(theta) * la(0),
+          std::sin(theta) * la(0), la(1),
           (velocity_drivetrain.coefficients().A_continuous * velocity +
            velocity_drivetrain.coefficients().B_continuous * U))
       .finished();
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.cc b/frc971/control_loops/drivetrain/trajectory_generator.cc
new file mode 100644
index 0000000..16b6809
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory_generator.cc
@@ -0,0 +1,30 @@
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+TrajectoryGenerator::TrajectoryGenerator(aos::EventLoop *event_loop,
+                                         const DrivetrainConfig<double> &config)
+    : event_loop_(event_loop),
+      dt_config_(config),
+      trajectory_sender_(
+          event_loop_->MakeSender<fb::Trajectory>("/drivetrain")) {
+  event_loop_->MakeWatcher("/drivetrain", [this](const SplineGoal &goal) {
+    HandleSplineGoal(goal);
+  });
+}
+
+void TrajectoryGenerator::HandleSplineGoal(const SplineGoal &goal) {
+  Trajectory trajectory(goal, dt_config_);
+  trajectory.Plan();
+
+  aos::Sender<fb::Trajectory>::Builder builder =
+      trajectory_sender_.MakeBuilder();
+
+  CHECK(builder.Send(trajectory.Serialize(builder.fbb())));
+}
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.h b/frc971/control_loops/drivetrain/trajectory_generator.h
new file mode 100644
index 0000000..0bf5e92
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory_generator.h
@@ -0,0 +1,28 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_GENERATOR_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_GENERATOR_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/spline_goal_generated.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+class TrajectoryGenerator {
+ public:
+  TrajectoryGenerator(aos::EventLoop *event_loop,
+                      const DrivetrainConfig<double> &config);
+  void HandleSplineGoal(const SplineGoal &goal);
+ private:
+  aos::EventLoop *const event_loop_;
+  const DrivetrainConfig<double> dt_config_;
+  aos::Sender<fb::Trajectory> trajectory_sender_;
+};
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_GENERATOR_H_
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 6aff128..6fe1fbf 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -29,10 +29,6 @@
 // https://photos.google.com/share/AF1QipPl34MOTPem2QmmTC3B21dL7GV2_HjxnseRrqxgR60TUasyIPliIuWmnH3yxuSNZw?key=cVhZLUYycXBIZlNTRy10cjZlWm0tcmlqQl9MTE13
 
 DEFINE_bool(plot, true, "If true, plot");
-DEFINE_double(qx, 0.05, "Q_xpos");
-DEFINE_double(qy, 0.05, "Q_ypos");
-DEFINE_double(qt, 0.2, "Q_thetapos");
-DEFINE_double(qv, 0.5, "Q_vel");
 
 DEFINE_double(dx, 0.0, "Amount to disturb x at the start");
 DEFINE_double(dy, 0.0, "Amount to disturb y at the start");
@@ -49,13 +45,14 @@
 namespace drivetrain {
 
 void Main() {
-  DistanceSpline distance_spline(Spline(
-      Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2 * FLAGS_forward,
-                  -0.2 * FLAGS_forward, FLAGS_forward, 0.0, 0.0, 1.0, 1.0)
-                     .finished())));
+  const DrivetrainConfig<double> config =
+      ::y2019::control_loops::drivetrain::GetDrivetrainConfig();
   Trajectory trajectory(
-      &distance_spline,
-      ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
+      DistanceSpline(Spline(Spline4To6(
+          (::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2 * FLAGS_forward,
+           -0.2 * FLAGS_forward, FLAGS_forward, 0.0, 0.0, 1.0, 1.0)
+              .finished()))),
+      config, nullptr);
   trajectory.set_lateral_acceleration(2.0);
   trajectory.set_longitudinal_acceleration(1.0);
 
@@ -66,8 +63,9 @@
   ::std::vector<double> spline_theta;
 
   for (const double distance : distances) {
-    const ::Eigen::Matrix<double, 2, 1> point = distance_spline.XY(distance);
-    const double theta = distance_spline.Theta(distance);
+    const ::Eigen::Matrix<double, 2, 1> point =
+        trajectory.spline().XY(distance);
+    const double theta = trajectory.spline().Theta(distance);
     spline_x.push_back(point(0));
     spline_y.push_back(point(1));
     spline_theta.push_back(theta);
@@ -88,7 +86,7 @@
 
   ::std::vector<double> plan_segment_center_distance;
   ::std::vector<double> plan_type;
-  for (Trajectory::SegmentType segment_type : trajectory.plan_segment_type()) {
+  for (fb::SegmentConstraint segment_type : trajectory.plan_segment_type()) {
     plan_type.push_back(static_cast<int>(segment_type));
   }
   for (size_t i = 0; i < distances.size() - 1; ++i) {
@@ -128,24 +126,14 @@
     }
   }
 
-  const double kXPos = FLAGS_qx;
-  const double kYPos = FLAGS_qy;
-  const double kThetaPos = FLAGS_qt;
-  const double kVel = FLAGS_qv;
-  const ::Eigen::DiagonalMatrix<double, 5> Q =
-      (::Eigen::DiagonalMatrix<double, 5>().diagonal()
-           << 1.0 / ::std::pow(kXPos, 2),
-       1.0 / ::std::pow(kYPos, 2), 1.0 / ::std::pow(kThetaPos, 2),
-       1.0 / ::std::pow(kVel, 2), 1.0 / ::std::pow(kVel, 2))
-          .finished()
-          .asDiagonal();
+  flatbuffers::FlatBufferBuilder fbb;
 
-  const ::Eigen::DiagonalMatrix<double, 2> R =
-      (::Eigen::DiagonalMatrix<double, 2>().diagonal()
-           << 1.0 / ::std::pow(12.0, 2),
-       1.0 / ::std::pow(12.0, 2))
-          .finished()
-          .asDiagonal();
+  fbb.Finish(trajectory.Serialize(&fbb));
+
+  aos::FlatbufferDetachedBuffer<fb::Trajectory> trajectory_buffer(
+      fbb.Release());
+
+  FinishedTrajectory finished_trajectory(config, &trajectory_buffer.message());
 
   ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
   state(0, 0) = FLAGS_dx;
@@ -186,17 +174,17 @@
     error_velocity_l.push_back(state_error(3));
     error_velocity_r.push_back(state_error(4));
 
-    ::Eigen::Matrix<double, 2, 5> K =
-        trajectory.KForState(state, chrono::microseconds(5050), Q, R);
+    const ::Eigen::Matrix<double, 2, 5> K =
+        finished_trajectory.GainForDistance(distance);
 
     const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory.FFVoltage(distance);
     const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
     const ::Eigen::Matrix<double, 2, 1> U = U_ff + U_fb;
     state = RungeKuttaU(
-        [&trajectory](const ::Eigen::Matrix<double, 5, 1> &X,
-                      const ::Eigen::Matrix<double, 2, 1> &U) {
+        [&trajectory, &config](const ::Eigen::Matrix<double, 5, 1> &X,
+                               const ::Eigen::Matrix<double, 2, 1> &U) {
           return ContinuousDynamics(trajectory.velocity_drivetrain().plant(),
-                                    trajectory.Tlr_to_la(), X, U);
+                                    config.Tlr_to_la(), X, U);
         },
         state, U, kDtDouble);
 
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index 2770f63..c2f6655 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -63,18 +63,22 @@
  public:
   ::aos::testing::TestSharedMemory shm_;
 
-  ::std::unique_ptr<DistanceSpline> distance_spline_;
+  const DistanceSpline *distance_spline_;
   ::std::unique_ptr<Trajectory> trajectory_;
+  std::unique_ptr<aos::FlatbufferDetachedBuffer<fb::Trajectory>>
+      trajectory_buffer_;
+  ::std::unique_ptr<FinishedTrajectory> finished_trajectory_;
   ::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
 
   ParameterizedSplineTest() : dt_config_(GetTestDrivetrainConfig()) {}
 
   void SetUp() {
-    distance_spline_ = ::std::unique_ptr<DistanceSpline>(
-        new DistanceSpline(Spline(GetParam().control_points)));
+    const int spline_index = 12345;
     // Run lots of steps to make the feedforwards terms more accurate.
     trajectory_ = ::std::unique_ptr<Trajectory>(new Trajectory(
-        distance_spline_.get(), dt_config_, GetParam().velocity_limit));
+        DistanceSpline(GetParam().control_points), dt_config_,
+        /*constraints=*/nullptr, spline_index, GetParam().velocity_limit));
+    distance_spline_ = &trajectory_->spline();
     trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
     trajectory_->set_longitudinal_acceleration(
         GetParam().longitudinal_acceleration);
@@ -98,6 +102,18 @@
     trajectory_->CalculatePathGains();
 
     length_plan_xva_ = trajectory_->PlanXVA(dt_config_.dt);
+
+    flatbuffers::FlatBufferBuilder fbb;
+
+    fbb.Finish(trajectory_->Serialize(&fbb));
+
+    trajectory_buffer_ = std::make_unique<aos::FlatbufferDetachedBuffer<
+        frc971::control_loops::drivetrain::fb::Trajectory>>(fbb.Release());
+
+    EXPECT_EQ(spline_index, trajectory_buffer_->message().handle());
+
+    finished_trajectory_ = std::make_unique<FinishedTrajectory>(
+        dt_config_, &trajectory_buffer_->message());
   }
 
   void TearDown() {
@@ -118,7 +134,7 @@
 
       ::std::vector<double> plan_segment_center_distance;
       ::std::vector<double> plan_type;
-      for (Trajectory::SegmentType segment_type :
+      for (fb::SegmentConstraint segment_type :
            trajectory_->plan_segment_type()) {
         plan_type.push_back(static_cast<int>(segment_type));
       }
@@ -351,12 +367,13 @@
 // to the right point.
 TEST_P(ParameterizedSplineTest, FFSpline) {
   ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
-  state = trajectory_->GoalState(0.0, 0.0);
+  state = finished_trajectory_->GoalState(0.0, 0.0);
 
   for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
     const double distance = length_plan_xva_[i](0);
 
-    const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
+    const ::Eigen::Matrix<double, 2, 1> U_ff =
+        finished_trajectory_->FFVoltage(distance);
     const ::Eigen::Matrix<double, 2, 1> U = U_ff;
 
     length_plan_vl_.push_back(U(0));
@@ -365,50 +382,17 @@
         [this](const ::Eigen::Matrix<double, 5, 1> &X,
                const ::Eigen::Matrix<double, 2, 1> &U) {
           return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
-                                    trajectory_->Tlr_to_la(), X, U);
+                                    dt_config_.Tlr_to_la(), X, U);
         },
         state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
   }
 
-  EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
+  EXPECT_LT((state - finished_trajectory_->GoalState(
+                         finished_trajectory_->length(), 0.0))
+                .norm(),
             4e-2);
 }
 
-// Tests that following a spline with both feed forwards and feed back gets
-// pretty darn close to the right point.
-TEST_P(ParameterizedSplineTest, FBSpline) {
-  ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
-
-  for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
-    const double distance = length_plan_xva_[i](0);
-    const double velocity = length_plan_xva_[i](1);
-    const ::Eigen::Matrix<double, 5, 1> goal_state =
-        trajectory_->GoalState(distance, velocity);
-
-    const ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
-
-    const ::Eigen::Matrix<double, 2, 5> K =
-        trajectory_->KForState(state, dt_config_.dt, Q, R);
-
-    const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
-    const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
-    const ::Eigen::Matrix<double, 2, 1> U = U_ff + U_fb;
-
-    length_plan_vl_.push_back(U(0));
-    length_plan_vr_.push_back(U(1));
-    state = RungeKuttaU(
-        [this](const ::Eigen::Matrix<double, 5, 1> &X,
-               const ::Eigen::Matrix<double, 2, 1> &U) {
-          return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
-                                    trajectory_->Tlr_to_la(), X, U);
-        },
-        state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
-  }
-
-  EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
-            2e-2);
-}
-
 // Tests that Iteratively computing the XVA plan is the same as precomputing it.
 TEST_P(ParameterizedSplineTest, IterativeXVA) {
   for (double v : trajectory_->plan()) {
@@ -417,7 +401,7 @@
   ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
   for (size_t i = 1; i < length_plan_xva_.size(); ++i) {
     ::Eigen::Matrix<double, 3, 1> xva =
-        trajectory_->GetNextXVA(dt_config_.dt, &state);
+        finished_trajectory_->GetNextXVA(dt_config_.dt, &state);
     EXPECT_LT((length_plan_xva_[i] - xva).norm(), 1e-2);
   }
 }
@@ -455,20 +439,22 @@
       continue;
     }
 
-    const Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
+    const Eigen::Matrix<double, 2, 1> U_ff =
+        finished_trajectory_->FFVoltage(distance);
     const Eigen::Matrix<double, 2, 1> U = U_ff;
 
     absolute_state = RungeKuttaU(
         [this](const Eigen::Matrix<double, 5, 1> &X,
                const Eigen::Matrix<double, 2, 1> &U) {
-          return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
-                                    trajectory_->Tlr_to_la(), X, U);
+          return ContinuousDynamics(finished_trajectory_->velocity_drivetrain().plant(),
+                                    dt_config_.Tlr_to_la(), X, U);
         },
         absolute_state, U, aos::time::DurationInSeconds(dt_config_.dt));
     const Eigen::Matrix<double, 5, 1> goal_absolute_state =
-        trajectory_->GoalState(distance, velocity);
+        finished_trajectory_->GoalState(distance, velocity);
     const Eigen::Matrix<double, 5, 1> goal_relative_state =
-        trajectory_->StateToPathRelativeState(distance, goal_absolute_state);
+        finished_trajectory_->StateToPathRelativeState(distance,
+                                                       goal_absolute_state);
     ASSERT_EQ(distance, goal_relative_state(0));
     ASSERT_EQ(0.0, goal_relative_state(1));
     ASSERT_NEAR(goal_absolute_state(2), goal_relative_state(2), 1e-2);
@@ -497,15 +483,18 @@
     const double velocity = length_plan_xva_[i](1);
 
     const Eigen::Matrix<double, 5, 1> goal_absolute_state =
-        trajectory_->GoalState(distance, velocity);
+        finished_trajectory_->GoalState(distance, velocity);
     const Eigen::Matrix<double, 5, 1> goal_relative_state =
-        trajectory_->StateToPathRelativeState(distance, goal_absolute_state);
+        finished_trajectory_->StateToPathRelativeState(distance,
+                                                       goal_absolute_state);
     const Eigen::Matrix<double, 5, 1> current_relative_state =
-        trajectory_->StateToPathRelativeState(distance, absolute_state);
+        finished_trajectory_->StateToPathRelativeState(distance,
+                                                       absolute_state);
 
-    const Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
+    const Eigen::Matrix<double, 2, 1> U_ff =
+        finished_trajectory_->FFVoltage(distance);
     Eigen::Matrix<double, 2, 1> U_fb =
-        trajectory_->GainForDistance(distance) *
+        finished_trajectory_->GainForDistance(distance) *
         (goal_relative_state - current_relative_state);
     if (i == 0) {
       initial_error = (goal_relative_state - current_relative_state).norm();
@@ -516,15 +505,15 @@
         [this](const Eigen::Matrix<double, 5, 1> &X,
                const Eigen::Matrix<double, 2, 1> &U) {
           return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
-                                    trajectory_->Tlr_to_la(), X, U);
+                                    dt_config_.Tlr_to_la(), X, U);
         },
         absolute_state, U, aos::time::DurationInSeconds(dt_config_.dt));
   }
 
-  EXPECT_LT(
-      (absolute_state - trajectory_->GoalState(trajectory_->length(), 0.0))
-          .norm(),
-      4e-2 + initial_error * 0.5);
+  EXPECT_LT((absolute_state - finished_trajectory_->GoalState(
+                                  finished_trajectory_->length(), 0.0))
+                .norm(),
+            4e-2 + initial_error * 0.5);
 }
 
 SplineTestParams MakeSplineTestParams(struct SplineTestParams params) {