diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 7f87cca..432dd0e 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -303,3 +303,53 @@
         "@com_github_gflags_gflags//:gflags",
     ],
 )
+
+cc_library(
+    name = "trajectory",
+    srcs = ["trajectory.cc"],
+    hdrs = ["trajectory.h"],
+    deps = [
+        ":distance_spline",
+        ":drivetrain_config",
+        "//aos/logging:matrix_logging",
+        "//frc971/control_loops:c2d",
+        "//frc971/control_loops:dlqr",
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:runge_kutta",
+        "//frc971/control_loops:state_feedback_loop",
+        "//third_party/eigen",
+    ],
+)
+
+cc_binary(
+    name = "trajectory_plot",
+    srcs = [
+        "trajectory_plot.cc",
+    ],
+    restricted_to = ["//tools:k8"],
+    deps = [
+        ":distance_spline",
+        ":trajectory",
+        "//aos/logging:implementations",
+        "//aos/logging:matrix_logging",
+        "//aos/network:team_number",
+        "//third_party/eigen",
+        "//third_party/matplotlib-cpp",
+        "//y2016/control_loops/drivetrain:drivetrain_base",
+        "@com_github_gflags_gflags//:gflags",
+    ],
+)
+
+cc_test(
+    name = "trajectory_test",
+    srcs = [
+        "trajectory_test.cc",
+    ],
+    deps = [
+        ":trajectory",
+        "//aos/testing:googletest",
+        "//aos/testing:test_shm",
+        "//y2016:constants",
+        "//y2016/control_loops/drivetrain:polydrivetrain_plants",
+    ],
+)
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index 3c1824c..7dea1dc 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -43,6 +43,10 @@
     return spline_.DTheta(alpha) / spline_.DPoint(alpha).norm();
   }
 
+  double DThetaDt(double distance, double velocity) const {
+    return DTheta(distance) * velocity;
+  }
+
   // Returns the angular acceleration as a function of distance.
   double DDTheta(double distance) const;
 
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
new file mode 100644
index 0000000..2c07992
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -0,0 +1,378 @@
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+#include <chrono>
+
+#include "Eigen/Dense"
+#include "aos/logging/matrix_logging.h"
+#include "frc971/control_loops/dlqr.h"
+#include "frc971/control_loops/c2d.h"
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+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>>>(
+              new StateFeedbackLoop<2, 2, 2, double,
+                                    StateFeedbackHybridPlant<2, 2, 2>,
+                                    HybridKalman<2, 2, 2>>(
+                  config.make_hybrid_drivetrain_velocity_loop()))),
+      robot_radius_l_(config.robot_radius),
+      robot_radius_r_(config.robot_radius),
+      longitudal_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()),
+      plan_(num_distance, vmax) {}
+
+void Trajectory::LateralAccelPass() {
+  for (size_t i = 0; i < plan_.size(); ++i) {
+    const double distance = Distance(i);
+    plan_[i] = ::std::min(plan_[i], LateralVelocityCurvature(distance));
+  }
+}
+
+// TODO(austin): Deduplicate this potentially with the backward accel function.
+// Need to sort out how the max velocity limit is going to work since the
+// velocity and acceleration need to match at all points.
+// TODO(austin): Accel check the wheels instead of the center of mass.
+double Trajectory::ForwardAcceleration(const double x, const double v) {
+  ::Eigen::Matrix<double, 2, 1> K3;
+  ::Eigen::Matrix<double, 2, 1> K4;
+  ::Eigen::Matrix<double, 2, 1> K5;
+  K345(x, &K3, &K4, &K5);
+
+  const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
+  // Now, solve for all a's and find the best one which meets our criteria.
+  double maxa = -::std::numeric_limits<double>::infinity();
+  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()) {
+      maxa = ::std::max(maxa, a);
+    }
+  }
+
+  // Then, assume an acceleration oval and stay inside it.
+  const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
+  const double squared =
+      1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_, 2.0);
+  // If we would end up with an imaginary number, cap us at 0 acceleration.
+  // TODO(austin): Investigate when this happens, why, and fix it.
+  if (squared < 0.0) {
+    LOG(ERROR, "Imaginary %f, d %f\n", squared, x);
+    return 0.0;
+  }
+  const double longitudal_acceleration =
+      ::std::sqrt(squared) * longitudal_acceleration_;
+  return ::std::min(longitudal_acceleration, maxa);
+}
+
+void Trajectory::ForwardPass() {
+  plan_[0] = 0.0;
+  const double delta_distance = Distance(1) - Distance(0);
+  for (size_t i = 0; i < plan_.size() - 1; ++i) {
+    const double distance = Distance(i);
+
+    // Integrate our acceleration forward one step.
+    plan_[i + 1] = ::std::min(
+        plan_[i + 1],
+        IntegrateAccelForDistance(
+            [this](double x, double v) { return ForwardAcceleration(x, v); },
+            plan_[i], distance, delta_distance));
+  }
+}
+
+double Trajectory::BackwardAcceleration(double x, double v) {
+  ::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 mina = ::std::numeric_limits<double>::infinity();
+  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()) {
+      mina = ::std::min(mina, a);
+    }
+  }
+
+  // Then, assume an acceleration oval and stay inside it.
+  const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
+  const double squared =
+      1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_, 2.0);
+  // If we would end up with an imaginary number, cap us at 0 acceleration.
+  // TODO(austin): Investigate when this happens, why, and fix it.
+  if (squared < 0.0) {
+    LOG(ERROR, "Imaginary %f, d %f\n", squared, x);
+    return 0.0;
+  }
+  const double longitudal_acceleration =
+      -::std::sqrt(squared) * longitudal_acceleration_;
+  return ::std::max(longitudal_acceleration, mina);
+}
+
+void Trajectory::BackwardPass() {
+  const double delta_distance = Distance(0) - Distance(1);
+  plan_.back() = 0.0;
+  for (size_t i = plan_.size() - 1; i > 0; --i) {
+    const double distance = Distance(i);
+
+    // Integrate our deceleration back one step.
+    plan_[i - 1] = ::std::min(
+        plan_[i - 1],
+        IntegrateAccelForDistance(
+            [this](double x, double v) { return BackwardAcceleration(x, v); },
+            plan_[i], distance, delta_distance));
+  }
+}
+
+::Eigen::Matrix<double, 3, 1> Trajectory::FFAcceleration(double distance) {
+  size_t before_index;
+  size_t after_index;
+  if (distance < Distance(1)) {
+    // Within the first step.
+    after_index = 1;
+    // Make sure we don't end up off the beginning of the curve.
+    if (distance < 0.0) {
+      distance = 0.0;
+    }
+  } else if (distance > Distance(plan_.size() - 2)) {
+    // Within the last step.
+    after_index = plan_.size() - 1;
+    // Make sure we don't end up off the end of the curve.
+    if (distance > length()) {
+      distance = length();
+    }
+  } else {
+    // Otherwise do the calculation normally.
+    after_index = static_cast<size_t>(
+        ::std::ceil(distance / length() * (plan_.size() - 1)));
+  }
+  before_index = after_index - 1;
+  const double before_distance = Distance(before_index);
+  const double after_distance = Distance(after_index);
+
+  // Now, compute the velocity that we could have if we accelerated from the
+  // previous step and decelerated from the next step.  The min will tell us
+  // which is in effect.
+  const double velocity_forwards = IntegrateAccelForDistance(
+      [this](double x, double v) { return ForwardAcceleration(x, v); },
+      plan_[before_index], before_distance, distance - before_distance);
+  const double velocity_backward = IntegrateAccelForDistance(
+      [this](double x, double v) { return BackwardAcceleration(x, v); },
+      plan_[after_index], after_distance, distance - after_distance);
+
+  // And then also make sure we aren't curvature limited.
+  const double vcurvature = LateralVelocityCurvature(distance);
+
+  double acceleration;
+  double velocity;
+  if (vcurvature < velocity_forwards && vcurvature < velocity_backward) {
+    // If we are curvature limited, we can't accelerate.
+    velocity = vcurvature;
+    acceleration = 0.0;
+  } else if (velocity_forwards < velocity_backward) {
+    // Otherwise, pick the acceleration and velocity from the forward pass if it
+    // was the predominate factor in this step.
+    velocity = velocity_forwards;
+    acceleration = ForwardAcceleration(distance, velocity);
+  } else {
+    // Otherwise, pick the acceleration and velocity from the backward pass if
+    // it was the predominate factor in this step.
+    velocity = velocity_backward;
+    acceleration = BackwardAcceleration(distance, velocity);
+  }
+  return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
+      .finished();
+}
+
+::Eigen::Matrix<double, 2, 1> Trajectory::FFVoltage(double distance) {
+  const Eigen::Matrix<double, 3, 1> xva = FFAcceleration(distance);
+  const double velocity = xva(1);
+  const double acceleration = xva(2);
+  const double current_ddtheta = spline_->DDTheta(distance);
+  const double current_dtheta = spline_->DTheta(distance);
+  // 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
+  const ::Eigen::Matrix<double, 2, 1> K5 = B_inverse * my_K2;
+  const ::Eigen::Matrix<double, 2, 1> K3 = B_inverse * K1(current_ddtheta);
+  const ::Eigen::Matrix<double, 2, 1> K4 =
+      -B_inverse * velocity_drivetrain_->plant().coefficients().A_continuous *
+      my_K2;
+
+  return K5 * acceleration + K3 * velocity * velocity + K4 * velocity;
+}
+
+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));
+  }
+  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) {
+    LOG_MATRIX(INFO, "K", K);
+  } else {
+    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();
+  }
+  ::Eigen::EigenSolver<::Eigen::Matrix<double, 5, 5>> eigensolver(A - B * K);
+  const auto eigenvalues = eigensolver.eigenvalues();
+  LOG(DEBUG,
+      "Eigenvalues: (%f + %fj), (%f + %fj), (%f + %fj), (%f + %fj), (%f + "
+      "%fj)\n",
+      eigenvalues(0).real(), eigenvalues(0).imag(), eigenvalues(1).real(),
+      eigenvalues(1).imag(), eigenvalues(2).real(), eigenvalues(2).imag(),
+      eigenvalues(3).real(), eigenvalues(3).imag(), eigenvalues(4).real(),
+      eigenvalues(4).imag());
+  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;
+}
+
+::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
+    ::std::chrono::nanoseconds dt) {
+  double dt_float =
+      ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt).count();
+  double t = 0.0;
+  ::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;
+
+  while (state(0) < length() - 1e-4) {
+    t += dt_float;
+
+    // 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();
+        },
+        state, dt_float);
+
+    result.emplace_back(FFAcceleration(state(0)));
+    state(1) = result.back()(1);
+  }
+  return result;
+}
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
new file mode 100644
index 0000000..a42476a
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -0,0 +1,239 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_H_
+
+#include <chrono>
+
+#include "Eigen/Dense"
+#include "frc971/control_loops/drivetrain/distance_spline.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.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"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+template <typename F>
+double IntegrateAccelForDistance(const F &fn, double v, double x, double dx) {
+  // Use a trick from
+  // https://www.johndcook.com/blog/2012/02/21/care-and-treatment-of-singularities/
+  const double a0 = fn(x, v);
+
+  return (RungeKutta(
+              [&fn, &a0](double t, double y) {
+                // Since we know that a0 == a(0) and that they are asymtotically
+                // the same at 0, we know that the limit is 0 at 0.  This is
+                // 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) {
+                  return 0.0;
+                }
+                return (fn(t, y) - a0) / y;
+              },
+              v, x, dx) -
+          v) +
+         ::std::sqrt(2.0 * a0 * dx + v * v);
+}
+
+// Class to plan and hold the velocity plan for a spline.
+class Trajectory {
+ public:
+  Trajectory(const DistanceSpline *spline,
+             const DrivetrainConfig<double> &config,
+             double vmax = 10.0, int num_distance = 500);
+  // Sets the plan longitudal acceleration limit
+  void set_longitudal_acceleration(double longitudal_acceleration) {
+    longitudal_acceleration_ = longitudal_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;
+  }
+
+  // Returns the velocity limit for a defined distance.
+  double LateralVelocityCurvature(double distance) const {
+    return ::std::sqrt(lateral_acceleration_ / spline_->DDXY(distance).norm());
+  }
+
+  // Runs the lateral acceleration (curvature) pass on the plan.
+  void LateralAccelPass();
+
+  // Returns the forward acceleration for a distance along the spline taking
+  // into account the lateral acceleration, longitudinal acceleration, and
+  // voltage limits.
+  double ForwardAcceleration(const double x, const double v);
+
+  // Runs the forwards pass, setting the starting velocity to 0 m/s
+  void ForwardPass();
+
+  // Returns the backwards acceleration for a distance along the spline taking
+  // into account the lateral acceleration, longitudinal acceleration, and
+  // voltage limits.
+  double BackwardAcceleration(double x, double v);
+
+  // Runs the forwards pass, setting the ending velocity to 0 m/s
+  void BackwardPass();
+
+  // Runs all the planning passes.
+  void Plan() {
+    LateralAccelPass();
+    ForwardPass();
+    BackwardPass();
+  }
+
+  // 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 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;
+  // 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.
+  // 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;
+
+  ::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
+      ::std::chrono::nanoseconds dt);
+
+ private:
+  // Computes alpha for a distance.
+  double DistanceToAlpha(double distance) const;
+
+  // 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();
+  }
+
+  // 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;
+  }
+
+  // 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_;
+
+  // Left and right robot radiuses.
+  const double robot_radius_l_;
+  const double robot_radius_r_;
+  // Acceleration limits.
+  double longitudal_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_;
+  // Plan voltage limit.
+  double voltage_limit_ = 12.0;
+};
+
+// 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(
+    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 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),
+          (velocity_drivetrain.coefficients().A_continuous * velocity +
+           velocity_drivetrain.coefficients().B_continuous * U))
+      .finished();
+}
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_H_
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
new file mode 100644
index 0000000..d9c12f4
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -0,0 +1,241 @@
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+#include <chrono>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/matrix_logging.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/dlqr.h"
+#include "gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2016/control_loops/drivetrain/drivetrain_base.h"
+
+// Notes:
+//   Basic ideas from spline following are from Jared Russell and
+//   http://msc.fe.uni-lj.si/Papers/Chapter10_MobileRobotsNewResearch_Lepetic2005.pdf
+//
+// For the future, I'd like to use the following to measure distance to the path.
+//   http://home.eps.hw.ac.uk/~ab226/papers/dist.pdf
+//
+// LQR controller was inspired by
+// https://calhoun.nps.edu/bitstream/handle/10945/40159/kanayama_a_stable.pdf
+//
+// I ended up just taking the jacobian of the dynamics.  That gives me a tangent
+// plane to design a LQR controller around.  That works because we have a good
+// feed forwards and a good idea where the robot will be next time so we only
+// need to handle disturbances.
+//
+// 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");
+DEFINE_double(dt, 0.0, "Amount to disturb theta at the start");
+DEFINE_double(dvl, 0.0, "Amount to disturb vl at the start");
+DEFINE_double(dvr, 0.0, "Amount to disturb vr at the start");
+
+DEFINE_double(forward, 1.0, "Amount to drive forwards");
+
+namespace chrono = ::std::chrono;
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+void Main() {
+  DistanceSpline distance_spline(
+      Spline((::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()));
+  Trajectory trajectory(
+      &distance_spline,
+      ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
+  trajectory.set_lateral_acceleration(2.0);
+  trajectory.set_longitudal_acceleration(1.0);
+
+  // Grab the spline.
+  ::std::vector<double> distances = trajectory.Distances();
+  ::std::vector<double> spline_x;
+  ::std::vector<double> spline_y;
+  ::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);
+    spline_x.push_back(point(0));
+    spline_y.push_back(point(1));
+    spline_theta.push_back(theta);
+  }
+
+  // Compute the velocity plan.
+  ::aos::monotonic_clock::time_point start_time = ::aos::monotonic_clock::now();
+  ::std::vector<double> initial_plan = trajectory.plan();
+  trajectory.LateralAccelPass();
+  ::std::vector<double> curvature_plan = trajectory.plan();
+  trajectory.ForwardPass();
+  ::std::vector<double> forward_plan = trajectory.plan();
+  trajectory.BackwardPass();
+
+  ::aos::monotonic_clock::time_point end_time = ::aos::monotonic_clock::now();
+  ::std::vector<double> backward_plan = trajectory.plan();
+
+  LOG(INFO, "Took %fms to plan\n",
+      chrono::duration_cast<chrono::duration<double, ::std::milli>>(end_time -
+                                                                    start_time)
+          .count());
+
+  // Now, compute the xva as a function of time.
+  ::std::vector<double> length_plan_t;
+  ::std::vector<double> length_plan_x;
+  ::std::vector<double> length_plan_v;
+  ::std::vector<double> length_plan_a;
+  ::std::vector<double> length_plan_vl;
+  ::std::vector<double> length_plan_vr;
+  const chrono::nanoseconds kDt = chrono::microseconds(5050);
+  const double kDtDouble =
+      ::std::chrono::duration_cast<::std::chrono::duration<double>>(kDt)
+          .count();
+  {
+    ::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva =
+        trajectory.PlanXVA(kDt);
+    for (size_t i = 0; i < length_plan_xva.size(); ++i) {
+      length_plan_t.push_back(i * kDtDouble);
+      length_plan_x.push_back(length_plan_xva[i](0));
+      length_plan_v.push_back(length_plan_xva[i](1));
+      length_plan_a.push_back(length_plan_xva[i](2));
+
+      ::Eigen::Matrix<double, 2, 1> U =
+          trajectory.FFVoltage(length_plan_xva[i](0));
+      length_plan_vl.push_back(U(0));
+      length_plan_vr.push_back(U(1));
+    }
+  }
+
+  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();
+
+  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();
+
+  ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+  state(0, 0) = FLAGS_dx;
+  state(1, 0) = FLAGS_dy;
+  state(2, 0) = FLAGS_dt;
+  state(3, 0) = FLAGS_dvl;
+  state(4, 0) = FLAGS_dvr;
+  ::std::vector<double> simulation_t = length_plan_t;
+  ::std::vector<double> simulation_x;
+  ::std::vector<double> error_x;
+  ::std::vector<double> simulation_y;
+  ::std::vector<double> error_y;
+  ::std::vector<double> simulation_theta;
+  ::std::vector<double> error_theta;
+  ::std::vector<double> simulation_velocity_l;
+  ::std::vector<double> error_velocity_l;
+  ::std::vector<double> simulation_velocity_r;
+  ::std::vector<double> error_velocity_r;
+  ::std::vector<double> simulation_ul;
+  ::std::vector<double> simulation_ur;
+  for (size_t i = 0; i < length_plan_t.size(); ++i) {
+    const double distance = length_plan_x[i];
+    const double velocity = length_plan_v[i];
+    const ::Eigen::Matrix<double, 5, 1> goal_state =
+        trajectory.GoalState(distance, velocity);
+
+    const ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+
+    simulation_x.push_back(state(0));
+    simulation_y.push_back(state(1));
+    simulation_theta.push_back(state(2));
+    simulation_velocity_l.push_back(state(3));
+    simulation_velocity_r.push_back(state(4));
+
+    error_x.push_back(state_error(0));
+    error_y.push_back(state_error(1));
+    error_theta.push_back(state_error(2));
+    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, 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) {
+          return ContinuousDynamics(trajectory.velocity_drivetrain().plant(),
+                                    trajectory.Tlr_to_la(), X, U);
+        },
+        state, U, kDtDouble);
+
+    simulation_ul.push_back(U(0));
+    simulation_ur.push_back(U(1));
+  }
+
+  if (FLAGS_plot) {
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(distances, backward_plan, {{"label", "backward"}});
+    matplotlibcpp::plot(distances, forward_plan, {{"label", "forward"}});
+    matplotlibcpp::plot(distances, curvature_plan, {{"label", "lateral"}});
+    matplotlibcpp::plot(distances, initial_plan, {{"label", "initial"}});
+    matplotlibcpp::legend();
+
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(length_plan_t, length_plan_x, {{"label", "x"}});
+    matplotlibcpp::plot(length_plan_t, length_plan_v, {{"label", "v"}});
+    matplotlibcpp::plot(length_plan_t, length_plan_a, {{"label", "a"}});
+    matplotlibcpp::plot(length_plan_t, length_plan_vl, {{"label", "Vl"}});
+    matplotlibcpp::plot(length_plan_t, length_plan_vr, {{"label", "Vr"}});
+    matplotlibcpp::legend();
+
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(length_plan_t, error_x, {{"label", "x error"}});
+    matplotlibcpp::plot(length_plan_t, error_y, {{"label", "y error"}});
+    matplotlibcpp::plot(length_plan_t, error_theta, {{"label", "theta error"}});
+    matplotlibcpp::plot(length_plan_t, error_velocity_l,
+                        {{"label", "velocityl error"}});
+    matplotlibcpp::plot(length_plan_t, error_velocity_r,
+                        {{"label", "velocityr error"}});
+    matplotlibcpp::legend();
+
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(spline_x, spline_y, {{"label", "spline"}});
+    matplotlibcpp::plot(simulation_x, simulation_y, {{"label", "robot"}});
+    matplotlibcpp::legend();
+
+    matplotlibcpp::show();
+  }
+}
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
+
+int main(int argc, char **argv) {
+  ::gflags::ParseCommandLineFlags(&argc, &argv, false);
+  ::aos::logging::Init();
+  ::aos::network::OverrideTeamNumber(971);
+  ::frc971::control_loops::drivetrain::Main();
+  return 0;
+}
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
new file mode 100644
index 0000000..0d3a6db
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -0,0 +1,179 @@
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+#include <chrono>
+#include <vector>
+
+#include "aos/testing/test_shm.h"
+#include "gtest/gtest.h"
+#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+namespace chrono = ::std::chrono;
+
+double a(double /*v*/, double /*x*/) { return 2.0; }
+
+// Tests that the derivitives of xy integrate back up to the position.
+TEST(IntegrateAccelForDistanceTest, IntegrateAccelForDistance) {
+  double v = 0.0;
+  const size_t count = 10;
+  const double dx = 4.0 / static_cast<double>(count);
+  for (size_t i = 0; i < count; ++i) {
+    v = IntegrateAccelForDistance(a, v, 0.0, dx);
+  }
+  EXPECT_NEAR(4.0, v, 1e-8);
+}
+
+const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25,
+                                                           0.75};
+
+// TODO(austin): factor this out of drivetrain_lib_test.cc
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+  static DrivetrainConfig<double> kDrivetrainConfig{
+      ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+      ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+      ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+      IMUType::IMU_X,
+      ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
+      ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+      ::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
+      ::y2016::control_loops::drivetrain::MakeHybridVelocityDrivetrainLoop,
+
+      chrono::duration_cast<chrono::nanoseconds>(
+          chrono::duration<double>(::y2016::control_loops::drivetrain::kDt)),
+      ::y2016::control_loops::drivetrain::kRobotRadius,
+      ::y2016::control_loops::drivetrain::kWheelRadius,
+      ::y2016::control_loops::drivetrain::kV,
+
+      ::y2016::control_loops::drivetrain::kHighGearRatio,
+      ::y2016::control_loops::drivetrain::kLowGearRatio,
+      kThreeStateDriveShifter,
+      kThreeStateDriveShifter,
+      false,
+      0,
+
+      0.25,
+      1.00,
+      1.00};
+
+  return kDrivetrainConfig;
+};
+
+class SplineTest : public ::testing::Test {
+ protected:
+  ::aos::testing::TestSharedMemory shm_;
+  static constexpr chrono::nanoseconds kDt =
+      chrono::duration_cast<chrono::nanoseconds>(
+          chrono::duration<double>(::y2016::control_loops::drivetrain::kDt));
+
+  DistanceSpline distance_spline_;
+  Trajectory trajectory_;
+  ::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
+
+  SplineTest()
+      : distance_spline_(Spline((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
+                                 -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
+                                    .finished())),
+        trajectory_(&distance_spline_, GetDrivetrainConfig()) {
+    trajectory_.set_lateral_acceleration(2.0);
+    trajectory_.set_longitudal_acceleration(1.0);
+    trajectory_.LateralAccelPass();
+    trajectory_.ForwardPass();
+    trajectory_.BackwardPass();
+    length_plan_xva_ = trajectory_.PlanXVA(kDt);
+  }
+
+  static constexpr double kXPos = 0.05;
+  static constexpr double kYPos = 0.05;
+  static constexpr double kThetaPos = 0.2;
+  static constexpr double kVel = 0.5;
+  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();
+
+  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();
+};
+
+constexpr chrono::nanoseconds SplineTest::kDt;
+
+// Tests that following a spline with feed forwards only gets pretty darn close
+// to the right point.
+TEST_F(SplineTest, FFSpline) {
+  ::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 ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_.FFVoltage(distance);
+    const ::Eigen::Matrix<double, 2, 1> U = U_ff;
+    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, ::y2016::control_loops::drivetrain::kDt);
+  }
+
+  EXPECT_LT((state - trajectory_.GoalState(trajectory_.length(), 0.0)).norm(),
+            2e-2);
+}
+
+// Tests that following a spline with both feed forwards and feed back gets
+// pretty darn close to the right point.
+TEST_F(SplineTest, 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, SplineTest::kDt, 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;
+    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, ::y2016::control_loops::drivetrain::kDt);
+  }
+
+  EXPECT_LT((state - trajectory_.GoalState(trajectory_.length(), 0.0)).norm(),
+            1.1e-2);
+}
+
+// TODO(austin): Try a velocity limited plan at some point.
+//
+// TODO(austin): Handle saturation.  254 does this by just not going that
+// fast...  We want to maybe replan when we get behind, or something.  Maybe
+// stop moving the setpoint like our 2018 arm?
+
+}  // namespace testing
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index 585d6b8..d045f45 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -491,11 +491,8 @@
         accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
                          K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
                          (-12.0 - C[1, 0]) / K5[1, 0]]
-        maxa = 0.0
+        maxa = -float('inf')
         for a in accelerations:
-            if a < 0.0:
-                continue
-
             U = K5 * a + K3 * v * v + K4 * v
             if not (numpy.abs(U) > 12.0 + 1e-6).any():
                 maxa = max(maxa, a)
@@ -537,11 +534,8 @@
         accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
                          K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
                          (-12.0 - C[1, 0]) / K5[1, 0]]
-        mina = 0.0
+        mina = float('inf')
         for a in accelerations:
-            if a > 0.0:
-                continue
-
             U = K5 * a + K3 * v * v + K4 * v
             if not (numpy.abs(U) > 12.0 + 1e-6).any():
                 mina = min(mina, a)