diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index f63f685..04bdaaf 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -34,6 +34,29 @@
   double right_velocity;
 };
 
+// For logging information about the state of the trajectory planning.
+struct TrajectoryLogging {
+  // state of planning the trajectory.
+  //  0: not currently planning
+  //  1: received a multispline to plan
+  //  2: Built the spline and planning.
+  //  3: Finished the plan and ready to excecute.
+  int8_t planning_state;
+
+  // State of the spline execution.
+  bool is_executing;
+
+  int32_t current_spline_handle;
+  int32_t current_spline_idx;
+
+  // Expected position and velocity on the spline
+  float x;
+  float y;
+  float theta;
+  float left_velocity;
+  float right_velocity;
+};
+
 queue_group DrivetrainQueue {
   implements aos.control_loops.ControlLoop;
 
@@ -79,7 +102,7 @@
     .frc971.MultiSpline spline;
 
     // Which spline to follow.
-    uint32_t spline_handle;
+    int32_t spline_handle;
   };
 
   message Position {
@@ -159,6 +182,7 @@
     // Information about shifting logic and curent gear, for logging purposes
     GearLogging gear_logging;
     CIMLogging cim_logging;
+    TrajectoryLogging trajectory_logging;
   };
 
   queue Goal goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 363204c..707f136 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -74,10 +74,30 @@
                 drivetrain_motor_plant_.GetRightPosition(), 1e-3);
   }
 
-  void VerifyNearPosition(::Eigen::Matrix<double, 2, 1> expected) {
+  void VerifyNearSplineGoal() {
+    my_drivetrain_queue_.status.FetchLatest();
+    double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
+    double expected_y = my_drivetrain_queue_.status->trajectory_logging.y;
     auto actual = drivetrain_motor_plant_.GetPosition();
-    EXPECT_NEAR(actual(0), expected(0), 1e-2);
-    EXPECT_NEAR(actual(1), expected(1), 1e-2);
+    EXPECT_NEAR(actual(0), expected_x, 3e-3);
+    EXPECT_NEAR(actual(1), expected_y, 3e-3);
+  }
+
+  void WaitForTrajectoryPlan() {
+    do {
+      // Run for fewer iterations while the worker thread computes.
+      ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
+      RunIteration();
+      my_drivetrain_queue_.status.FetchLatest();
+    } while (my_drivetrain_queue_.status->trajectory_logging.planning_state !=
+        (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
+  }
+
+  void WaitForTrajectoryExecution() {
+    do {
+      RunIteration();
+      my_drivetrain_queue_.status.FetchLatest();
+    } while (my_drivetrain_queue_.status->trajectory_logging.is_executing);
   }
 
   virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
@@ -358,14 +378,101 @@
   goal.Send();
   RunIteration();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      start_goal = my_drivetrain_queue_.goal.MakeMessage();
   start_goal->controller_type = 2;
   start_goal->spline_handle = 1;
   start_goal.Send();
+  WaitForTrajectoryPlan();
 
   RunForTime(chrono::milliseconds(2000));
-  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+  VerifyNearSplineGoal();
+}
+
+// Tests that simple spline with a single goal message.
+TEST_F(DrivetrainTest, SplineSingleGoal) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal->spline_handle = 1;
+  goal.Send();
+  WaitForTrajectoryPlan();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearSplineGoal();
+}
+
+// Tests that a trajectory can be stopped in the middle.
+TEST_F(DrivetrainTest, SplineStop) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal->spline_handle = 1;
+  goal.Send();
+  WaitForTrajectoryPlan();
+
+  RunForTime(chrono::milliseconds(500));
+  my_drivetrain_queue_.status.FetchLatest();
+  const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
+  const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  stop_goal->controller_type = 2;
+  stop_goal->spline_handle = 0;
+  stop_goal.Send();
+  RunForTime(chrono::milliseconds(2000));
+
+  // The goal shouldn't change after being stopped.
+  my_drivetrain_queue_.status.FetchLatest();
+  EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
+  EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
+}
+
+// Tests that a spline can't be restarted.
+TEST_F(DrivetrainTest, SplineRestart) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal->spline_handle = 1;
+  goal.Send();
+  WaitForTrajectoryPlan();
+
+  RunForTime(chrono::milliseconds(500));
+  my_drivetrain_queue_.status.FetchLatest();
+  const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
+  const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  stop_goal->controller_type = 2;
+  stop_goal->spline_handle = 0;
+  stop_goal.Send();
+  RunForTime(chrono::milliseconds(500));
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> restart_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  restart_goal->controller_type = 2;
+  restart_goal->spline_handle = 1;
+  restart_goal.Send();
+  RunForTime(chrono::milliseconds(2000));
+
+  // The goal shouldn't change after being stopped and restarted.
+  my_drivetrain_queue_.status.FetchLatest();
+  EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
+  EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
 }
 
 // Tests that simple spline converges when it doesn't start where it thinks.
@@ -380,14 +487,15 @@
   goal.Send();
   RunIteration();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      start_goal = my_drivetrain_queue_.goal.MakeMessage();
   start_goal->controller_type = 2;
   start_goal->spline_handle = 1;
   start_goal.Send();
+  WaitForTrajectoryPlan();
 
   RunForTime(chrono::milliseconds(2000));
-  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+  VerifyNearSplineGoal();
 }
 
 // Tests that simple spline converges when it starts to the side of where it
@@ -403,14 +511,15 @@
   goal.Send();
   RunIteration();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      start_goal = my_drivetrain_queue_.goal.MakeMessage();
   start_goal->controller_type = 2;
   start_goal->spline_handle = 1;
   start_goal.Send();
+  WaitForTrajectoryPlan();
 
   RunForTime(chrono::milliseconds(2000));
-  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+  VerifyNearSplineGoal();
 }
 
 // Tests that a multispline converges on a goal.
@@ -420,19 +529,22 @@
   goal->controller_type = 2;
   goal->spline.spline_idx = 1;
   goal->spline.spline_count = 2;
-  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+  goal->spline.spline_x = {
+      {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+  goal->spline.spline_y = {
+      {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
   goal.Send();
   RunIteration();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      start_goal = my_drivetrain_queue_.goal.MakeMessage();
   start_goal->controller_type = 2;
   start_goal->spline_handle = 1;
   start_goal.Send();
+  WaitForTrajectoryPlan();
 
   RunForTime(chrono::milliseconds(4000));
-  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+  VerifyNearSplineGoal();
 }
 
 // Tests that several splines converges on a goal.
@@ -443,20 +555,23 @@
   goal->spline.spline_idx = 1;
   goal->spline.spline_count = 1;
   goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
-  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
   goal.Send();
   RunIteration();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      start_goal = my_drivetrain_queue_.goal.MakeMessage();
   start_goal->controller_type = 2;
   start_goal->spline_handle = 1;
   start_goal.Send();
-  RunForTime(chrono::milliseconds(2000));
-  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+  WaitForTrajectoryPlan();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_spline_goal =
-      my_drivetrain_queue_.goal.MakeMessage();
+  WaitForTrajectoryExecution();
+
+  VerifyNearSplineGoal();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
   second_spline_goal->controller_type = 2;
   second_spline_goal->spline.spline_idx = 2;
   second_spline_goal->spline.spline_count = 1;
@@ -465,14 +580,105 @@
   second_spline_goal.Send();
   RunIteration();
 
-  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_start_goal =
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
+  second_start_goal->controller_type = 2;
+  second_start_goal->spline_handle = 2;
+  second_start_goal.Send();
+  WaitForTrajectoryPlan();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearSplineGoal();
+}
+// Tests that a second spline will run if the first is stopped.
+TEST_F(DrivetrainTest, SplineStopFirst) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
       my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal->spline_handle = 1;
+  goal.Send();
+  WaitForTrajectoryPlan();
+
+  RunForTime(chrono::milliseconds(1000));
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  stop_goal->controller_type = 2;
+  stop_goal->spline_handle = 0;
+  stop_goal.Send();
+  RunForTime(chrono::milliseconds(500));
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
+  second_spline_goal->controller_type = 2;
+  second_spline_goal->spline.spline_idx = 2;
+  second_spline_goal->spline.spline_count = 1;
+  second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+  second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+  second_spline_goal->spline_handle = 2;
+  second_spline_goal.Send();
+  WaitForTrajectoryPlan();
+
+  WaitForTrajectoryExecution();
+  VerifyNearSplineGoal();
+}
+
+// Tests that splines can excecute and plan at the same time.
+TEST_F(DrivetrainTest, ParallelSplines) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+  goal.Send();
+  WaitForTrajectoryPlan();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
+  second_spline_goal->spline_handle = 1;
+  second_spline_goal->controller_type = 2;
+  second_spline_goal->spline.spline_idx = 2;
+  second_spline_goal->spline.spline_count = 1;
+  second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+  second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+  second_spline_goal.Send();
+  WaitForTrajectoryExecution();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+      second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
   second_start_goal->controller_type = 2;
   second_start_goal->spline_handle = 2;
   second_start_goal.Send();
 
-  RunForTime(chrono::milliseconds(2000));
-  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+  RunForTime(chrono::milliseconds(4000));
+  VerifyNearSplineGoal();
+}
+
+//Tests that a trajectory never told to execute will not replan.
+TEST_F(DrivetrainTest, OnlyPlanSpline) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+  goal.Send();
+  WaitForTrajectoryPlan();
+
+  for (int i = 0; i < 100; ++i) {
+    RunIteration();
+    my_drivetrain_queue_.status.FetchLatest();
+    EXPECT_EQ(my_drivetrain_queue_.status->trajectory_logging.planning_state, 3);
+    ::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
+  }
+  VerifyNearSplineGoal();
 }
 
 ::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 9aa1708..d8e2f9a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,38 +1,44 @@
 #include "frc971/control_loops/drivetrain/splinedrivetrain.h"
 
-#include <iostream>
-
 #include "Eigen/Dense"
 
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 
-const int kMaxSplineConstraints = 6;
-
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
 
 SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
-    : dt_config_(dt_config),
-      current_state_(::Eigen::Matrix<double, 2, 1>::Zero()) {}
+    : dt_config_(dt_config), new_goal_(&mutex_) {
+  worker_thread_ = std::thread(&SplineDrivetrain::ComputeTrajectory, this);
+}
 
 void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
-  bool output_was_capped = ::std::abs((*U)(0, 0)) > 12.0 ||
-                       ::std::abs((*U)(1, 0)) > 12.0;
+  output_was_capped_ =
+      ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
 
-  if (output_was_capped) {
+  if (output_was_capped_) {
     *U *= 12.0 / U->lpNorm<Eigen::Infinity>();
   }
 }
 
-// TODO(alex): put in another thread to avoid malloc in RT.
-void SplineDrivetrain::SetGoal(
-    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
-  current_spline_handle_ = goal.spline_handle;
-  const ::frc971::MultiSpline &multispline = goal.spline;
-  if (multispline.spline_idx) {
-    current_spline_idx_ = multispline.spline_idx;
+// TODO(alex): work on setting priority.
+void SplineDrivetrain::ComputeTrajectory() {
+  ::aos::MutexLocker locker(&mutex_);
+  while (run_) {
+    while (goal_.spline.spline_idx == future_spline_idx_) {
+      CHECK(!new_goal_.Wait());
+      if (!run_) {
+        return;
+      }
+    }
+    past_distance_spline_.reset();
+    past_trajectory_.reset();
+
+    plan_state_ = PlanState::kBuildingTrajectory;
+    const ::frc971::MultiSpline &multispline = goal_.spline;
+    future_spline_idx_ = multispline.spline_idx;
     auto x = multispline.spline_x;
     auto y = multispline.spline_y;
     ::std::vector<Spline> splines = ::std::vector<Spline>();
@@ -46,63 +52,100 @@
       splines.emplace_back(Spline(points));
     }
 
-    distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+    future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
         new DistanceSpline(::std::move(splines)));
 
-    current_trajectory_ = ::std::unique_ptr<Trajectory>(
-        new Trajectory(distance_spline_.get(), dt_config_));
+    future_trajectory_ = ::std::unique_ptr<Trajectory>(
+        new Trajectory(future_distance_spline_.get(), dt_config_));
 
-    for (int i = 0; i < kMaxSplineConstraints; ++i) {
+    for (size_t i = 0; i < multispline.constraints.size(); ++i) {
       const ::frc971::Constraint &constraint = multispline.constraints[i];
       switch (constraint.constraint_type) {
         case 0:
           break;
         case 1:
-          current_trajectory_->set_longitudal_acceleration(constraint.value);
+          future_trajectory_->set_longitudal_acceleration(constraint.value);
           break;
         case 2:
-          current_trajectory_->set_lateral_acceleration(constraint.value);
+          future_trajectory_->set_lateral_acceleration(constraint.value);
           break;
         case 3:
-          current_trajectory_->set_voltage_limit(constraint.value);
+          future_trajectory_->set_voltage_limit(constraint.value);
           break;
         case 4:
-          current_trajectory_->LimitVelocity(constraint.start_distance,
-                                             constraint.end_distance,
-                                             constraint.value);
+          future_trajectory_->LimitVelocity(constraint.start_distance,
+                                            constraint.end_distance,
+                                            constraint.value);
           break;
       }
     }
+    plan_state_ = PlanState::kPlanningTrajectory;
 
-    current_trajectory_->Plan();
-    current_xva_ = current_trajectory_->FFAcceleration(0);
-    current_xva_(1) = 0.0;
-    current_state_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+    future_trajectory_->Plan();
+    plan_state_ = PlanState::kPlannedTrajectory;
+  }
+}
+
+void SplineDrivetrain::SetGoal(
+    const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+  current_spline_handle_ = goal.spline_handle;
+  // If told to stop, set the executing spline to an invalid index.
+  if (current_spline_handle_ == 0) {
+    current_spline_idx_ = -1;
+  }
+
+  ::aos::Mutex::State mutex_state = mutex_.TryLock();
+  if (mutex_state == ::aos::Mutex::State::kLocked) {
+    if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
+      goal_ = goal;
+      new_goal_.Broadcast();
+    }
+    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_spline_idx_ = future_spline_idx_;
+
+      // Reset internal state to a trajectory start position.
+      current_xva_ = current_trajectory_->FFAcceleration(0);
+      current_xva_(1) = 0.0;
+    }
+    mutex_.Unlock();
   }
 }
 
 // TODO(alex): Hold position when done following the spline.
 // TODO(Austin): Compensate for voltage error.
-void SplineDrivetrain::Update(bool enable,
-                              const ::Eigen::Matrix<double, 5, 1> &state) {
+void SplineDrivetrain::Update(bool enable, const ::Eigen::Matrix<double, 5, 1> &state) {
+  next_U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
   enable_ = enable;
   if (enable && current_trajectory_) {
     ::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
-    if (!IsAtEnd()) {
+    if (!IsAtEnd() &&
+        current_spline_handle_ == current_spline_idx_) {
       // 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));
     }
+
     ::Eigen::Matrix<double, 2, 5> K =
         current_trajectory_->KForState(state, dt_config_.dt, Q, R);
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
     ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
     ::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_U_ = U_ff + U_fb;
     uncapped_U_ = next_U_;
     ScaleCapU(&next_U_);
-
-    next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &current_state_);
   }
 }
 
@@ -111,9 +154,6 @@
   if (!output) {
     return;
   }
-  if (!current_trajectory_) {
-    return;
-  }
   if (current_spline_handle_ == current_spline_idx_) {
     if (!IsAtEnd()) {
       output->left_voltage = next_U_(0);
@@ -121,6 +161,8 @@
       current_xva_ = next_xva_;
     }
   }
+  output->left_voltage = next_U_(0);
+  output->right_voltage = next_U_(1);
 }
 
 void SplineDrivetrain::PopulateStatus(
@@ -129,6 +171,22 @@
     status->uncapped_left_voltage = uncapped_U_(0);
     status->uncapped_right_voltage = uncapped_U_(1);
     status->robot_speed = current_xva_(1);
+    status->output_was_capped = output_was_capped_;
+  }
+
+  if (status) {
+    if (current_distance_spline_) {
+      ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
+      status->trajectory_logging.x = goal_state(0);
+      status->trajectory_logging.y = goal_state(1);
+      status->trajectory_logging.theta = goal_state(2);
+      status->trajectory_logging.left_velocity = goal_state(3);
+      status->trajectory_logging.right_velocity = goal_state(4);
+    }
+    status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
+    status->trajectory_logging.is_executing = !IsAtEnd();
+    status->trajectory_logging.current_spline_handle = current_spline_handle_;
+    status->trajectory_logging.current_spline_idx = current_spline_idx_;
   }
 }
 
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 2a79df6..c9ebbe4 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -1,8 +1,13 @@
 #ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
 
+#include <atomic>
+#include <thread>
+
 #include "Eigen/Dense"
 
+#include "aos/condition.h"
+#include "aos/mutex/mutex.h"
 #include "frc971/control_loops/drivetrain/distance_spline.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -17,40 +22,83 @@
  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::DrivetrainQueue::Goal &goal);
 
   void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
 
   void SetOutput(
       ::frc971::control_loops::DrivetrainQueue::Output *output);
-  // TODO(alex): What status do we need?
   void PopulateStatus(
       ::frc971::control_loops::DrivetrainQueue::Status *status) const;
 
   // Accessor for the current goal state, pretty much only present for debugging
   // purposes.
-  Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
-    return current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+  ::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();
   }
 
   bool IsAtEnd() const {
-    return current_trajectory_->is_at_end(current_state_);
+    return current_trajectory_
+        ? current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) :
+              true;
   }
+
+  enum class PlanState : int8_t {
+    kNoPlan = 0,
+    kBuildingTrajectory = 1,
+    kPlanningTrajectory = 2,
+    kPlannedTrajectory = 3,
+  };
+
  private:
+  void ComputeTrajectory();
   void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
 
   const DrivetrainConfig<double> dt_config_;
 
-  uint32_t current_spline_handle_; // Current spline told to excecute.
-  uint32_t current_spline_idx_;  // Current excecuting spline.
-  ::std::unique_ptr<DistanceSpline> distance_spline_;
-  ::std::unique_ptr<Trajectory> current_trajectory_;
-  ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
-  ::Eigen::Matrix<double, 2, 1> current_state_;
-  ::Eigen::Matrix<double, 2, 1> next_U_;
-  ::Eigen::Matrix<double, 2, 1> uncapped_U_;
+  int32_t current_spline_handle_ = 0;  // Current spline told to excecute.
+  int32_t current_spline_idx_ = 0;     // Current executing spline.
 
-  bool enable_;
+  ::std::unique_ptr<DistanceSpline> current_distance_spline_;
+  ::std::unique_ptr<Trajectory> current_trajectory_;
+
+  // State required to compute the next iteration's output.
+  ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
+  ::Eigen::Matrix<double, 2, 1> next_U_;
+
+  // Information used for status message.
+  ::Eigen::Matrix<double, 2, 1> uncapped_U_;
+  bool enable_ = false;
+  bool output_was_capped_ = false;
+
+  std::atomic<PlanState> plan_state_ = {PlanState::kNoPlan};
+
+  ::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;
+  ::frc971::control_loops::DrivetrainQueue::Goal 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_;
+  int32_t future_spline_idx_ = 0;  // Current spline being computed.
 
   // TODO(alex): pull this out of dt_config.
   const ::Eigen::DiagonalMatrix<double, 5> Q =