diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index aee3a37..d95e101 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -49,6 +49,23 @@
 )
 
 cc_library(
+    name = "splinedrivetrain",
+    srcs = [
+        "splinedrivetrain.cc",
+    ],
+    hdrs = [
+        "splinedrivetrain.h",
+    ],
+    deps = [
+        ":drivetrain_config",
+        ":drivetrain_queue",
+        ":spline",
+        ":distance_spline",
+        ":trajectory",
+    ]
+)
+
+cc_library(
     name = "ssdrivetrain",
     srcs = [
         "ssdrivetrain.cc",
@@ -170,6 +187,7 @@
         ":gear",
         ":polydrivetrain",
         ":ssdrivetrain",
+        ":splinedrivetrain",
         "//aos/controls:control_loop",
         "//aos/logging:matrix_logging",
         "//aos/logging:queue_logging",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a50b429..ec9faaa 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -38,6 +38,7 @@
       kf_(dt_config_.make_kf_drivetrain_loop()),
       dt_openloop_(dt_config_, &kf_),
       dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+      dt_spline_(dt_config_),
       down_estimator_(MakeDownEstimatorLoop()),
       left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
       right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
@@ -329,6 +330,7 @@
 
     dt_closedloop_.SetGoal(*goal);
     dt_openloop_.SetGoal(*goal);
+    dt_spline_.SetGoal(*goal);
   }
 
   dt_openloop_.Update(robot_state().voltage_battery);
@@ -342,6 +344,9 @@
     case 1:
       dt_closedloop_.SetOutput(output);
       break;
+    case 2:
+      dt_spline_.SetOutput(output);
+      break;
   }
 
   // The output should now contain the shift request.
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a357eab..05bf711 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -11,6 +11,7 @@
 #include "frc971/control_loops/drivetrain/gear.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -50,6 +51,7 @@
   StateFeedbackLoop<7, 2, 4> kf_;
   PolyDrivetrain<double> dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
+  SplineDrivetrain dt_spline_;
   ::aos::monotonic_clock::time_point last_gyro_time_ =
       ::aos::monotonic_clock::min_time;
 
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index c66b03e..f63f685 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -72,6 +72,14 @@
     // The control loop will profile if these are all non-zero.
     .frc971.ProfileParameters linear;
     .frc971.ProfileParameters angular;
+
+    // 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.
+    .frc971.MultiSpline spline;
+
+    // Which spline to follow.
+    uint32_t spline_handle;
   };
 
   message Position {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
new file mode 100644
index 0000000..11a4f69
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -0,0 +1,90 @@
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+
+#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) {}
+
+// 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;
+    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));
+    }
+
+    distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+        new DistanceSpline(::std::move(splines)));
+
+    current_trajectory_ = ::std::unique_ptr<Trajectory>(
+        new Trajectory(distance_spline_.get(), dt_config_));
+
+    for (int i = 0; i < kMaxSplineConstraints; ++i) {
+      const ::frc971::Constraint &constraint = multispline.constraints[i];
+      switch (constraint.constraint_type) {
+        case 0:
+          break;
+        case 1:
+          current_trajectory_->set_longitudal_acceleration(constraint.value);
+          break;
+        case 2:
+          current_trajectory_->set_lateral_acceleration(constraint.value);
+          break;
+        case 3:
+          current_trajectory_->set_voltage_limit(constraint.value);
+          break;
+        case 4:
+          current_trajectory_->LimitVelocity(constraint.start_distance,
+                                             constraint.end_distance,
+                                             constraint.value);
+          break;
+      }
+    }
+
+    current_trajectory_->Plan();
+    current_xva_plan_ = current_trajectory_->PlanXVA(dt_config_.dt);
+  }
+}
+
+// TODO(alex): Handle drift.
+void SplineDrivetrain::SetOutput(
+    ::frc971::control_loops::DrivetrainQueue::Output *output) {
+  if (!output) {
+    return;
+  }
+  if (current_spline_handle_ == current_spline_idx_) {
+    if (current_xva_idx_ < current_xva_plan_.size()) {
+      double current_distance = current_xva_plan_[current_xva_idx_](0);
+      ::Eigen::Matrix<double, 2, 1> FFVoltage =
+          current_trajectory_->FFVoltage(current_distance);
+      output->left_voltage = FFVoltage(0);
+      output->right_voltage = FFVoltage(1);
+      ++current_xva_idx_;
+    }
+  }
+}
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
new file mode 100644
index 0000000..b311e14
--- /dev/null
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -0,0 +1,40 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_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"
+#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+class SplineDrivetrain {
+ public:
+  SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
+
+  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+
+  void SetOutput(
+      ::frc971::control_loops::DrivetrainQueue::Output *output);
+  // TODO(alex): What status do we need?
+  void PopulateStatus(
+      ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ private:
+  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_;
+  ::std::vector<::Eigen::Matrix<double, 3, 1>> current_xva_plan_;
+  size_t current_xva_idx_;
+};
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_