diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index c0b71f7..2f50468 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -108,7 +108,7 @@
         "//external:python-gflags",
         "//external:python-glog",
         "//frc971/control_loops/python:controls",
-        "//y2018/control_loops/python:polydrivetrain_lib",
+        "//y2016/control_loops/python:polydrivetrain_lib",
         "@matplotlib",
     ],
 )
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index d045f45..e69d874 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -8,11 +8,13 @@
 import numpy
 import scipy
 import scipy.integrate
+import scipy.optimize
 import sys
 
 from frc971.control_loops.python import polydrivetrain
 from frc971.control_loops.python import drivetrain
-import y2018.control_loops.python.drivetrain
+from frc971.control_loops.python import controls
+import y2016.control_loops.python.drivetrain
 
 """This file is my playground for implementing spline following.
 
@@ -236,7 +238,7 @@
         # table to get an alpha -> distance calculation.  Gaussian Quadrature
         # is quite accurate, so we can get away with fewer points here than we
         # might think.
-        for alpha in numpy.linspace(0.0, 1.0, num_alpha)[:-1]:
+        for alpha in numpy.linspace(0.0, 1.0, num_alpha)[1:]:
             self._point_distances.append(
                 scipy.integrate.fixed_quad(spline_velocity, alpha, alpha + 1.0
                                            / (num_alpha - 1.0))[0] +
@@ -325,6 +327,10 @@
 
         return dtheta_points / numpy.linalg.norm(dspline_points, axis=0)
 
+    def dtheta_dt(self, distance, velocity, dspline_points=None, ddspline_points=None):
+        """Returns the angular velocity as a function of time."""
+        return self.dtheta(distance, dspline_points, ddspline_points) * velocity
+
     def ddtheta(self,
                 distance,
                 dspline_points=None,
@@ -394,11 +400,13 @@
             return 0.0
         return (f(t, y) - a0) / y
 
-    return (RungeKutta(integrablef, v, x, dx) - v) + numpy.sqrt(2.0 * a0 * dx + v * v)
+    return (RungeKutta(integrablef, v, x, dx) - v
+            ) + numpy.sqrt(2.0 * a0 * dx + v * v)
 
 
 class Trajectory(object):
-    def __init__(self, path, drivetrain, longitudal_accel, lateral_accel, distance_count):
+    def __init__(self, path, drivetrain, longitudal_accel, lateral_accel,
+                 distance_count):
         self._path = path
         self._drivetrain = drivetrain
         self.distances = numpy.linspace(0.0,
@@ -441,42 +449,6 @@
         K5 = self._B_inverse * K2
         return K3, K4, K5
 
-    def curvature_voltage_pass(self, plan):
-        plan = plan.copy()
-        for i, distance in enumerate(self.distances):
-            current_ddtheta = self._path.ddtheta(distance)[0]
-            current_dtheta = self._path.dtheta(distance)[0]
-            # We've now got the equation:
-            #     K2 * d^2x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
-            # Now, rephrase it as K3 v^2 + K4 v = U
-            K3, K4, K5 = self.compute_K345(current_dtheta, current_ddtheta)
-            # But, we are going to assume that d^2x/dt^2 = 0
-
-            x = []
-            for a, b in [(K3[0, 0], K4[0, 0]), (K3[1, 0], K4[1, 0])]:
-                for c in [12.0, -12.0]:
-                    middle = b * b - 4.0 * a * c
-                    if middle >= 0.0:
-                        x.append((-b + numpy.sqrt(middle)) / (2.0 * a))
-                        x.append((-b - numpy.sqrt(middle)) / (2.0 * a))
-
-            maxx = 0.0
-            for newx in x:
-                if newx < 0.0:
-                    continue
-                U = (K3 * newx * newx + K4 * newx).T
-                # TODO(austin): We know that one of these *will* be +-12.0.  Only
-                # check the other one.
-                if not (numpy.abs(U) > 12.0 + 1e-6).any():
-                    maxx = max(newx, maxx)
-
-            if maxx == 0.0:
-                print('Could not solve')
-                return None
-            plan[i] = min(plan[i], maxx)
-            pass
-        return plan
-
     def forward_acceleration(self, x, v):
         current_ddtheta = self._path.ddtheta(x)[0]
         current_dtheta = self._path.dtheta(x)[0]
@@ -562,7 +534,7 @@
         return plan
 
     # TODO(austin): The plan should probably not be passed in...
-    def ff_voltage(self, plan, distance):
+    def ff_accel(self, plan, distance):
         if distance < self.distances[1]:
             after_index = 1
             before_index = after_index - 1
@@ -598,7 +570,10 @@
         else:
             velocity = vbackward
             accel = self.backward_acceleration(distance, velocity)
+        return (distance, velocity, accel)
 
+    def ff_voltage(self, plan, distance):
+        _, velocity, accel = self.ff_accel(plan, distance)
         current_ddtheta = self._path.ddtheta(distance)[0]
         current_dtheta = self._path.dtheta(distance)[0]
         # TODO(austin): Factor these out.
@@ -610,6 +585,20 @@
         U = K5 * accel + K3 * velocity * velocity + K4 * velocity
         return U
 
+    def goal_state(self, distance, velocity):
+        width = (
+            self._drivetrain.robot_radius_l + self._drivetrain.robot_radius_r)
+        goal = numpy.matrix(numpy.zeros((5, 1)))
+
+        goal[0:2, :] = self._path.xy(distance)
+        goal[2, :] = self._path.theta(distance)
+
+        Ter = numpy.linalg.inv(numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
+        goal[3:5, :] = Ter * numpy.matrix(
+            [[velocity], [self._path.dtheta_dt(distance, velocity)]])
+        return goal
+
+
 def main(argv):
     # Build up the control point matrix
     start = numpy.matrix([[0.0, 0.0]]).T
@@ -784,9 +773,9 @@
     # derivitives of our spline.
 
     velocity_drivetrain = polydrivetrain.VelocityDrivetrainModel(
-        y2018.control_loops.python.drivetrain.kDrivetrain)
+        y2016.control_loops.python.drivetrain.kDrivetrain)
     position_drivetrain = drivetrain.Drivetrain(
-        y2018.control_loops.python.drivetrain.kDrivetrain)
+        y2016.control_loops.python.drivetrain.kDrivetrain)
 
     longitudal_accel = 3.0
     lateral_accel = 2.0
@@ -825,47 +814,132 @@
     pylab.ylabel("velocity (m/s)")
     pylab.legend()
 
-    voltages = numpy.matrix(numpy.zeros((2, len(distances))))
-    for i in range(len(distances)):
-        voltages[:, i] = trajectory.ff_voltage(backward_accel_plan,
-                                               distances[i])
-
-    # Now, let's integrate up the feed forwards voltage to see where we end up.
-    spline_state = numpy.matrix(numpy.zeros((4, 1)))
-    # x, y, theta, vl, vr
-    state = numpy.matrix(numpy.zeros((5, 1)))
     dt = 0.005
-    num_timesteps = 400
-    spline_states = numpy.matrix(numpy.zeros((4, num_timesteps)))
-    states = numpy.matrix(numpy.zeros((5, num_timesteps)))
-    Us = numpy.matrix(numpy.zeros((2, num_timesteps)))
-    t = numpy.array(range(num_timesteps)) * dt
-    for i in range(num_timesteps):
-        spline_distance = 0.5 * (spline_state[0, 0] + spline_state[2, 0])
-        spline_velocity = 0.5 * (spline_state[1, 0] + spline_state[3, 0])
-        def distance_spline_diffeq(t, x):
-            spline_distance = 0.5 * (x[0, 0] + x[2, 0])
-            spline_velocity = 0.5 * (x[1, 0] + x[3, 0])
-            U = trajectory.ff_voltage(
-                backward_accel_plan,
-                spline_distance)
-            dXdt = (position_drivetrain.A_continuous * x +
-                      position_drivetrain.B_continuous * U)
-            return dXdt
-        spline_state = RungeKutta(distance_spline_diffeq, spline_state, i * dt, dt)
+    # Now, let's integrate up the path centric coordinates to get a distance,
+    # velocity, and acceleration as a function of time to follow the path.
 
-        def spline_diffeq(t, x):
-            spline_distance = 0.5 * (spline_state[0, 0] + spline_state[2, 0])
-            spline_velocity = 0.5 * (spline_state[1, 0] + spline_state[3, 0])
-            velocity = x[3:, :]
+    length_plan_t = [0.0]
+    length_plan_x = [numpy.matrix(numpy.zeros((2, 1)))]
+    length_plan_v = [0.0]
+    length_plan_a = [trajectory.ff_accel(backward_accel_plan, 0.0)[2]]
+    t = 0.0
+    spline_state = length_plan_x[-1][0:2, :]
+
+    while spline_state[0, 0] < path.length():
+        t += dt
+        def along_spline_diffeq(t, x):
+            _, v, a = trajectory.ff_accel(backward_accel_plan, x[0, 0])
+            return numpy.matrix([[x[1, 0]], [a]])
+
+        spline_state = RungeKutta(along_spline_diffeq,
+                                  spline_state.copy(), t, dt)
+
+        d, v, a = trajectory.ff_accel(backward_accel_plan, length_plan_x[-1][0, 0])
+
+        length_plan_v.append(v)
+        length_plan_a.append(a)
+        length_plan_t.append(t)
+        length_plan_x.append(spline_state.copy())
+        spline_state[1, 0] = v
+
+    xva_plan = numpy.matrix(numpy.zeros((3, len(length_plan_t))))
+    u_plan = numpy.matrix(numpy.zeros((2, len(length_plan_t))))
+    state_plan = numpy.matrix(numpy.zeros((5, len(length_plan_t))))
+
+    state = numpy.matrix(numpy.zeros((5, 1)))
+    state[3, 0] = 0.1
+    state[4, 0] = 0.1
+    states = numpy.matrix(numpy.zeros((5, len(length_plan_t))))
+    full_us = numpy.matrix(numpy.zeros((2, len(length_plan_t))))
+    x_es = []
+    y_es = []
+    theta_es = []
+    vel_es = []
+    omega_es = []
+    omega_rs = []
+    omega_cs = []
+
+    width = (velocity_drivetrain.robot_radius_l +
+             velocity_drivetrain.robot_radius_r)
+    Ter = numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]])
+
+    for i in xrange(len(length_plan_t)):
+        xva_plan[0, i] = length_plan_x[i][0, 0]
+        xva_plan[1, i] = length_plan_v[i]
+        xva_plan[2, i] = length_plan_a[i]
+
+        xy_r = path.xy(xva_plan[0, i])
+        x_r = xy_r[0, 0]
+        y_r = xy_r[1, 0]
+        theta_r = path.theta(xva_plan[0, i])[0]
+        vel_omega_r = numpy.matrix(
+            [[xva_plan[1, i]],
+             [path.dtheta_dt(xva_plan[0, i], xva_plan[1, i])[0]]])
+        vel_lr = numpy.linalg.inv(Ter) * vel_omega_r
+
+        state_plan[:, i] = numpy.matrix(
+            [[x_r], [y_r], [theta_r], [vel_lr[0, 0]], [vel_lr[1, 0]]])
+        u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i])
+
+    Q = numpy.matrix(
+        numpy.diag([
+            1.0 / (0.05**2), 1.0 / (0.05**2), 1.0 / (0.2**2), 1.0 / (0.5**2),
+            1.0 / (0.5**2)
+        ]))
+    R = numpy.matrix(numpy.diag([1.0 / (12.0**2), 1.0 / (12.0**2)]))
+    kMinVelocity = 0.1
+
+    for i in xrange(len(length_plan_t)):
+        states[:, i] = state
+
+        theta = state[2, 0]
+        sintheta = numpy.sin(theta)
+        costheta = numpy.cos(theta)
+        linear_velocity = (state[3, 0] + state[4, 0]) / 2.0
+        if abs(linear_velocity) < kMinVelocity / 100.0:
+            linear_velocity = 0.1
+        elif abs(linear_velocity) > kMinVelocity:
+            pass
+        elif linear_velocity > 0:
+            linear_velocity = kMinVelocity
+        elif linear_velocity < 0:
+            linear_velocity = -kMinVelocity
+
+        width = (velocity_drivetrain.robot_radius_l +
+                velocity_drivetrain.robot_radius_r)
+        A_linearized_continuous = numpy.matrix([[
+            0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta, 0.5 *
+            costheta
+        ], [
+            0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta, 0.5 *
+            sintheta
+        ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
+                                                [0.0, 0.0, 0.0, 0.0, 0.0],
+                                                [0.0, 0.0, 0.0, 0.0, 0.0]])
+        A_linearized_continuous[3:5, 3:5] = velocity_drivetrain.A_continuous
+        B_linearized_continuous = numpy.matrix(numpy.zeros((5, 2)))
+        B_linearized_continuous[3:5, :] = velocity_drivetrain.B_continuous
+
+        A, B = controls.c2d(A_linearized_continuous, B_linearized_continuous,
+                            dt)
+
+        if i >= 0:
+            K = controls.dlqr(A, B, Q, R)
+            print("K", K)
+            print("eig", numpy.linalg.eig(A - B * K)[0])
+        goal_state = trajectory.goal_state(xva_plan[0, i], xva_plan[1, i])
+        state_error = goal_state - state
+
+        U = (trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i]) + K *
+             (state_error))
+
+        def spline_diffeq(U, t, x):
+            velocity = x[3:5, :]
             theta = x[2, 0]
             linear_velocity = (velocity[0, 0] + velocity[1, 0]) / 2.0
             angular_velocity = (velocity[1, 0] - velocity[0, 0]) / (
                 velocity_drivetrain.robot_radius_l +
                 velocity_drivetrain.robot_radius_r)
-            U = trajectory.ff_voltage(
-                backward_accel_plan,
-                spline_distance + numpy.linalg.norm(x[0:2, 0] - state[0:2, 0]))
             accel = (velocity_drivetrain.A_continuous * velocity +
                      velocity_drivetrain.B_continuous * U)
             return numpy.matrix(
@@ -873,45 +947,32 @@
                  [numpy.sin(theta) * linear_velocity], [angular_velocity],
                  [accel[0, 0]], [accel[1, 0]]])
 
-        U = trajectory.ff_voltage(backward_accel_plan, spline_distance)
-        state = RungeKutta(spline_diffeq, state, i * dt, dt)
-        spline_states[:, i] = spline_state
-        states[:, i] = state
-        Us[:, i] = U
+        full_us[:, i] = U
 
-    spline_distances = numpy.array(
-        (numpy.array(spline_states)[0, :] + numpy.array(spline_states)[2, :]) /
-        2.0)
+        state = RungeKutta(lambda t, x: spline_diffeq(U, t, x),
+                                state, i * dt, dt)
+
     pylab.figure()
-    pylab.plot(distances, numpy.array(voltages)[0, :], label='vl')
-    pylab.plot(distances, numpy.array(voltages)[1, :], label='vr')
+    pylab.plot(length_plan_t, numpy.array(xva_plan)[0, :], label='x')
+    pylab.plot(length_plan_t, [x[1, 0] for x in length_plan_x], label='v')
+    pylab.plot(length_plan_t, numpy.array(xva_plan)[1, :], label='planv')
+    pylab.plot(length_plan_t, numpy.array(xva_plan)[2, :], label='a')
+
+    pylab.plot(length_plan_t, numpy.array(full_us)[0, :], label='vl')
+    pylab.plot(length_plan_t, numpy.array(full_us)[1, :], label='vr')
     pylab.legend()
 
     pylab.figure()
     pylab.plot(
+        numpy.array(states)[0, :],
+        numpy.array(states)[1, :],
+        label='robot')
+    pylab.plot(
         numpy.array(spline_points)[0, :],
         numpy.array(spline_points)[1, :],
         label='spline')
-    pylab.plot(
-        numpy.array(states)[0, :], numpy.array(states)[1, :], label="robot")
     pylab.legend()
 
-    pylab.figure()
-    pylab.plot(
-        spline_distances, (numpy.array(states)[0, :] - numpy.array(
-            path.xy(spline_distances))[0, :]) * 100.0,
-        label='robotx_error * 100')
-    pylab.plot(
-        spline_distances, (numpy.array(states)[1, :] - numpy.array(
-            path.xy(spline_distances))[1, :]) * 100.0,
-        label='roboty_error * 100')
-    pylab.plot(
-        spline_distances, (numpy.array(states)[2, :] - numpy.array(
-            path.theta(spline_distances))) * 100.0,
-        label='robottheta_error * 100')
-    pylab.plot(distances, numpy.array(voltages)[0, :], label='voltsl')
-    pylab.plot(distances, numpy.array(voltages)[1, :], label='voltsr')
-    pylab.legend()
 
     def a(_, x):
         return 2.0