diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index e332cfe..7dedc71 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -101,6 +101,43 @@
 )
 
 cc_library(
+    name = "line_follow_drivetrain",
+    srcs = [
+        "line_follow_drivetrain.cc",
+    ],
+    hdrs = [
+        "line_follow_drivetrain.h",
+    ],
+    deps = [
+        ":drivetrain_config",
+        ":drivetrain_queue",
+        "//aos:math",
+        "//aos/util:math",
+        "//frc971/control_loops:c2d",
+        "//frc971/control_loops:dlqr",
+        "//frc971/control_loops:pose",
+        "//third_party/eigen",
+    ],
+)
+
+cc_test(
+    name = "line_follow_drivetrain_test",
+    srcs = ["line_follow_drivetrain_test.cc"],
+    linkstatic = True,
+    restricted_to = ["//tools:k8"],
+    deps = [
+        ":drivetrain_config",
+        ":drivetrain_test_lib",
+        ":line_follow_drivetrain",
+        ":trajectory",
+        "//aos/testing:googletest",
+        "//aos/testing:test_shm",
+        "//third_party/matplotlib-cpp",
+        "@com_github_gflags_gflags//:gflags",
+    ],
+)
+
+cc_library(
     name = "ssdrivetrain",
     srcs = [
         "ssdrivetrain.cc",
@@ -221,6 +258,7 @@
         ":down_estimator",
         ":drivetrain_queue",
         ":gear",
+        ":line_follow_drivetrain",
         ":localizer",
         ":polydrivetrain",
         ":splinedrivetrain",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 19e604d..2bfaaf5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,6 +41,7 @@
       dt_openloop_(dt_config_, &kf_),
       dt_closedloop_(dt_config_, &kf_, localizer_),
       dt_spline_(dt_config_),
+      dt_line_follow_(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),
@@ -247,17 +248,26 @@
     dt_closedloop_.SetGoal(*goal);
     dt_openloop_.SetGoal(*goal);
     dt_spline_.SetGoal(*goal);
+    // TODO(james): Use a goal other than zero (which is what Pose default
+    // constructs to). Need to query the EKF in some way to determine what the
+    // logical target is.
+    const LineFollowDrivetrain::Pose goal_pose;
+    dt_line_follow_.SetGoal(*goal, goal_pose);
   }
 
   dt_openloop_.Update(robot_state().voltage_battery);
 
   dt_closedloop_.Update(output != NULL && controller_type == 1);
 
-  dt_spline_.Update(output != NULL && controller_type == 2,
-                    (Eigen::Matrix<double, 5, 1>() << localizer_->x(),
-                     localizer_->y(), localizer_->theta(),
-                     localizer_->left_velocity(), localizer_->right_velocity())
-                        .finished());
+  const Eigen::Matrix<double, 5, 1> trajectory_state =
+      (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
+       localizer_->theta(), localizer_->left_velocity(),
+       localizer_->right_velocity())
+          .finished();
+
+  dt_spline_.Update(output != NULL && controller_type == 2, trajectory_state);
+
+  dt_line_follow_.Update(trajectory_state);
 
   switch (controller_type) {
     case 0:
@@ -269,6 +279,9 @@
     case 2:
       dt_spline_.SetOutput(output);
       break;
+    case 3:
+      dt_line_follow_.SetOutput(output);
+      break;
   }
 
   // The output should now contain the shift request.
@@ -310,6 +323,7 @@
     dt_openloop_.PopulateStatus(status);
     dt_closedloop_.PopulateStatus(status);
     dt_spline_.PopulateStatus(status);
+    dt_line_follow_.PopulateStatus(status);
   }
 
   double left_voltage = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 1aa3e13..35d6084 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -12,6 +12,7 @@
 #include "frc971/control_loops/drivetrain/localizer.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
 #include "frc971/control_loops/drivetrain/splinedrivetrain.h"
 
 namespace frc971 {
@@ -50,6 +51,7 @@
   PolyDrivetrain<double> dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
   SplineDrivetrain dt_spline_;
+  LineFollowDrivetrain dt_line_follow_;
   ::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 04bdaaf..66e9c32 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -81,7 +81,8 @@
     // Type of controller in charge of the drivetrain.
     //  0: polydrive
     //  1: motion profiled position drive (statespace)
-    //  2: spline follower.
+    //  2: spline follower
+    //  3: line follower (for guiding into a target)
     uint8_t controller_type;
 
     // Position goals for each drivetrain side (in meters) when the
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 7ce34e5..2917bc6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -719,6 +719,21 @@
   VerifyNearSplineGoal();
 }
 
+// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
+// tests that the integration with drivetrain_lib worked properly.
+TEST_F(DrivetrainTest, BasicLineFollow) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 3;
+  goal->throttle = 0.5;
+  goal.Send();
+
+  RunForTime(chrono::seconds(5));
+  // Should be on the x-axis, with y = 0 and x having increased:
+  EXPECT_LT(0.5, drivetrain_motor_plant_.GetPosition().x());
+  EXPECT_NEAR(0.0, drivetrain_motor_plant_.GetPosition().y(), 1e-4);
+}
+
 ::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
                                              double x2_min, double x2_max) {
   Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
new file mode 100644
index 0000000..e9a6c70
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -0,0 +1,212 @@
+#include <iostream>
+#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
+
+#include "aos/commonmath.h"
+#include "aos/util/math.h"
+#include "frc971/control_loops/c2d.h"
+#include "frc971/control_loops/dlqr.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+constexpr double LineFollowDrivetrain::kMaxVoltage;
+constexpr double LineFollowDrivetrain::kPolyN;
+
+namespace {
+::Eigen::Matrix<double, 3, 3> AContinuous(
+    const DrivetrainConfig<double> &dt_config) {
+  ::Eigen::Matrix<double, 3, 3> result;
+  result.setZero();
+  result(0, 2) = 1.0;
+  result.block<2, 2>(1, 1) = dt_config.Tlr_to_la() *
+                             dt_config.make_hybrid_drivetrain_velocity_loop()
+                                 .plant()
+                                 .coefficients()
+                                 .A_continuous *
+                             dt_config.Tla_to_lr();
+  return result;
+}
+::Eigen::Matrix<double, 3, 2> BContinuous(
+    const DrivetrainConfig<double> &dt_config) {
+  ::Eigen::Matrix<double, 3, 2> result;
+  result.setZero();
+  result.block<2, 2>(1, 0) = dt_config.Tlr_to_la() *
+                             dt_config.make_hybrid_drivetrain_velocity_loop()
+                                 .plant()
+                                 .coefficients()
+                                 .B_continuous;
+  return result;
+}
+void AB(const DrivetrainConfig<double> &dt_config,
+        ::Eigen::Matrix<double, 3, 3> *A, ::Eigen::Matrix<double, 3, 2> *B) {
+  controls::C2D(AContinuous(dt_config), BContinuous(dt_config), dt_config.dt, A,
+                B);
+}
+
+::Eigen::Matrix<double, 3, 3> ADiscrete(
+    const DrivetrainConfig<double> &dt_config) {
+  ::Eigen::Matrix<double, 3, 3> ADiscrete;
+  ::Eigen::Matrix<double, 3, 2> BDiscrete;
+  AB(dt_config, &ADiscrete, &BDiscrete);
+  return ADiscrete;
+}
+
+::Eigen::Matrix<double, 3, 2> BDiscrete(
+    const DrivetrainConfig<double> &dt_config) {
+  ::Eigen::Matrix<double, 3, 3> ADiscrete;
+  ::Eigen::Matrix<double, 3, 2> BDiscrete;
+  AB(dt_config, &ADiscrete, &BDiscrete);
+  return BDiscrete;
+}
+
+::Eigen::Matrix<double, 2, 3> CalcK(
+    const ::Eigen::Matrix<double, 3, 3> & A,
+    const ::Eigen::Matrix<double, 3, 2> & B,
+    const ::Eigen::Matrix<double, 3, 3> & Q,
+    const ::Eigen::Matrix<double, 2, 2> & R) {
+  Eigen::Matrix<double, 2, 3> K;
+  Eigen::Matrix<double, 3, 3> S;
+  int info = ::frc971::controls::dlqr<3, 2>(A, B, Q, R, &K, &S);
+  if (info != 0) {
+    // We allow a FATAL error here since this should only be called during
+    // initialization.
+    LOG(FATAL, "Failed to solve %d, controllability: %d\n", info,
+        controls::Controllability(A, B));
+  }
+  return K;
+}
+
+::Eigen::Matrix<double, 2, 3> CalcKff(const ::Eigen::Matrix<double, 3, 2> &B) {
+  ::Eigen::Matrix<double, 2, 3> Kff;
+  Kff.setZero();
+  Kff.block<2, 2>(0, 1) = B.block<2, 2>(1, 0).inverse();
+  return Kff;
+}
+
+}  // namespace
+
+// When we create A/B, we do recompute A/B, but we don't really care about
+// optimizing it since it is only happening at initialization time...
+LineFollowDrivetrain::LineFollowDrivetrain(
+    const DrivetrainConfig<double> &dt_config)
+    : dt_config_(dt_config),
+      A_d_(ADiscrete(dt_config_)),
+      B_d_(BDiscrete(dt_config_)),
+      K_(CalcK(A_d_, B_d_, Q_, R_)),
+      Kff_(CalcKff(B_d_)) {}
+
+double LineFollowDrivetrain::GoalTheta(
+    const ::Eigen::Matrix<double, 5, 1> &state) const {
+  // TODO(james): Consider latching the sign of the goal so that the driver
+  // can back up without the robot trying to turn around...
+  // On the other hand, we may just want to force the driver to take over
+  // entirely if they need to backup.
+  int sign = ::aos::sign(goal_velocity_);
+  // Given a Nth degree polynomial with just a single term that
+  // has its minimum (with a slope of zero) at (0, 0) and passing
+  // through (x0, y0), we will have:
+  // y = a * x^N
+  // a = y0 / x0^N
+  // And the slope of the tangent line at (x0, y0) will be
+  // N * a * x0^(N-1)
+  // = N * y0 / x0^N * x0^(N-1)
+  // = N * y0 / x0
+  // Giving a heading of
+  // atan2(-N * y0, -x0)
+  // where we add the negative signs to make things work out properly when we
+  // are trying to drive forwards towards zero. We reverse the sign of both
+  // terms if we want to drive backwards.
+  return ::std::atan2(-sign * kPolyN * state.y(), -sign * state.x());
+}
+
+double LineFollowDrivetrain::GoalThetaDot(
+    const ::Eigen::Matrix<double, 5, 1> &state) const {
+  // theta = atan2(-N * y0, -x0)
+  // Note: d(atan2(p(t), q(t)))/dt
+  //       = (p(t) * q'(t) - q(t) * p'(t)) / (p(t)^2 + q(t)^2)
+  // Note: x0' = cos(theta) * v, y0' = sin(theta) * v
+  // Note that for the sin/cos calculations we discard the
+  //   negatives in the arctangent to make the signs work out (the
+  //   dtheta/dt needs to be correct as we travel along the path). This
+  //   also corresponds with the fact that thetadot is agnostic towards
+  //   whether the robot is driving forwards or backwards, so long as it is
+  //   trying to drive towards the target.
+  // Note: sin(theta) = sin(atan2(N * y0, x0))
+  //       = N * y0 / sqrt(N^2 * y0^2 + x0^2)
+  // Note: cos(theta) = cos(atan2(N * y0, x0))
+  //       = x0 / sqrt(N^2 * y0^2 + x0^2)
+  // dtheta/dt
+  // = (-x0 * (-N * y0') - -N * y0 * (-x0')) / (N^2 * y0^2 + x0^2)
+  // = N * (x0 * v * sin(theta) - y0 * v * cos(theta)) / (N^2 * y0^2 + x0^2)
+  // = N * v * (x0 * (N * y0) - y0 * (x0)) / (N^2 * y0^2 + x0^2)^1.5
+  // = N * v * (N - 1) * x0 * y0 / (N^2 * y0^2 + x0^2)^1.5
+  const double linear_vel = (state(3, 0) + state(4, 0)) / 2.0;
+  const double x2 = ::std::pow(state.x(), 2);
+  const double y2 = ::std::pow(state.y(), 2);
+  const double norm = y2 * kPolyN * kPolyN + x2;
+  // When we get too near the goal, avoid singularity in a sane manner.
+  if (norm < 1e-3) {
+    return 0.0;
+  }
+  return kPolyN * (kPolyN - 1) * linear_vel * state.x() * state.y() /
+         (norm * ::std::sqrt(norm));
+}
+
+void LineFollowDrivetrain::SetGoal(
+    const ::frc971::control_loops::DrivetrainQueue::Goal &goal,
+    const Pose &goal_pose) {
+  // TODO(james): More properly calculate goal velocity from throttle.
+  goal_velocity_ = goal.throttle;
+  target_pose_ = goal_pose;
+}
+
+void LineFollowDrivetrain::SetOutput(
+    ::frc971::control_loops::DrivetrainQueue::Output *output) {
+  // TODO(james): Account for voltage error terms, and/or provide driver with
+  // ability to influence steering.
+  if (output != nullptr) {
+    output->left_voltage = U_(0, 0);
+    output->right_voltage = U_(1, 0);
+  }
+}
+
+void LineFollowDrivetrain::Update(
+    const ::Eigen::Matrix<double, 5, 1> &abs_state) {
+  // Get the robot pose in the target coordinate frame.
+  const Pose relative_robot_pose = Pose({abs_state.x(), abs_state.y(), 0.0},
+                                        abs_state(2, 0)).Rebase(&target_pose_);
+  // Always force a slight negative X, so that the driver can continue to drive
+  // past zero if they want.
+  // The "slight negative" needs to be large enough that we won't force
+  // obnoxiously sharp turning radii if we run past the end of the
+  // line--because, in practice, the driver should never be trying to drive to a
+  // target where they are significantly off laterally at <0.1m from the target,
+  // this should not be a problem.
+  const double relative_x = ::std::min(relative_robot_pose.rel_pos().x(), -0.1);
+  const ::Eigen::Matrix<double, 5, 1> rel_state =
+      (::Eigen::Matrix<double, 5, 1>() << relative_x,
+       relative_robot_pose.rel_pos().y(), relative_robot_pose.rel_theta(),
+       abs_state(3, 0), abs_state(4, 0))
+          .finished();
+  ::Eigen::Matrix<double, 3, 1> controls_goal(
+      GoalTheta(rel_state), goal_velocity_, GoalThetaDot(rel_state));
+  ::Eigen::Matrix<double, 3, 1> controls_state;
+  controls_state(0, 0) = rel_state(2, 0);
+  controls_state.block<2, 1>(1, 0) =
+      dt_config_.Tlr_to_la() * rel_state.block<2, 1>(3, 0);
+  ::Eigen::Matrix<double, 3, 1> controls_err = controls_goal - controls_state;
+  // Because we are taking the difference of an angle, normaliez to [-pi, pi].
+  controls_err(0, 0) = ::aos::math::NormalizeAngle(controls_err(0, 0));
+  // TODO(james): Calculate the next goal so that we are properly doing
+  // feed-forwards.
+  ::Eigen::Matrix<double, 2, 1> U_ff =
+      Kff_ * (controls_goal - A_d_ * controls_goal);
+  U_ = K_ * controls_err + U_ff;
+  const double maxU = U_.lpNorm<::Eigen::Infinity>();
+  U_ *= (maxU > kMaxVoltage) ? kMaxVoltage / maxU : 1.0;
+}
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
new file mode 100644
index 0000000..eb6c3a2
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -0,0 +1,93 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+namespace testing {
+class LineFollowDrivetrainTest;
+}  // namespace testing
+
+// A drivetrain that permits a velocity input from the driver while controlling
+// lateral motion.
+// TODO(james): Do we want to allow some driver input on lateral control? It may
+// just be too confusing to make work effectively, but it also wouldn't be too
+// hard to just allow the driver to specify an offset to the angle/angular
+// velocity.
+class LineFollowDrivetrain {
+ public:
+  typedef TypedPose<double> Pose;
+
+  LineFollowDrivetrain(const DrivetrainConfig<double> &dt_config);
+  // Sets the current goal; the drivetrain queue contains throttle_velocity
+  // which is used to command overall robot velocity. The goal_pose is a Pose
+  // representing where we are tring to go. This would typically be the Pose of
+  // a Target; the positive X-axis in the Pose's frame represents the direction
+  // we want to go (we approach the pose from the left-half plane).
+  void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal,
+               const Pose &goal_pose);
+  // State: [x, y, theta, left_vel, right_vel]
+  void Update(const ::Eigen::Matrix<double, 5, 1> &state);
+  void SetOutput(
+      ::frc971::control_loops::DrivetrainQueue::Output *output);
+  // TODO(james): Populate.
+  void PopulateStatus(
+      ::frc971::control_loops::DrivetrainQueue::Status * /*status*/) const {}
+
+ private:
+  // Nominal max voltage.
+  // TODO(james): Is there a config for this or anything?
+  static constexpr double kMaxVoltage = 12.0;
+  // Degree of the polynomial to follow in. Should be strictly greater than 1.
+  // A value of 1 would imply that we drive straight to the target (but not hit
+  // it straight on, unless we happened to start right in front of it). A value
+  // of zero would imply driving straight until we hit the plane of the target.
+  // Values between 0 and 1 would imply hitting the target from the side.
+  static constexpr double kPolyN = 3.0;
+  static_assert(kPolyN > 1.0,
+                "We want to hit targets straight on (see above comments).");
+
+  double GoalTheta(const ::Eigen::Matrix<double, 5, 1> &state) const;
+  double GoalThetaDot(const ::Eigen::Matrix<double, 5, 1> &state) const;
+
+  const DrivetrainConfig<double> dt_config_;
+  // TODO(james): These should probably be factored out somewhere.
+  const ::Eigen::DiagonalMatrix<double, 3> Q_ =
+      (::Eigen::DiagonalMatrix<double, 3>().diagonal()
+           << 1.0 / ::std::pow(0.01, 2),
+       1.0 / ::std::pow(0.1, 2), 1.0 / ::std::pow(10.0, 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();
+  // The matrices we use for the linear controller.
+  // State for these is [theta, linear_velocity, angular_velocity]
+  const ::Eigen::Matrix<double, 3, 3> A_d_;
+  const ::Eigen::Matrix<double, 3, 2> B_d_;
+  const ::Eigen::Matrix<double, 2, 3> K_;
+  const ::Eigen::Matrix<double, 2, 3> Kff_;
+
+  Pose target_pose_;
+  double goal_velocity_ = 0.0;
+
+  // Voltage output to apply
+  ::Eigen::Matrix<double, 2, 1> U_;
+
+  friend class testing::LineFollowDrivetrainTest;
+};
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
new file mode 100644
index 0000000..44d1e31
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -0,0 +1,299 @@
+#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
+
+#include <chrono>
+
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+#include "gflags/gflags.h"
+#include "gtest/gtest.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
+DEFINE_bool(plot, false, "If true, plot");
+
+namespace chrono = ::std::chrono;
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+class LineFollowDrivetrainTest : public ::testing::Test {
+ public:
+  typedef TypedPose<double> Pose;
+
+  ::aos::testing::TestSharedMemory shm_;
+
+  LineFollowDrivetrainTest()
+      : config_(GetTestDrivetrainConfig()),
+        drivetrain_(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>>(
+                config_.make_hybrid_drivetrain_velocity_loop()))),
+        Tlr_to_la_(config_.Tlr_to_la()),
+        Tla_to_lr_(config_.Tla_to_lr()) {}
+
+  void set_goal_pose(const Pose &pose) {
+    goal_pose_ = pose;
+  }
+
+  void RunForTime(chrono::nanoseconds t) {
+    const int niter = t / config_.dt;
+    for (int ii = 0; ii < niter; ++ii) {
+      Iterate();
+    }
+  }
+
+  void Iterate() {
+    ::frc971::control_loops::DrivetrainQueue::Goal goal;
+    goal.throttle = driver_model_(state_);
+    drivetrain_.SetGoal(goal, goal_pose_);
+    drivetrain_.Update(state_);
+
+    ::frc971::control_loops::DrivetrainQueue::Output output;
+    drivetrain_.SetOutput(&output);
+
+    EXPECT_LE(::std::abs(output.left_voltage), 12.0 + 1e-6);
+    EXPECT_LE(::std::abs(output.right_voltage), 12.0 + 1e-6);
+
+    ::Eigen::Matrix<double, 2, 1> U(output.left_voltage, output.right_voltage);
+
+    state_ = RungeKuttaU(
+        [this](const ::Eigen::Matrix<double, 5, 1> &X,
+               const ::Eigen::Matrix<double, 2, 1> &U) {
+          return ContinuousDynamics(velocity_drivetrain_->plant(), Tlr_to_la_,
+                                    X, U);
+        },
+        state_, U,
+        chrono::duration_cast<chrono::duration<double>>(config_.dt).count());
+    t_ += config_.dt;
+
+    time_.push_back(chrono::duration_cast<chrono::duration<double>>(
+                        t_.time_since_epoch()).count());
+    simulation_ul_.push_back(U(0, 0));
+    simulation_ur_.push_back(U(1, 0));
+    simulation_x_.push_back(state_.x());
+    simulation_y_.push_back(state_.y());
+  }
+
+  // Check that left/right velocities are near zero and the absolute position is
+  // near that of the goal Pose.
+  void VerifyNearGoal() const {
+    EXPECT_NEAR(goal_pose_.abs_pos().x(), state_(0, 0), 1e-3);
+    EXPECT_NEAR(goal_pose_.abs_pos().y(), state_(1, 0), 1e-3);
+    // state should be at 0 or PI (we should've come in striaght on or straight
+    // backwards).
+    const double angle_err =
+        ::aos::math::DiffAngle(goal_pose_.abs_theta(), state_(2, 0));
+    const double angle_pi_err = ::aos::math::DiffAngle(angle_err, M_PI);
+    EXPECT_LT(::std::min(::std::abs(angle_err), ::std::abs(angle_pi_err)),
+              1e-3);
+    // Left/right velocities are zero:
+    EXPECT_NEAR(0.0, state_(3, 0), 1e-3);
+    EXPECT_NEAR(0.0, state_(4, 0), 1e-3);
+  }
+
+  void CheckGoalTheta(double x, double y, double v, double expected_theta,
+                          double expected_thetadot) {
+    ::Eigen::Matrix<double, 5, 1> state;
+    state << x, y, 0.0, v, v;
+    const double theta = drivetrain_.GoalTheta(state);
+    const double thetadot = drivetrain_.GoalThetaDot(state);
+    EXPECT_EQ(expected_theta, theta) << "x " << x << " y " << y << " v " << v;
+    EXPECT_EQ(expected_thetadot, thetadot)
+        << "x " << x << " y " << y << " v " << v;
+  }
+
+  void CheckGoalThetaDotAtState(double x, double y, double v) {
+    ::Eigen::Matrix<double, 5, 1> state;
+    state << x, y, 0.0, v, v;
+    const double theta = drivetrain_.GoalTheta(state);
+    const double thetadot = drivetrain_.GoalThetaDot(state);
+    const double dx = v * ::std::cos(theta);
+    const double dy = v * ::std::sin(theta);
+    constexpr double kEps = 1e-5;
+    state(0, 0) += dx * kEps;
+    state(1, 0) += dy * kEps;
+    const double next_theta = drivetrain_.GoalTheta(state);
+    EXPECT_NEAR(thetadot, ::aos::math::DiffAngle(next_theta, theta) / kEps,
+                1e-4)
+        << "theta: " << theta << " nexttheta: " << next_theta << " x " << x
+        << " y " << y << " v " << v;
+  }
+
+  void TearDown() override {
+    if (FLAGS_plot) {
+      matplotlibcpp::figure();
+      matplotlibcpp::plot(time_, simulation_ul_, {{"label", "ul"}});
+      matplotlibcpp::plot(time_, simulation_ur_, {{"label", "ur"}});
+      matplotlibcpp::legend();
+
+      Pose base_pose(&goal_pose_, {-1.0, 0.0, 0.0}, 0.0);
+      ::std::vector<double> line_x(
+          {base_pose.abs_pos().x(), goal_pose_.abs_pos().x()});
+      ::std::vector<double> line_y(
+          {base_pose.abs_pos().y(), goal_pose_.abs_pos().y()});
+      matplotlibcpp::figure();
+      matplotlibcpp::plot(line_x, line_y, {{"label", "line"}, {"marker", "o"}});
+      matplotlibcpp::plot(simulation_x_, simulation_y_,
+                          {{"label", "robot"}, {"marker", "o"}});
+      matplotlibcpp::legend();
+
+      matplotlibcpp::show();
+    }
+  }
+
+ protected:
+  // Strategy used by the driver to command throttle.
+  // This function should return a throttle to apply given the current state.
+  // Default to simple proportional gain:
+  ::std::function<double(::Eigen::Matrix<double, 5, 1>)> driver_model_ =
+      [](const ::Eigen::Matrix<double, 5, 1> &state) {
+        return -5.0 * state.x();
+      };
+
+  // Current state; [x, y, theta, left_velocity, right_velocity]
+  ::Eigen::Matrix<double, 5, 1> state_;
+
+ private:
+  const DrivetrainConfig<double> config_;
+
+  LineFollowDrivetrain drivetrain_;
+
+  ::std::unique_ptr<
+      StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+                        HybridKalman<2, 2, 2>>>
+      velocity_drivetrain_;
+
+  // 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_;
+
+  // Current goal pose we are trying to drive to.
+  Pose goal_pose_;
+
+  aos::monotonic_clock::time_point t_;
+
+  // Debugging vectors for plotting:
+  ::std::vector<double> time_;
+  ::std::vector<double> simulation_ul_;
+  ::std::vector<double> simulation_ur_;
+  ::std::vector<double> simulation_x_;
+  ::std::vector<double> simulation_y_;
+};
+
+TEST_F(LineFollowDrivetrainTest, ValidGoalThetaDot) {
+  for (double x : {1.0, 0.0, -1.0, -2.0, -3.0}) {
+    for (double y : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
+      for (double v : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
+        if (x == 0.0 && y == 0.0) {
+          // When x/y are zero, we can encounter singularities. The code should
+          // just provide zeros in this case.
+          CheckGoalTheta(x, y, v, 0.0, 0.0);
+        } else {
+          CheckGoalThetaDotAtState(x, y, v);
+        }
+      }
+    }
+  }
+}
+
+// If the driver commands the robot to keep going, we should just run straight
+// off the end and keep going along the line:
+TEST_F(LineFollowDrivetrainTest, RunOffEnd) {
+  state_ << -1.0, 0.1, 0.0, 0.0, 0.0;
+  driver_model_ = [](const ::Eigen::Matrix<double, 5, 1> &) { return 1.0; };
+  RunForTime(chrono::seconds(10));
+  EXPECT_LT(6.0, state_.x());
+  EXPECT_NEAR(0.0, state_.y(), 1e-4);
+  EXPECT_NEAR(0.0, state_(2, 0), 1e-4);
+  EXPECT_NEAR(1.0, state_(3, 0), 1e-4);
+  EXPECT_NEAR(1.0, state_(4, 0), 1e-4);
+}
+
+class LineFollowDrivetrainTargetParamTest
+    : public LineFollowDrivetrainTest,
+      public ::testing::WithParamInterface<Pose> {};
+TEST_P(LineFollowDrivetrainTargetParamTest, NonZeroTargetTest) {
+  // Start the state at zero and then put the target in a
+  state_.setZero();
+  driver_model_ = [this](const ::Eigen::Matrix<double, 5, 1> &state) {
+    return (state.topRows<2>() - GetParam().abs_pos().topRows<2>()).norm();
+  };
+  set_goal_pose(GetParam());
+  RunForTime(chrono::seconds(10));
+  VerifyNearGoal();
+}
+INSTANTIATE_TEST_CASE_P(TargetPosTest, LineFollowDrivetrainTargetParamTest,
+                        ::testing::Values(Pose({0.0, 0.0, 0.0}, 0.0),
+                                          Pose({1.0, 0.0, 0.0}, 0.0),
+                                          Pose({3.0, 1.0, 0.0}, 0.0),
+                                          Pose({3.0, 0.0, 0.0}, 0.5),
+                                          Pose({3.0, 0.0, 0.0}, -0.5),
+                                          Pose({-3.0, -1.0, 0.0}, -2.5)));
+
+class LineFollowDrivetrainParamTest
+    : public LineFollowDrivetrainTest,
+      public ::testing::WithParamInterface<::std::tuple<
+          ::Eigen::Matrix<double, 5, 1>,
+          ::std::function<double(const ::Eigen::Matrix<double, 5, 1> &)>>> {};
+
+TEST_P(LineFollowDrivetrainParamTest, VaryPositionAndModel) {
+  state_ = ::std::get<0>(GetParam());
+  driver_model_ = ::std::get<1>(GetParam());
+  RunForTime(chrono::seconds(10));
+  VerifyNearGoal();
+}
+
+INSTANTIATE_TEST_CASE_P(
+    PositionAndModelTest, LineFollowDrivetrainParamTest,
+    ::testing::Combine(
+        ::testing::Values(
+            (::Eigen::Matrix<double, 5, 1>() << -1.0, 0.0, 0.0, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, 0.0, 0.0, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, 0.4, 0.0, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, -0.4, 0.0, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, 0.0, 0.5, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, 0.0, -0.5, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, 1.0, 0.5, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, 1.0, -0.5, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 0.5, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, -0.5, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, 1.0, -2.0, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 2.0, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 12.0, 0.0, 0.0)
+                .finished(),
+            (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 0.0, 0.5, -0.5)
+                .finished()),
+        ::testing::Values(
+            [](const ::Eigen::Matrix<double, 5, 1> &state) {
+              return -5.0 * state.x();
+            },
+            [](const ::Eigen::Matrix<double, 5, 1> &state) {
+              return 5.0 * state.x();
+            },
+            [](const ::Eigen::Matrix<double, 5, 1> &state) {
+              return -1.0 * ::std::abs(state.x()) - 0.5 * state.x() * state.x();
+            })));
+
+}  // namespace testing
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971