diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 71c7278..a723d96 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -628,10 +628,12 @@
     linkstatic = True,
     deps = [
         ":trajectory",
+        ":drivetrain_test_lib",
         "//aos/testing:googletest",
         "//aos/testing:test_shm",
         "//y2016:constants",
         "//y2016/control_loops/drivetrain:polydrivetrain_plants",
+        "//y2019/control_loops/drivetrain:drivetrain_base",
     ] + cpu_select({
         "amd64": [
             "//third_party/matplotlib-cpp",
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index c9cb9f0..55b40dd 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -73,6 +73,8 @@
 
     point += dpoint * ddistance;
     dpoint += distance_spline_.DDXY(distance) * ddistance;
+    EXPECT_FLOAT_EQ(distance_spline_.DDXY(distance).norm(),
+                    ::std::abs(distance_spline_.DTheta(distance)));
   }
 
 #if defined(SUPPORT_PLOT)
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index b2a533e..365bac6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -80,8 +80,8 @@
     const double expected_y =
         CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
     const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
-    EXPECT_NEAR(actual(0), expected_x, 2e-2);
-    EXPECT_NEAR(actual(1), expected_y, 2e-2);
+    EXPECT_NEAR(actual(0), expected_x, 3e-2);
+    EXPECT_NEAR(actual(1), expected_y, 3e-2);
   }
 
   void WaitForTrajectoryPlan() {
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 5f5b02e..f0b9537 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -130,8 +130,8 @@
 
   void deleteTrajectory(Trajectory *t) { delete t; }
 
-  void TrajectorySetLongitudalAcceleration(Trajectory *t, double accel) {
-    t->set_longitudal_acceleration(accel);
+  void TrajectorySetLongitudinalAcceleration(Trajectory *t, double accel) {
+    t->set_longitudinal_acceleration(accel);
   }
 
   void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
diff --git a/frc971/control_loops/drivetrain/spline_math.tex b/frc971/control_loops/drivetrain/spline_math.tex
new file mode 100644
index 0000000..49164bf
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline_math.tex
@@ -0,0 +1,567 @@
+\documentclass{article}
+\usepackage[utf8]{inputenc}
+\usepackage{amsmath}
+\DeclareMathOperator{\sgn}{sgn}
+\usepackage{hyperref}
+\usepackage{enumitem}
+\usepackage{commath}
+\usepackage[margin=0.5in]{geometry}
+\title{Spline Math}
+\author{James Kuszmaul}
+\begin{document}
+
+\maketitle
+
+\section{Introductions}
+
+This document describes some of the math going on in computing constraints on
+the maximum velocities in \texttt{trajectory.*}. Some of the thoughts are
+somewhat disorganized and represent a stream-of-consciousness more than actually
+useful documentation.
+
+For a given path, we will have:
+
+\begin{tabular}{r|l}
+Name & Description \\ \hline
+$v_l$, $v_r$ & Left and right wheel velocities \\
+$\theta$ & Robot heading/yaw at a position along the path \\
+$r$ & Robot radius (assuming center of mass in center of robot) \\
+$u_l$, $u_r$ & Left and right wheel voltages \\
+$V_{max}$ & Maximum allowable voltage (typically 12V) \\
+$v_{max}$ & User-specified maximum velocity \\
+$u$ & $\begin{bmatrix} u_l \\ u_r \end{bmatrix}$ \\
+$A$, $B$ & LTI, matrices such that $\begin{bmatrix} \dot{v}_l \\ \dot{v}_r
+\end{bmatrix} = A \begin{bmatrix} v_l \\ v_r \end{bmatrix} + B u$ \\
+$s$ & Distance along the path \\
+\end{tabular}
+
+We are interested in discovering the fastest way to traverse the path in
+question, given constraints on:
+\begin{itemize}
+\item Friction limits of the wheels (assumed to static and time/velocity
+      invariant).
+\item Voltage limits on the motors.
+\item Maximum velocity.
+\end{itemize}
+
+The voltage constraints are essentially provided by the linear dynamics shown
+above:
+
+\begin{align}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+   A \begin{bmatrix} v_l \\ v_r \end{bmatrix} + B u
+\end{align}
+
+Also, at any given point we must maintain velocities/accelerations that actually
+allow us to follow the path (as opposed to optimizing $v_l$/$v_r$ separately...).
+
+Using the velocities of the individual wheels as a starting point, and
+introducing dummy variables for the velocity/acceleration of the center of the
+robot (corresponding to time derivatives of $s$):
+
+\begin{align*}
+v_c &= \frac{v_l  r_r + v_r  r_l}{r_r + r_l} \\
+\begin{bmatrix} v_l \\ v_r \end{bmatrix} &=
+    \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+\end{bmatrix} v_c \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+  \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} - r_l \frac{d^2\theta}{ds^2} v_c
+  \\  r_r \frac{d^2\theta}{ds^2} v_c \end{bmatrix} v_c \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+  \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2 \\
+\end{align*}
+
+And finally, we need to know the lateral accelerations of the two wheels as
+part of determining whether we would be breaking friction. Note that, because of
+the general assumptions we are making about the setup of the robot, the lateral
+acceleration on the two wheels will be identical (if they were not identical,
+that would imply the robot was tearing itself apart side-to-side).
+
+\begin{align*}
+a_{lat} &= \frac{d\theta}{ds} v_c^2 \\
+\end{align*}
+
+Finally, we need to be combine the lateral/longitudinal accelerations of each
+wheel to determine whether the wheel would break friction. While technically
+there is only one expression for this, as a practical matter we have a bit of
+leeway in defining exactly what the relationship should be (e.g., if we want
+separate max longitudinal/lateral accelerations, we can create an ellipse; we
+could also just create a raw rectangle and decouple the two, although that would
+probably be a particularly poor approximation of the actual dynamics). For now
+we will say that our friction is limited by some nonnegative convex function
+$g(a_{lat}, a_{lng}) \le 1$ for any given wheel.
+
+Summarizing our equations and constraints, we have:
+
+\begin{align*}
+v_c &= \frac{v_l r_r + v_r r_l}{r_r + r_l} \\
+a_{lat} &= \frac{d\theta}{ds} v_c^2 \\
+g(a_{lat}, \dot{v}_l) &\le 1 \\
+g(a_{lat}, \dot{v}_r) &\le 1 \\
+\abs{u_l} &\le V_{max} \\
+\abs{u_r} &\le V_{max} \\
+\begin{bmatrix} v_l \\ v_r \end{bmatrix} &= \begin{bmatrix} 1 - r_l
+    \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+\end{bmatrix} v_c \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+   A \begin{bmatrix} v_l \\ v_r \end{bmatrix} + B \begin{bmatrix} u_l \\ u_r
+   \end{bmatrix} \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+  \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2 \\
+\end{align*}
+
+With $v_l$, $v_r$, $u_l$, and $u_r$ free at all $s$ except the start and end, at
+which the velocities must
+be zero (technically, we could make do with allowing non-zero velocities, but
+then we could end up in situations that would be impossible to solve--because
+the splines have differentiable curvature, any of the splines we use will always
+have some valid velocity profile, but depending on the curvature, that velocity
+may be arbitrarily small).
+
+We can provide an upper limit on the goal velocity at any given point with the
+min of the specified max velocity and the lateral-acceleration limited velocity.
+This also gives an initial pass of the longitudinal accelerations that would be
+required to follow this profile.
+
+\begin{align*}
+v_{lat} &= \min (v_{max}, \sqrt{\frac{a_{lat,max}}{\frac{d\theta}{ds}}}) \\
+\dot{v}_{lat} &= \begin{cases} 0 & v_{lat} = v_{max} \\
+-\frac12 \sqrt{\frac{a_{lat,max}}{\frac{d\theta}{ds}^3}} \frac{d^2\theta}{ds^2}
+v_{lat} & v_{lat} \ne v_{max} \\
+\end{cases} \\
+&= \begin{cases} 0 & v_{lat} = v_{max} \\
+-\frac12 \frac{a_{lat,max}}{\frac{d\theta}{ds}^2} \frac{d^2\theta}{ds^2}
+ & v_{lat} \ne v_{max} \\
+\end{cases} \\
+\end{align*}
+
+If we start to consider accounting also for longitudinal accelerations, we can
+start with:
+
+\begin{align}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+  \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2 \\
+(r_r + r_l)
+\left[ \begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+  \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \frac{\dot{v}_l r_r + \dot{v}_r r_l}{r_r + r_l} +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  \right] \\
+\begin{bmatrix}
+r_l + r_l r_r \frac{d\theta}{ds} & -r_l + r_lr_l \frac{d\theta}{ds} \\
+-r_r - r_r r_r \frac{d\theta}{ds} & r_r - r_r r_l \frac{d\theta}{ds} \\
+\end{bmatrix}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+  \begin{bmatrix} -r_l \\ r_r \end{bmatrix} (r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 \\
+\begin{bmatrix}
+1 + r_r \frac{d\theta}{ds} & -1 + r_l \frac{d\theta}{ds} \\
+-1 - r_r \frac{d\theta}{ds} & 1 - r_l \frac{d\theta}{ds} \\
+\end{bmatrix}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+  \begin{bmatrix} -1 \\ 1 \end{bmatrix} (r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 \\
+\begin{bmatrix}
+1 + r_r \frac{d\theta}{ds} & -1 + r_l \frac{d\theta}{ds} \\
+1 + r_r \frac{d\theta}{ds} & -1 + r_l \frac{d\theta}{ds} \\
+\end{bmatrix}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+   -(r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 \\
+(1 + r_r \frac{d\theta}{ds}) \dot{v}_l + (-1 + r_l \frac{d\theta}{ds}) \dot{v}_r &=
+   -(r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 \\
+%(1 + r \frac{d\theta}{ds}) \dot{v}_l + (-1 + r \frac{d\theta}{ds}) (2\dot{v}_c -
+%\dot{v}_l) &=
+%   -2 r \frac{d^2\theta}{ds^2} v_c^2 \\
+%2 \dot{v}_l + 2(-1 + r \frac{d\theta}{ds}) \dot{v}_c
+% &= -2 r \frac{d^2\theta}{ds^2} v_c^2 \\
+%\dot{v}_l &= (1 - r \frac{d\theta}{ds}) \dot{v}_c - 2 r \frac{d^2\theta}{ds^2} v_c^2 \\
+%\dot{v}_r &= (1 + r \frac{d\theta}{ds}) \dot{v}_c + 2 r \frac{d^2\theta}{ds^2} v_c^2 \\
+\begin{bmatrix}
+1 & 0 \\
+0 & 1 \\
+-1 & 0 \\
+0 & -1 \\
+\end{bmatrix}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &\le g'(v_c, s) \\
+\begin{bmatrix}
+1 & 0 \\
+0 & 1 \\
+-1 & 0 \\
+0 & -1 \\
+\end{bmatrix} \left(
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+\end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+\right) &\le g'(v_c, s)
+\end{align}
+
+Note how we have a linear relationship between $\dot{v}_l$ and $\dot{v}_r$,
+where the slope of the line between the two is constant at any given point on
+the path, but the y-intercept and the max/min bounds get tighter with speed.
+Furthermore, the velocity along the path ($v_c$) is a linear function of the
+wheel velocities $v_l$ and $v_r$. Thus, we can state, at any given point on the
+path, a lower velocity along the path will not decrease our acceleration
+bounds (if we are travelling in a straight line, then our speed does not affect
+the straight-line acceleration limits).
+
+We also can introduce the voltage constraints:
+
+\begin{align}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+   A \begin{bmatrix} v_l \\ v_r \end{bmatrix} + B \begin{bmatrix} u_l \\ u_r
+   \end{bmatrix} \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+   A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c + B \begin{bmatrix} u_l \\ u_r \end{bmatrix} \\
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  &= A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c + B \begin{bmatrix} u_l \\ u_r \end{bmatrix} \\
+\end{align}
+
+With these constraints, we can no longer guarantee that the set of feasible
+velocities at any given point along the path is contiguous. However, we can
+guarantee that at any given point there will be at most 2 contiguous sets of
+valid velocities---in the case where there are two separate sets of allowable
+velocities, then one corresponds to the lower velocities which start at zero and
+go up until we can no longer use our finite available voltage to track the path.
+The second zone can occur at higher velocities where we are going so fast that
+the deceleration from the Back-EMF of the motor is enough to slow us down, but
+not so fast that we would lose friction.
+
+This divergence does technically create a bit of a challenge--if it were to,
+e.g., split into arbitrarily many possible paths than that would complicate the
+optimization problem substantially. However, because:
+\begin{enumerate}
+\item We only split into two branches at once; and
+\item we should only have a limited number of branchings for a given N-spline
+\footnote{I'll leave this as an exercise to the reader/my future self. However,
+logically, a spline with a given number of points can only have so many features
+and so should be able to have a very finite number of branchings.}.
+\end{enumerate}
+
+It should not actually be an issue to exhaustively attempt to plan for all
+possible branchings, knowing that the path of ``always follow the slower
+branch'' will always provide a feasible path, and choose whatever feasible plan
+is fastest.
+
+Once we choose which branch to take, it is a relatively simple matter of
+exercising the existing method. Essentially, taking the set of feasible
+velocities/accelerations as a starting point we do a forwards and backwards pass
+(order is unimportant)) where we start at our initial/terminal velocity, and
+accelerate forwards/decelerate backwards as hard as possible for each pass. If
+attempting to accelerate as hard as possible on the forwards pass (or,
+conversely, decelerate on the backwards) results in us violating the feasible
+velocity bounds, we can simply set the velocity at that point for that pass to
+the highest feasible velocity. We can guarantee that the pass in the opposite
+direction will \emph{not} get stuck at that same point because if that would
+suggest we had different physics going backwards than forwards, which is not the
+case\footnote{I'm too lazy to spell this logic out more carefully, and I don't
+think it is wrong. That is not guaranteed, however.}.
+
+\section{Specialization for Friction Circle}
+
+As our particular friction circle, we will attempt to use a straight
+ellipse\footnote{Note that this has numerical stability issues due to the
+infinite slope near zero}.
+With a $a_{lat,max}$ and $a_{lng,max}$ for the maximum lateral and longitudinal
+accelerations, i.e.:
+
+\begin{align*}
+g(a_{lat}, a_{lng}) &= \left(\frac{a_{lat}}{a_{lat,max}}\right)^2
+                       + \left(\frac{a_{lng}}{a_{lng,max}}\right)^2 \\
+\end{align*}
+
+We care about turning this into a function of the path geometry and the current
+velocity that returns the max longitudinal acceleration and lets us calculate
+the friction-limited maximum acceleration, $v_{c,fmax}$. In order to do so, we
+note that, in order to be at the edge of a feasible velocity, we observe that,
+at any given $v_c$, we will have two pairs of constraints on the center-of-mass
+longitudinal acceleration that both must be met,
+corresponding to the fact that the each wheel creates an upper/lower bound
+on the valid accelerations of the center-of-mass of the robot, but at high
+$\frac{d^2\theta}{ds^2}v_c^2$, the constraints from each wheel may not overlap.
+Thus, we care about the $v_c$ such that the pairs of constraints \emph{just
+barely} overlap. This will happen when one constraint from the right wheel and
+one from the left are just on the edge of being violated; furthermore, of the
+four possible left/right pairings (+/- of left/right), only two pairs correspond
+to being on the edge of overlapping (the other two pairs correspond to, e.g.,
+having the upper acceleration limit in the same spot, as occurs when driving
+straight; in such a situation, there are plenty of valid accelerations, we just
+happen to start slipping both wheels at the same time). Which pair to look at is
+determined by the signs on $\dot{v}_c$--if they are the same (as would occur
+when going nearly straight), then we care about when the opposite limits
+overlap (e.g., upper limit on the left, lower on the right).
+
+\begin{align*}
+\dot{v}_l, \dot{v}_r &\le a_{lng, max} \sqrt{1 - \left(\frac{a_{lat}}{a_{lat,max}}\right)^2} \\
+\dot{v}_l, \dot{v}_r &\le a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+  \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds} \\
+ -1 + r_l \frac{d\theta}{ds} \\ -1 - r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c  &\le  \begin{bmatrix} r_l \\ -r_r \\ -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ + a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+0 &= \pm \{r_l,r_r\} \frac{d^2\theta}{ds^2} v_{c,fmax}^2 +  a_{lng, max} \sqrt{1 -
+  \left(\frac{\frac{d\theta}{ds}v_{c,fmax}^2}{a_{lat,max}}\right)^2} \\
+\left(\frac{d^2\theta}{ds^2} \frac{\{r_l,r_r\}v_{c,fmax}^2}{a_{lng, max}}\right)^2 &=
+  1 - \left(\frac{\frac{d\theta}{ds}v_{c,fmax}^2}{a_{lat,max}}\right)^2 \\
+v_{c,fmax} &= \min_{r \in \{r_l, r_r\}}
+\left(\left(\frac{d^2\theta}{ds^2} \frac{r}{a_{lng, max}}\right)^2 +
+   \left(\frac{\frac{d\theta}{ds}}{a_{lat,max}}\right)^2\right)^{-\frac14} \\
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+\end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+&= \begin{bmatrix}\pm 1 \\ \pm 1\end{bmatrix}
+   a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+K_2 \dot{v}_c + K_1 v_c^2
+&= \begin{bmatrix}\pm 1 \\ \pm 1\end{bmatrix}
+   a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+K_2' K_2 \dot{v}_c + K_2' K_1 v_c^2
+&= K_2' \begin{bmatrix}\pm 1 \\ \pm 1\end{bmatrix}
+   a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+K_2' K_1 v_c^2
+&= K_2' \begin{bmatrix}\pm 1 \\ \pm 1\end{bmatrix}
+   a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+\left(\frac{K_2' K_1 v_c^2}{K_2' \begin{bmatrix} 1 \\ \pm 1\end{bmatrix}a_{lng,max}}\right)^2
+&= 1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2 \\
+v_c &= \min \left(
+\left(\frac{K_2'K_1}{K_2' \begin{bmatrix} 1 \\ \pm 1\end{bmatrix}a_{lng, max}}\right)^2 +
+   \left(\frac{\frac{d\theta}{ds}}{a_{lat,max}}\right)^2
+  \right)^{-\frac14}
+\end{align*}
+
+Where the $\pm 1$ term will have opposite signs of the two coefficients of
+$K_2$ have the same sign, and will have the same sign of the coefficients of
+$K_2$ have opposite signs.
+
+$K_2'$ refers to a transformed version of $K_2$ such that $K_2'K_2 = 0$ (see
+below for exact definition).
+
+\section{Calculating Feasible Voltage Velocities}
+
+Given:
+
+\begin{align*}
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  &= A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c + B \begin{bmatrix} u_l \\ u_r \end{bmatrix} \\
+\abs{u} &\le V_{max} \\
+\norm{B^{-1} \left(\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c\right) }_\infty \le V_{max} \\
+\end{align}
+
+This is a linear program for fixed $v_c$. We wish to determine the limits of
+$v_c$ for which the linear program will have a solution.
+
+In order to analyze this linear program in $\dot{v}_c$, it is useful to know the
+signs of the coefficients of $\dot{v}_c$, which in this case are
+$B^{-1} \begin{bmatrix}1 - r_l\frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}\end{bmatrix}$
+
+If either coefficient is zero, then for any given
+$v_c$, then the linear program
+becomes trivial, as the term with $\dot{v}_c$ will drop out of one of the rows
+and so the other row will be guaranteed to be valid for some $\dot{v}_c$ while
+the row with the dropped $\dot{v}_c$ will either be valid or not. If $B$ were
+the identity matrix, then this would
+correspond to situations where one wheel is stationary and the
+acceleration of that wheel is not affected by the overall acceleration along the
+path. Determining the limits of allowable velocities is then an issue of
+calculating the point at which the stationary wheel goes from being controllable
+to not, for a given overall velocity.
+
+Otherwise, the edge of feasibility will occur
+when the pair of constraints (where one pair is for the left voltage, one for
+the right) cross and so we will have each voltage as $\pm V_{max}$; if
+the signs of the coefficients differ, then we will have
+$u_l = u_r$. Otherwise, $u_l = -u_r$.
+
+For $u_l = u_r$, attempting to maximize $v_c$:
+\begin{align*}
+B^{-1} \left(\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c\right) &= \pm \begin{bmatrix} V_{max} \\ V_{max} \end{bmatrix} \\
+\begin{pmatrix} 1 + r_r\frac{d\theta}{ds} \\ r_l\frac{d\theta}{ds} - 1
+  \end{pmatrix}\left(
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c &= \pm B \begin{bmatrix} V_{max} \\ V_{max} \end{bmatrix}
+     \right) \\
+(r_l \frac{d\theta}{ds} - 1) (1 + r_r \frac{d\theta}{ds}) \dot{v}_c +
+  \begin{bmatrix} -r_l - r_lr_r\frac{d\theta}{ds} \\ -r_r +
+  r_lr_r\frac{d\theta}{ds} \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  - \begin{bmatrix} 1 + r_r \frac{d\theta}{ds} & 0 \\ 0 & r_l \frac{d\theta}{ds} - 1
+  \end{bmatrix} A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c &=
+   \begin{bmatrix} 1 + r_r \frac{d\theta}{ds} & 0 \\ 0 & r_l \frac{d\theta}{ds} - 1
+   \end{bmatrix} B
+     \begin{bmatrix} \pm V_{max} \\ \pm V_{max} \end{bmatrix}
+\end{align*}
+
+\begin{align*}
+ -(r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2
+ - ((1 + r_r\frac{d\theta}{ds})(A_{11}(1 - r_l\frac{d\theta}{ds}) + A_{12}(1 + r_r\frac{d\theta}{ds}))
+  + (r_l\frac{d\theta}{ds} - 1)(A_{12}(1 - r_l\frac{d\theta}{ds}) + A_{11}(1 + r_r\frac{d\theta}{ds}))
+     ) v_c &= \\
+   \pm V_{max}\frac{d\theta}{ds}(B_{12}r_l + B_{11}r_l + B_{11}r_r + B_{12}r_r) \\
+ (r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}( A_{11} (-r_l - r_r) + A_{12}(r_r + r_l) +
+  r_r(A_{11}(1 - r_l\frac{d\theta}{ds}) + A_{12}(1 + r_r\frac{d\theta}{ds}))  +
+  r_l(A_{12}(1 - r_l\frac{d\theta}{ds}) + A_{11}(1 + r_r\frac{d\theta}{ds}))
+     ) v_c &= \\
+ (r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}( (r_l + r_r) (A_{12} - A_{11}) +
+ A_{11}(r_l + r_r) + A_{12}(r_l + r_r) +
+ A_{12}\frac{d\theta}{ds}(r_r^2 - r_l^2)) v_c &= \\
+ (r_l + r_r) \left( \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}A_{12}\left(2 +
+ \frac{d\theta}{ds}(r_r - r_l)\right) v_c\right) &= \\
+     \pm V_{max}(r_l + r_r)\frac{d\theta}{ds}(B_{11} + B_{12}) \\
+ \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}A_{12}\left(2 + \frac{d\theta}{ds}(r_r - r_l)\right) v_c =
+     \pm V_{max}\frac{d\theta}{ds}(B_{11} + B_{12}) \\
+\end{align*}
+
+Note that when I eliminate parts of $A$ and $B$ I am taking advantage of the
+fact that different rows of $A$ and $B$ are just reordered version of one
+another (i.e., each is of the format $\begin{bmatrix} a & b \\ b & a
+\end{bmatrix}$).
+
+In the final equation, we then execute the quadratic formula for $v_c$.
+Note that if you write
+the quadratic formula out there will be up to 4 real solutions (the two
+solutions to the quadratic formula by the two $\pm V_{max}$ results); because
+of the placement of the $\pm V_{max}$, then
+0, 1, or 3 of the solutions will be greater than zero \footnote{The basic format of
+the quadratic formula
+in this case will be $b \pm \sqrt{b^2 \pm k}$ or some equivalent, and
+$0 < \abs{k} \le b^2$ ($\abs{k} \le b^$ is necessary to have 4 real solutions;
+and $k = 0$ only if
+$b$ also is zero, or if $V_{max} = 0$, which are not particularly
+relevant corner cases). If $b$ is positive,
+then 3 will be positive ($b +$ anything is positive, and since
+$\sqrt{b^2 - k} \le b$, $b - \sqrt{b^2 - k} \ge 0$, while $b - \sqrt{b^2 + k}
+\le 0$). A similar argument holds for negative $b$. If $b = 0$, then if there
+are four real solutions, all four are zero. If there are not four real
+solutions, then $\abs{k} > b^2$ and we have two real solutions. In this case,
+because $\sqrt{b^2 + k} > b$, we will have one positive and one negative
+solution.}. The zero positive solution case is not particularly interesting, as
+it only applies at zero curvature (which cannot apply here, as we already stated
+that $\abs{r\frac{d\theta}{ds}} > 1$ (or, if we have zero $V_{max}$). The one
+or three provide us our cutoffs for where there may be a gap in allowable
+accelerations.
+
+For situations where the coefficients have the same sign, attempting to maximize $v_c$:
+
+\begin{align*}
+(r_l \frac{d\theta}{ds} - 1) (1 + r_r \frac{d\theta}{ds}) \dot{v}_c +
+  \begin{bmatrix} -r_l - r_lr_r\frac{d\theta}{ds} \\ -r_r +
+  r_lr_r\frac{d\theta}{ds} \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  - \begin{bmatrix} 1 + r_r \frac{d\theta}{ds} & 0 \\ 0 & r_l \frac{d\theta}{ds} - 1
+  \end{bmatrix} A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c &=
+   \begin{bmatrix} 1 + r_r \frac{d\theta}{ds} & 0 \\ 0 & r_l \frac{d\theta}{ds} - 1
+   \end{bmatrix} B
+     \begin{bmatrix} \pm V_{max} \\ \mp V_{max} \end{bmatrix}
+\end{align*}
+
+\begin{align*}
+ (r_l + r_r) \left( \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}A_{12}\left(2 +
+ \frac{d\theta}{ds}(r_r - r_l)\right) v_c\right) &=
+   \pm V_{max}(B_{11} - B_{12})(2 + \frac{d\theta}{ds}(r_r - r_l)) \\
+ \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}A_{12}\left(2 +
+ \frac{d\theta}{ds}(r_r - r_l)\right) v_c &=
+   \pm V_{max}\frac{(B_{11} - B_{12})(2 + \frac{d\theta}{ds}(r_r - r_l))}{r_r + r_l}\\
+\end{align*}
+
+Where the same basic rules apply to the quadratic formula solutions.
+
+Considering situations where one of the rows in the equations disappear,
+we end up with the $\dot{v}_c$ term disappearing and
+we must solve the following equation for whichever row of $B^{-1}$ $B^{-1}_N$
+causes the $\dot{v}_c$ term to disappear:
+\begin{align*}
+B^{-1} &= \frac{1}{B_{11}^2 - B_{12}^2}\begin{bmatrix} B_{11} & -B_{12} \\
+                                   -B_{12} & B_{11} \end{bmatrix} \\
+B^{-1}_N\left(
+  \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\
+                      1 + r_r \frac{d\theta}{ds} \end{bmatrix} v_c \right)
+                      &= \pm V_{max} \\
+\end{align*}
+
+
+Trying to be more general:
+\begin{align*}
+B^{-1} \left(\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+  - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+     \end{bmatrix} v_c\right) &= \pm \begin{bmatrix} V_{max} \\ V_{max} \end{bmatrix} \\
+B^{-1} \left(K_2 \dot{v}_c +   K_1 v_c^2 - A K_2 v_c\right) &=
+   \pm \begin{bmatrix} V_{max} \\ V_{max} \end{bmatrix} \\
+K_2' &= K_2^T  \begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix} \\
+K_2' \left(K_2 \dot{v}_c +   K_1 v_c^2 - A K_2 v_c\right) &=
+   K_2' B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+K_2' \left(K_2 \dot{v}_c +   K_1 v_c^2 - A K_2 v_c\right) &=
+   K_2' B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+K_2' \left(K_1 v_c^2 - A K_2 v_c\right) &=
+   K_2' B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+ \end{align*}
+
+The quadratic formula coefficients:
+\begin{align*}
+a v_c^2 + b v_c + c &= 0 \\
+a &= K_2'K_1 \\
+b &= -K_2'AK_2 \\
+c &= -K_2' B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+b^2 - 4 ac &= (K_2'AK_2)^2 + 4K_2'K_1 K_2'B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+\end{align*}
+
+%Interestingly, the solutions for both basically devolve to:
+%\begin{align*}
+% \frac{d^2\theta}{ds^2} v_c^2 + 2 \frac{d\theta}{ds} A_{12} v_c
+%  = \pm V_{max}\max(\abs{\frac{d\theta}{ds}}, 1)(B_{11} + B_{12}) \\
+%\end{align*}
+%
+%While not overly interesting, the moments when we will have a split in
+%feasibility will be at:
+%
+%\begin{align*}
+%0 < \abs{4V_{max}\max(\abs{\frac{d\theta}{ds}}, 1)(B_{11} +
+%B_{12})\frac{d^2\theta}{ds^2}} &\le \abs{2\frac{d\theta}{ds} A_{12}} \\
+%\abs{\frac{d\theta}{ds}} &\le 1 : \\
+%0 < \abs{2V_{max}(B_{11} + B_{12})\frac{d^2\theta}{ds^2}} &\le
+%\abs{\frac{d\theta}{ds} A_{12}} \\
+%\abs{\frac{d\theta}{ds}} &> 1 : \\
+%0 < \abs{2V_{max}(B_{11} + B_{12})\frac{d^2\theta}{ds^2}} &\le \abs{A_{12}} \\
+%\end{align*}
+%
+%Note that, in general, $A_{12}$ will be relatively small when compared to
+%$2V_{max}(B_{11} + B_{12})$.
+
+%\begin{align*}
+%B_{12} = B_{21} &, B_{11} = B_{22} \\
+%2 r \frac{d^2\theta}{ds^2} v_c^2 &= \pm 2V_{max}r\frac{d\theta}{ds} (B_{12} - B_{11}) \\
+%v_c &= \sqrt{\pm\frac{V_{max}\frac{d\theta}{ds}}{\frac{d^2\theta}{ds^2}} (B_{12} - B_{11})} \\
+%\end{align*}
+%
+%\begin{align*}
+%\begin{bmatrix} 1 - r^2 \frac{d\theta}{ds}^2 \\ 1 - r^2 \frac{d\theta}{ds}^2
+%  \end{bmatrix} \dot{v}_c +   \begin{bmatrix} -1 - r\frac{d\theta}{ds} \\ 1 -
+%  r\frac{d\theta}{ds} \end{bmatrix} r \frac{d^2\theta}{ds^2} v_c^2
+%  - A \begin{bmatrix} 1 - r^2 \frac{d\theta}{ds}^2 \\ 1 - r^2 \frac{d\theta}{ds}^2
+%     \end{bmatrix} v_c &=  B
+%     \begin{bmatrix} \mp V_{max} (1 + r\frac{d\theta}{ds}) \\ \pm V_{max} (1 -
+%     r\frac{d\theta}{ds}) \end{bmatrix} \\
+% 2 r \frac{d^2\theta}{ds^2} v_c^2
+%      &= \pm V_{max}(B_{21} - B_{22} - B_{11} + B_{12}) \\
+% r \frac{d^2\theta}{ds^2} v_c^2
+%      &= \pm V_{max}(B_{12} - B_{11}) \\
+%v_c &= \sqrt{\pm \frac{V_{max}}{r\frac{d^2\theta}{ds^2}} (B_{12} - B_{11})} \\
+%\end{align*}
+
+\end{document}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index c467e1f..bfd0c96 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -73,7 +73,7 @@
         case frc971::ConstraintType_CONSTRAINT_TYPE_UNDEFINED:
           break;
         case frc971::ConstraintType_LONGITUDINAL_ACCELERATION:
-          future_trajectory_->set_longitudal_acceleration(constraint.value);
+          future_trajectory_->set_longitudinal_acceleration(constraint.value);
           break;
         case frc971::ConstraintType_LATERAL_ACCELERATION:
           future_trajectory_->set_lateral_acceleration(constraint.value);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index ba4c0c2..97662d4 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -18,22 +18,20 @@
                        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()))),
+      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),
+      longitudinal_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()),
+                  1.0 / (robot_radius_l_ + robot_radius_r_)).finished()),
       Tla_to_lr_(Tlr_to_la_.inverse()),
       plan_(num_distance == 0
                 ? ::std::max(100, static_cast<int>(spline_->length() / 0.0025))
@@ -44,56 +42,293 @@
 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));
+    const double velocity_limit =  LateralVelocityCurvature(distance);
+    if (velocity_limit < plan_[i]) {
+      plan_[i] = velocity_limit;
+      plan_segment_type_[i] = CURVATURE_LIMITED;
+    }
   }
 }
 
-// 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) {
+void Trajectory::VoltageFeasibilityPass(VoltageLimit limit_type) {
+  for (size_t i = 0; i < plan_.size(); ++i) {
+    const double distance = Distance(i);
+    const double velocity_limit = VoltageVelocityLimit(distance, limit_type);
+    if (velocity_limit < plan_[i]) {
+      plan_[i] = velocity_limit;
+      plan_segment_type_[i] = VOLTAGE_LIMITED;
+    }
+  }
+}
+
+double Trajectory::BestAcceleration(double x, double v, bool backwards) {
   ::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();
+  const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
+  double min_voltage_accel = ::std::numeric_limits<double>::infinity();
+  double max_voltage_accel = -min_voltage_accel;
   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);
+      min_voltage_accel = ::std::min(a, min_voltage_accel);
+      max_voltage_accel = ::std::max(a, max_voltage_accel);
     }
   }
+  double best_accel = backwards ? min_voltage_accel : max_voltage_accel;
 
-  // Then, assume an acceleration oval and stay inside it.
-  const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
-  const double ellipse_down_shift = longitudal_acceleration_ * 1.0;
-  const double ellipse_width_stretch = ::std::sqrt(
-      1.0 / (1.0 - ::std::pow(ellipse_down_shift / (longitudal_acceleration_ +
-                                                    ellipse_down_shift),
-                              2.0)));
-  const double squared =
-      1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_ /
-                           ellipse_width_stretch,
-                       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) {
-    AOS_LOG(ERROR, "Imaginary %f, maxa: %f, fa(%f, %f) -> 0.0\n", squared, maxa,
-            x, v);
-    return 0.0;
+  double min_friction_accel, max_friction_accel;
+  FrictionLngAccelLimits(x, v, &min_friction_accel, &max_friction_accel);
+  if (backwards) {
+    best_accel = ::std::max(best_accel, min_friction_accel);
+  } else {
+    best_accel = ::std::min(best_accel, max_friction_accel);
   }
-  const double longitudal_acceleration =
-      ::std::sqrt(::std::abs(squared)) *
-          (longitudal_acceleration_ + ellipse_down_shift) -
-      ellipse_down_shift;
-  return ::std::min(longitudal_acceleration, maxa);
+
+  if (max_friction_accel < min_friction_accel - 0.1 ||
+      best_accel < min_voltage_accel || best_accel > max_voltage_accel) {
+    AOS_LOG(WARNING,
+        "Viable friction limits and viable voltage limits do not overlap (x: "
+        "%f, v: %f, backwards: %d) best_accel = %f, min voltage %f, max "
+        "voltage %f min friction %f max friction %f.\n",
+        x, v, backwards, best_accel, min_voltage_accel, max_voltage_accel,
+        min_friction_accel, max_friction_accel);
+    // Don't actually do anything--this will just result in attempting to drive
+    // higher voltages thatn we have available. In practice, that'll probably
+    // work out fine.
+  }
+
+  return best_accel;
+}
+
+double Trajectory::LateralVelocityCurvature(double distance) const {
+  // To calculate these constraints, we first note that:
+  // wheel accels = K2 * v_robot' + K1 * v_robot^2
+  // All that this logic does is solve for v_robot, leaving v_robot' free,
+  // assuming that the wheels are at their limits.
+  // To do this, we:
+  //
+  // 1) Determine what the wheel accels will be at the limit--since we have
+  // two free variables (v_robot, v_robot'), both wheels will be at their
+  // limits--if in a sufficiently tight turn (such that the signs of the
+  // coefficients of K2 are different), then the wheels will be accelerating
+  // in opposite directions; otherwise, they accelerate in the same direction.
+  // The magnitude of these per-wheel accelerations is a function of velocity,
+  // so it must also be solved for.
+  //
+  // 2) Eliminate that v_robot' term (since we don't care
+  // about it) by multiplying be a "K2prime" term (where K2prime * K2 = 0) on
+  // both sides of the equation.
+  //
+  // 3) Solving the relatively tractable remaining equation, which is
+  // basically just grouping all the terms together in one spot and taking the
+  // 4th root of everything.
+  const double dtheta = spline_->DTheta(distance);
+  const ::Eigen::Matrix<double, 1, 2> K2prime =
+      K2(dtheta).transpose() *
+      (::Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
+  // Calculate whether the wheels are spinning in opposite directions.
+  const bool opposites = K2prime(0) * K2prime(1) < 0;
+  const ::Eigen::Matrix<double, 2, 1> K1calc = K1(spline_->DDTheta(distance));
+  const double lat_accel_squared =
+      ::std::pow(dtheta / lateral_acceleration_, 2);
+  const double curvature_change_term =
+      (K2prime * K1calc).value() /
+      (K2prime *
+       (::Eigen::Matrix<double, 2, 1>() << 1.0, (opposites ? -1.0 : 1.0))
+           .finished() *
+       longitudinal_acceleration_)
+          .value();
+  const double vel_inv = ::std::sqrt(
+      ::std::sqrt(::std::pow(curvature_change_term, 2) + lat_accel_squared));
+  if (vel_inv == 0.0) {
+    return ::std::numeric_limits<double>::infinity();
+  }
+  return 1.0 / vel_inv;
+}
+
+void Trajectory::FrictionLngAccelLimits(double x, double v, double *min_accel,
+                                        double *max_accel) const {
+  // First, calculate the max longitudinal acceleration that can be achieved
+  // by either wheel given the friction elliipse that we have.
+  const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
+  const double max_wheel_lng_accel_squared =
+      1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_, 2.0);
+  if (max_wheel_lng_accel_squared < 0.0) {
+    AOS_LOG(DEBUG,
+        "Something (probably Runge-Kutta) queried invalid velocity %f at "
+        "distance %f\n",
+        v, x);
+    // If we encounter this, it means that the Runge-Kutta has attempted to
+    // sample points a bit past the edge of the friction boundary. If so, we
+    // gradually ramp the min/max accels to be more and more incorrect (note
+    // how min_accel > max_accel if we reach this case) to avoid causing any
+    // numerical issues.
+    *min_accel =
+        ::std::sqrt(-max_wheel_lng_accel_squared) * longitudinal_acceleration_;
+    *max_accel = -*min_accel;
+    return;
+  }
+  *min_accel = -::std::numeric_limits<double>::infinity();
+  *max_accel = ::std::numeric_limits<double>::infinity();
+
+  // Calculate max/min accelerations by calculating what the robots overall
+  // longitudinal acceleration would be if each wheel were running at the max
+  // forwards/backwards longitudinal acceleration.
+  const double max_wheel_lng_accel =
+      longitudinal_acceleration_ * ::std::sqrt(max_wheel_lng_accel_squared);
+  const ::Eigen::Matrix<double, 2, 1> K1v2 = K1(spline_->DDTheta(x)) * v * v;
+  const ::Eigen::Matrix<double, 2, 1> K2inv =
+      K2(spline_->DTheta(x)).cwiseInverse();
+  // Store the accelerations of the robot corresponding to each wheel being at
+  // the max/min acceleration. The first coefficient in each vector
+  // corresponds to the left wheel, the second to the right wheel.
+  const ::Eigen::Matrix<double, 2, 1> accels1 =
+      K2inv.array() * (-K1v2.array() + max_wheel_lng_accel);
+  const ::Eigen::Matrix<double, 2, 1> accels2 =
+      K2inv.array() * (-K1v2.array() - max_wheel_lng_accel);
+
+  // If either term is non-finite, that suggests that a term of K2 is zero
+  // (which is physically possible when turning such that one wheel is
+  // stationary), so just ignore that side of the drivetrain.
+  if (::std::isfinite(accels1(0))) {
+    // The inner max/min in this case determines which of the two cases (+ or
+    // - acceleration on the left wheel) we care about--in a sufficiently
+    // tight turning radius, the left hweel may be accelerating backwards when
+    // the robot as a whole accelerates forwards. We then use that
+    // acceleration to bound the min/max accel.
+    *min_accel = ::std::max(*min_accel, ::std::min(accels1(0), accels2(0)));
+    *max_accel = ::std::min(*max_accel, ::std::max(accels1(0), accels2(0)));
+  }
+  // Same logic as previous if-statement, but for the right wheel.
+  if (::std::isfinite(accels1(1))) {
+    *min_accel = ::std::max(*min_accel, ::std::min(accels1(1), accels2(1)));
+    *max_accel = ::std::min(*max_accel, ::std::max(accels1(1), accels2(1)));
+  }
+}
+
+double Trajectory::VoltageVelocityLimit(
+    double distance, VoltageLimit limit_type,
+    Eigen::Matrix<double, 2, 1> *constraint_voltages) const {
+  // To sketch an outline of the math going on here, we start with the basic
+  // dynamics of the robot along the spline:
+  // K2 * v_robot' + K1 * v_robot^2 = A * K2 * v_robot + B * U
+  // We need to determine the maximum v_robot given constrained U and free
+  // v_robot'.
+  // Similarly to the friction constraints, we accomplish this by first
+  // multiplying by a K2prime term to eliminate the v_robot' term.
+  // As with the friction constraints, we also know that the limits will occur
+  // when both sides of the drivetrain are driven at their max magnitude
+  // voltages, although they may be driven at different signs.
+  // Once we determine whether the voltages match signs, we still have to
+  // consider both possible pairings (technically we could probably
+  // predetermine which pairing, e.g. +/- or -/+, we acre about, but we don't
+  // need to).
+  //
+  // For each pairing, we then get to solve a quadratic formula for the robot
+  // velocity at those voltages. This gives us up to 4 solutions, of which
+  // up to 3 will give us positive velocities; each solution velocity
+  // corresponds to a transition from feasibility to infeasibility, where a
+  // velocity of zero is always feasible, and there will always be 0, 1, or 3
+  // positive solutions. Among the positive solutions, we take both the min
+  // and the max--the min will be the highest velocity such that all
+  // velocities between zero and that velocity are valid; the max will be
+  // the highest feasible velocity. Which we return depends on what the
+  // limit_type is.
+  //
+  // Sketching the actual math:
+  // K2 * v_robot' + K1 * v_robot^2 = A * K2 * v_robot +/- B * U_max
+  // K2prime * K1 * v_robot^2 = K2prime * (A * K2 * v_robot +/- B * U_max)
+  // a v_robot^2 + b v_robot +/- c = 0
+  const ::Eigen::Matrix<double, 2, 2> B =
+      velocity_drivetrain_->plant().coefficients().B_continuous;
+  const double dtheta = spline_->DTheta(distance);
+  const ::Eigen::Matrix<double, 2, 1> BinvK2 = B.inverse() * K2(dtheta);
+  // Because voltages can actually impact *both* wheels, in order to determine
+  // whether the voltages will have opposite signs, we need to use B^-1 * K2.
+  const bool opposite_voltages = BinvK2(0) * BinvK2(1) > 0.0;
+  const ::Eigen::Matrix<double, 1, 2> K2prime =
+      K2(dtheta).transpose() *
+      (::Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
+  const double a = K2prime * K1(spline_->DDTheta(distance));
+  const double b = -K2prime *
+                   velocity_drivetrain_->plant().coefficients().A_continuous *
+                   K2(dtheta);
+  const ::Eigen::Matrix<double, 1, 2> c_coeff = -K2prime * B;
+  // Calculate the "positive" version of the voltage limits we will use.
+  const ::Eigen::Matrix<double, 2, 1> abs_volts =
+      voltage_limit_ *
+      (::Eigen::Matrix<double, 2, 1>() << 1.0, (opposite_voltages ? -1.0 : 1.0))
+          .finished();
+
+  double min_valid_vel = ::std::numeric_limits<double>::infinity();
+  if (limit_type == VoltageLimit::kAggressive) {
+    min_valid_vel = 0.0;
+  }
+  // Iterate over both possibilites for +/- voltage, and solve the quadratic
+  // formula. For every positive solution, adjust the velocity limit
+  // appropriately.
+  for (const double sign : {1.0, -1.0}) {
+    const ::Eigen::Matrix<double, 2, 1> U = sign * abs_volts;
+    const double prev_vel = min_valid_vel;
+    const double c = c_coeff * U;
+    const double determinant = b * b - 4 * a * c;
+    if (a == 0) {
+      // If a == 0, that implies we are on a constant curvature path, in which
+      // case we just have b * v + c = 0.
+      // Note that if -b * c > 0.0, then vel will be greater than zero and b
+      // will be non-zero.
+      if (-b * c > 0.0) {
+        const double vel = -c / b;
+        if (limit_type == VoltageLimit::kConservative) {
+          min_valid_vel = ::std::min(min_valid_vel, vel);
+        } else {
+          min_valid_vel = ::std::max(min_valid_vel, vel);
+        }
+      } else if (b == 0) {
+        // If a and b are zero, then we are travelling in a straight line and
+        // have no voltage-based velocity constraints.
+        min_valid_vel = ::std::numeric_limits<double>::infinity();
+      }
+    } else if (determinant > 0) {
+      const double sqrt_determinant = ::std::sqrt(determinant);
+      const double high_vel = (-b + sqrt_determinant) / (2.0 * a);
+      const double low_vel = (-b - sqrt_determinant) / (2.0 * a);
+      if (low_vel > 0) {
+        if (limit_type == VoltageLimit::kConservative) {
+          min_valid_vel = ::std::min(min_valid_vel, low_vel);
+        } else {
+          min_valid_vel = ::std::max(min_valid_vel, low_vel);
+        }
+      }
+      if (high_vel > 0) {
+        if (limit_type == VoltageLimit::kConservative) {
+          min_valid_vel = ::std::min(min_valid_vel, high_vel);
+        } else {
+          min_valid_vel = ::std::max(min_valid_vel, high_vel);
+        }
+      }
+    } else if (determinant == 0 && -b * a > 0) {
+      const double vel = -b / (2.0 * a);
+      if (vel > 0.0) {
+        if (limit_type == VoltageLimit::kConservative) {
+          min_valid_vel = ::std::min(min_valid_vel, vel);
+        } else {
+          min_valid_vel = ::std::max(min_valid_vel, vel);
+        }
+      }
+    }
+    if (constraint_voltages != nullptr && prev_vel != min_valid_vel) {
+      *constraint_voltages = U;
+    }
+  }
+  return min_valid_vel;
 }
 
 void Trajectory::ForwardPass() {
@@ -107,57 +342,13 @@
         [this](double x, double v) { return ForwardAcceleration(x, v); },
         plan_[i], distance, delta_distance);
 
-    if (new_plan_velocity < plan_[i + 1]) {
+    if (new_plan_velocity <= plan_[i + 1]) {
       plan_[i + 1] = new_plan_velocity;
       plan_segment_type_[i] = SegmentType::ACCELERATION_LIMITED;
     }
   }
 }
 
-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 ellipse_down_shift = longitudal_acceleration_ * 1.0;
-  const double ellipse_width_stretch = ::std::sqrt(
-      1.0 / (1.0 - ::std::pow(ellipse_down_shift / (longitudal_acceleration_ +
-                                                    ellipse_down_shift),
-                              2.0)));
-  const double squared =
-      1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_ /
-                           ellipse_width_stretch,
-                       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) {
-    AOS_LOG(ERROR, "Imaginary %f, mina: %f, fa(%f, %f) -> 0.0\n", squared, mina,
-            x, v);
-    return 0.0;
-  }
-  const double longitudal_acceleration =
-      -::std::sqrt(::std::abs(squared)) *
-          (longitudal_acceleration_ + ellipse_down_shift) +
-      ellipse_down_shift;
-  return ::std::max(longitudal_acceleration, mina);
-}
-
 void Trajectory::BackwardPass() {
   const double delta_distance = Distance(0) - Distance(1);
   plan_.back() = 0.0;
@@ -169,7 +360,7 @@
         [this](double x, double v) { return BackwardAcceleration(x, v); },
         plan_[i], distance, delta_distance);
 
-    if (new_plan_velocity < plan_[i - 1]) {
+    if (new_plan_velocity <= plan_[i - 1]) {
       plan_[i - 1] = new_plan_velocity;
       plan_segment_type_[i - 1] = SegmentType::DECELERATION_LIMITED;
     }
@@ -195,6 +386,9 @@
 
   double acceleration;
   double velocity;
+  // TODO(james): While technically correct for sufficiently small segment
+  // steps, this method of switching between limits has a tendency to produce
+  // sudden jumps in acceelrations, which is undesirable.
   switch (plan_segment_type_[DistanceToSegment(distance)]) {
     case SegmentType::VELOCITY_LIMITED:
       acceleration = 0.0;
@@ -205,9 +399,20 @@
       break;
     case SegmentType::CURVATURE_LIMITED:
       velocity = vcurvature;
-      acceleration = 0.0;
+      FrictionLngAccelLimits(distance, velocity, &acceleration, &acceleration);
+      break;
+    case SegmentType::VOLTAGE_LIMITED:
+      // Normally, we expect that voltage limited plans will all get dominated
+      // by the acceleration/deceleration limits. This may not always be true;
+      // if we ever encounter this error, we just need to back out what the
+      // accelerations would be in this case.
+      LOG(FATAL) <<  "Unexpectedly got VOLTAGE_LIMITED plan.";
       break;
     case SegmentType::ACCELERATION_LIMITED:
+      // TODO(james): The integration done here and in the DECELERATION_LIMITED
+      // can technically cause us to violate friction constraints. We currently
+      // don't do anything about it to avoid causing sudden jumps in voltage,
+      // but we probably *should* at some point.
       velocity = IntegrateAccelForDistance(
           [this](double x, double v) { return ForwardAcceleration(x, v); },
           plan_[before_index], before_distance, distance - before_distance);
@@ -226,11 +431,6 @@
       break;
   }
 
-  if (vcurvature < velocity) {
-    velocity = vcurvature;
-    acceleration = 0.0;
-    AOS_LOG(ERROR, "Curvature won\n");
-  }
   return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
       .finished();
 }
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 66dcba1..a5b9807 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -43,9 +43,9 @@
   Trajectory(const DistanceSpline *spline,
              const DrivetrainConfig<double> &config, double vmax = 10.0,
              int num_distance = 0);
-  // Sets the plan longitudal acceleration limit
-  void set_longitudal_acceleration(double longitudal_acceleration) {
-    longitudal_acceleration_ = longitudal_acceleration;
+  // Sets the plan longitudinal acceleration limit
+  void set_longitudinal_acceleration(double longitudinal_acceleration) {
+    longitudinal_acceleration_ = longitudinal_acceleration;
   }
   // Sets the plan lateral acceleration limit
   void set_lateral_acceleration(double lateral_acceleration) {
@@ -56,10 +56,42 @@
     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());
-  }
+  // Returns the friction-constrained velocity limit at a given distance along
+  // the path. At the returned velocity, one or both wheels will be on the edge
+  // of slipping.
+  // There are some very disorganized thoughts on the math here and in some of
+  // the other functions in spline_math.tex.
+  double LateralVelocityCurvature(double distance) const;
+
+  // Returns the range of allowable longitudinal accelerations for the center of
+  // the robot at a particular distance (x) along the path and velocity (v).
+  // min_accel and max_accel correspodn to the min/max accelerations that can be
+  // achieved without breaking friction limits on one or both wheels.
+  // If max_accel < min_accel, that implies that v is too high for there to be
+  // any valid acceleration. FrictionLngAccelLimits(x,
+  // LateralVelocityCurvature(x), &min_accel, &max_accel) should result in
+  // min_accel == max_accel.
+  void FrictionLngAccelLimits(double x, double v, double *min_accel,
+                              double *max_accel) const;
+
+  enum class VoltageLimit {
+    kConservative,
+    kAggressive,
+  };
+
+  // Calculates the maximum voltage at which we *can* track the path. In some
+  // cases there will be two ranges of feasible velocities for traversing the
+  // path--in such a situation, from zero to velocity A we will be able to track
+  // the path, from velocity A to B we can't, from B to C we can and above C we
+  // can't. If limit_type = kConservative, we return A; if limit_type =
+  // kAggressive, we return C. We currently just use the kConservative limit
+  // because that way we can guarantee that all velocities between zero and A
+  // are allowable and don't have to handle a more complicated planning problem.
+  // constraint_voltages will be populated by the only wheel voltages that are
+  // valid at the returned limit.
+  double VoltageVelocityLimit(
+      double distance, VoltageLimit limit_type,
+      Eigen::Matrix<double, 2, 1> *constraint_voltages = nullptr) const;
 
   // Limits the velocity in the specified segment to the max velocity.
   void LimitVelocity(double starting_distance, double ending_distance,
@@ -67,25 +99,28 @@
 
   // 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);
+  void VoltageFeasibilityPass(VoltageLimit limit_type);
 
   // 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);
+  // Returns the forwards/backwards acceleration for a distance along the spline
+  // taking into account the lateral acceleration, longitudinal acceleration,
+  // and voltage limits.
+  double BestAcceleration(double x, double v, bool backwards);
+  double BackwardAcceleration(double x, double v) {
+    return BestAcceleration(x, v, true);
+  }
+  double ForwardAcceleration(double x, double v) {
+    return BestAcceleration(x, v, false);
+  }
 
   // Runs the forwards pass, setting the ending velocity to 0 m/s
   void BackwardPass();
 
   // Runs all the planning passes.
   void Plan() {
+    VoltageFeasibilityPass(VoltageLimit::kConservative);
     LateralAccelPass();
     ForwardPass();
     BackwardPass();
@@ -167,13 +202,29 @@
     VELOCITY_LIMITED,
     CURVATURE_LIMITED,
     ACCELERATION_LIMITED,
-    DECELERATION_LIMITED
+    DECELERATION_LIMITED,
+    VOLTAGE_LIMITED,
   };
 
   const ::std::vector<SegmentType> &plan_segment_type() const {
     return plan_segment_type_;
   }
 
+  // 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();
+  }
+
  private:
   // Computes alpha for a distance.
   size_t DistanceToSegment(double distance) const {
@@ -184,22 +235,6 @@
                                                     (plan_.size() - 1)))));
   }
 
-  // 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,
@@ -229,11 +264,11 @@
                         HybridKalman<2, 2, 2>>>
       velocity_drivetrain_;
 
-  // Left and right robot radiuses.
+  // Robot radiuses.
   const double robot_radius_l_;
   const double robot_radius_r_;
   // Acceleration limits.
-  double longitudal_acceleration_;
+  double longitudinal_acceleration_;
   double lateral_acceleration_;
   // Transformation matrix from left, right to linear, angular
   const ::Eigen::Matrix<double, 2, 2> Tlr_to_la_;
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 2a990f9..60bd21f 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -57,7 +57,7 @@
       &distance_spline,
       ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
   trajectory.set_lateral_acceleration(2.0);
-  trajectory.set_longitudal_acceleration(1.0);
+  trajectory.set_longitudinal_acceleration(1.0);
 
   // Grab the spline.
   ::std::vector<double> distances = trajectory.Distances();
@@ -76,6 +76,8 @@
   // Compute the velocity plan.
   ::aos::monotonic_clock::time_point start_time = ::aos::monotonic_clock::now();
   ::std::vector<double> initial_plan = trajectory.plan();
+  trajectory.VoltageFeasibilityPass(Trajectory::VoltageLimit::kConservative);
+  ::std::vector<double> voltage_plan = trajectory.plan();
   trajectory.LateralAccelPass();
   ::std::vector<double> curvature_plan = trajectory.plan();
   trajectory.ForwardPass();
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index f65adf1..a443b1e 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -4,6 +4,7 @@
 #include <vector>
 
 #include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
 #include "gflags/gflags.h"
 #include "gtest/gtest.h"
 #if defined(SUPPORT_PLOT)
@@ -14,8 +15,9 @@
 #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"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
 
-DEFINE_bool(plot, false, "If true, plot");
+DECLARE_bool(plot);
 
 namespace frc971 {
 namespace control_loops {
@@ -37,47 +39,10 @@
   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,
-      ::y2016::control_loops::drivetrain::kJ,
-      ::y2016::control_loops::drivetrain::kMass,
-      kThreeStateDriveShifter,
-      kThreeStateDriveShifter,
-      false,
-      0,
-
-      0.25,
-      1.00,
-      1.00};
-
-  return kDrivetrainConfig;
-};
-
 struct SplineTestParams {
-  ::Eigen::Matrix<double, 2, 4> control_points;
+  ::Eigen::Matrix<double, 2, 6> control_points;
   double lateral_acceleration;
-  double longitudal_acceleration;
+  double longitudinal_acceleration;
   double velocity_limit;
   double voltage_limit;
   ::std::function<void(Trajectory *)> trajectory_modification_fn;
@@ -89,29 +54,35 @@
     : public ::testing::TestWithParam<SplineTestParams> {
  public:
   ::aos::testing::TestSharedMemory shm_;
-  static constexpr chrono::nanoseconds kDt =
-      chrono::duration_cast<chrono::nanoseconds>(
-          chrono::duration<double>(::y2016::control_loops::drivetrain::kDt));
 
   ::std::unique_ptr<DistanceSpline> distance_spline_;
   ::std::unique_ptr<Trajectory> trajectory_;
   ::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
 
-  ParameterizedSplineTest() {}
+  ParameterizedSplineTest()
+      : dt_config_(GetTestDrivetrainConfig()) {}
 
   void SetUp() {
     distance_spline_ = ::std::unique_ptr<DistanceSpline>(
-        new DistanceSpline(Spline(Spline4To6(GetParam().control_points))));
+        new DistanceSpline(Spline(GetParam().control_points)));
+    // Run lots of steps to make the feedforwards terms more accurate.
     trajectory_ = ::std::unique_ptr<Trajectory>(
-        new Trajectory(distance_spline_.get(), GetDrivetrainConfig(),
+        new Trajectory(distance_spline_.get(), dt_config_,
                        GetParam().velocity_limit));
     trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
-    trajectory_->set_longitudal_acceleration(GetParam().longitudal_acceleration);
+    trajectory_->set_longitudinal_acceleration(
+        GetParam().longitudinal_acceleration);
     trajectory_->set_voltage_limit(GetParam().voltage_limit);
 
     GetParam().trajectory_modification_fn(trajectory_.get());
 
     initial_plan_ = trajectory_->plan();
+    trajectory_->VoltageFeasibilityPass(
+        Trajectory::VoltageLimit::kAggressive);
+    aggressive_voltage_plan_ = trajectory_->plan();
+    trajectory_->VoltageFeasibilityPass(
+        Trajectory::VoltageLimit::kConservative);
+    voltage_plan_ = trajectory_->plan();
     trajectory_->LateralAccelPass();
     curvature_plan_ = trajectory_->plan();
     trajectory_->ForwardPass();
@@ -119,18 +90,20 @@
     trajectory_->BackwardPass();
     backward_plan_ = trajectory_->plan();
 
-    length_plan_xva_ = trajectory_->PlanXVA(kDt);
+    length_plan_xva_ = trajectory_->PlanXVA(dt_config_.dt);
   }
 
   void TearDown() {
     printf("  Spline takes %f seconds to follow\n",
-           length_plan_xva_.size() * ::y2016::control_loops::drivetrain::kDt);
+           length_plan_xva_.size() *
+               ::aos::time::DurationInSeconds(dt_config_.dt));
 #if defined(SUPPORT_PLOT)
     if (FLAGS_plot) {
       ::std::vector<double> distances = trajectory_->Distances();
 
       for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
-        length_plan_t_.push_back(i * ::y2016::control_loops::drivetrain::kDt);
+        length_plan_t_.push_back(i *
+                                 ::aos::time::DurationInSeconds(dt_config_.dt));
         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));
@@ -153,6 +126,8 @@
       matplotlibcpp::plot(distances, backward_plan_, {{"label", "backward"}});
       matplotlibcpp::plot(distances, forward_plan_, {{"label", "forward"}});
       matplotlibcpp::plot(distances, curvature_plan_, {{"label", "lateral"}});
+      matplotlibcpp::plot(distances, aggressive_voltage_plan_, {{"label", "aggressive voltage"}});
+      matplotlibcpp::plot(distances, voltage_plan_, {{"label", "voltage"}});
       matplotlibcpp::plot(distances, initial_plan_, {{"label", "initial"}});
       matplotlibcpp::legend();
 
@@ -169,6 +144,7 @@
 #endif
   }
 
+  const DrivetrainConfig<double> dt_config_;
   static constexpr double kXPos = 0.05;
   static constexpr double kYPos = 0.05;
   static constexpr double kThetaPos = 0.2;
@@ -192,6 +168,8 @@
   ::std::vector<double> curvature_plan_;
   ::std::vector<double> forward_plan_;
   ::std::vector<double> backward_plan_;
+  ::std::vector<double> voltage_plan_;
+  ::std::vector<double> aggressive_voltage_plan_;
 
   ::std::vector<double> length_plan_t_;
   ::std::vector<double> length_plan_x_;
@@ -202,12 +180,174 @@
 
 };
 
-constexpr chrono::nanoseconds ParameterizedSplineTest::kDt;
+// Tests that the VoltageVelocityLimit function produces correct results by
+// calculating the limit at a variety of points and then ensuring that we can
+// indeed drive +/- 12V at the limit and that we can't go faster.
+TEST_P(ParameterizedSplineTest, VoltageFeasibilityCheck) {
+  for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
+    const double distance = length_plan_xva_[i](0);
+    ::Eigen::Matrix<double, 2, 1> U;
+    const ::Eigen::Matrix<double, 2, 1> K2 =
+        trajectory_->K2(distance_spline_->DTheta(distance));
+    const ::Eigen::Matrix<double, 2, 1> K1 =
+        trajectory_->K1(distance_spline_->DDTheta(distance));
+    const ::Eigen::Matrix<double, 2, 2> A =
+        trajectory_->velocity_drivetrain().plant().coefficients().A_continuous;
+    const ::Eigen::Matrix<double, 2, 2> B =
+        trajectory_->velocity_drivetrain().plant().coefficients().B_continuous;
+    const double conservative_limit = trajectory_->VoltageVelocityLimit(
+      distance, Trajectory::VoltageLimit::kConservative, &U);
+    ASSERT_LT(0.0, conservative_limit)
+        << "Voltage limit should be strictly positive.";
+    const bool on_straight_line = distance_spline_->DTheta(distance) == 0 &&
+                                  distance_spline_->DDTheta(distance) == 0;
+    if (on_straight_line) {
+      EXPECT_EQ(::std::numeric_limits<double>::infinity(), conservative_limit);
+    } else {
+      const ::Eigen::Matrix<double, 2, 1> wheel_accel =
+          A * K2 * conservative_limit + B * U;
+      // TODO(james): Technically, K2 can contain zeros.
+      const ::Eigen::Matrix<double, 2, 1> implied_accels =
+          K2.cwiseInverse().cwiseProduct(
+              wheel_accel - K1 * ::std::pow(conservative_limit, 2));
+      EXPECT_NEAR(implied_accels(0), implied_accels(1), 1e-9);
+      EXPECT_TRUE((GetParam().voltage_limit == U.array().abs()).all()) << U;
+      const double accel = implied_accels(0);
+      // Check that we really are at a limit by confirming that even slightly
+      // disturbing the acceleration in either direction would result in invalid
+      // accelerations.
+      for (const double perturbed_accel : {accel - 1e-5, accel + 1e-5}) {
+        const ::Eigen::Matrix<double, 2, 1> perturbed_wheel_accels =
+            K2 * perturbed_accel + K1 * conservative_limit * conservative_limit;
+        const ::Eigen::Matrix<double, 2, 1> perturbed_voltages =
+            B.inverse() *
+            (perturbed_wheel_accels - A * K2 * conservative_limit);
+        EXPECT_GT(perturbed_voltages.lpNorm<::Eigen::Infinity>(),
+                  GetParam().voltage_limit)
+            << "We were able to perturb the voltage!!.";
+      }
+    }
+    length_plan_vl_.push_back(U(0));
+    length_plan_vr_.push_back(U(1));
+
+    // And check the same for the "aggressive" configuration:
+    {
+      const double aggressive_limit = trajectory_->VoltageVelocityLimit(
+          distance, Trajectory::VoltageLimit::kAggressive, &U);
+      if (on_straight_line) {
+        EXPECT_EQ(::std::numeric_limits<double>::infinity(),
+                  aggressive_limit);
+        continue;
+      }
+      EXPECT_TRUE((GetParam().voltage_limit == U.array().abs()).all()) << U;
+      const ::Eigen::Matrix<double, 2, 1> wheel_accel =
+          A * K2 * aggressive_limit + B * U;
+      const ::Eigen::Matrix<double, 2, 1> implied_accels =
+          K2.cwiseInverse().cwiseProduct(wheel_accel -
+                                         K1 * ::std::pow(aggressive_limit, 2));
+      EXPECT_NEAR(implied_accels(0), implied_accels(1), 1e-9);
+      EXPECT_LE(conservative_limit, aggressive_limit)
+          << "The aggressive velocity limit should not be less than the "
+             "conservative.";
+      const double accel = implied_accels(0);
+      for (const double perturbed_accel : {accel - 1e-5, accel + 1e-5}) {
+        const ::Eigen::Matrix<double, 2, 1> perturbed_wheel_accels =
+            K2 * perturbed_accel + K1 * aggressive_limit * aggressive_limit;
+        const ::Eigen::Matrix<double, 2, 1> perturbed_voltages =
+            B.inverse() *
+            (perturbed_wheel_accels - A * K2 * aggressive_limit);
+        EXPECT_GT(perturbed_voltages.lpNorm<::Eigen::Infinity>(),
+                  GetParam().voltage_limit)
+            << "We were able to perturb the voltage!!.";
+      }
+    }
+
+  }
+}
+
+// Tests that the friction-based velocity limits are correct.
+TEST_P(ParameterizedSplineTest, FrictionLimitCheck) {
+  // To do this check, retrieve the lateral acceleration velocity limit and
+  // confirm that we can indeed travel at that velocity without violating
+  // friction constraints and that we cannot go any faster.
+  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> K2 =
+        trajectory_->K2(distance_spline_->DTheta(distance));
+    const ::Eigen::Matrix<double, 2, 1> K1 =
+        trajectory_->K1(distance_spline_->DDTheta(distance));
+    const ::Eigen::Matrix<double, 2, 2> A =
+        trajectory_->velocity_drivetrain().plant().coefficients().A_continuous;
+    const ::Eigen::Matrix<double, 2, 2> B =
+        trajectory_->velocity_drivetrain().plant().coefficients().B_continuous;
+    const bool on_straight_line = distance_spline_->DTheta(distance) == 0 &&
+                                  distance_spline_->DDTheta(distance) == 0;
+    const double velocity_limit =
+        trajectory_->LateralVelocityCurvature(distance);
+    ASSERT_LT(0.0, velocity_limit)
+        << "Acceleration limit should be strictly positive.";
+    if (on_straight_line) {
+      EXPECT_EQ(::std::numeric_limits<double>::infinity(), velocity_limit);
+      continue;
+    }
+    const double lat_accel =
+        velocity_limit * velocity_limit * distance_spline_->DTheta(distance);
+    const double allowed_lng_accel =
+        GetParam().longitudinal_acceleration *
+        ::std::sqrt(1.0 -
+                    ::std::pow(lat_accel / GetParam().lateral_acceleration, 2));
+    ::Eigen::Matrix<double, 2, 1> wheel_accels;
+    wheel_accels << 1.0, ((K2(0) * K2(1) > 0.0) ? -1.0 : 1.0);
+    wheel_accels *= allowed_lng_accel;
+    const ::Eigen::Matrix<double, 2, 1> implied_accels1 =
+        K2.cwiseInverse().cwiseProduct(wheel_accels -
+                                       K1 * ::std::pow(velocity_limit, 2));
+    const ::Eigen::Matrix<double, 2, 1> implied_accels2 =
+        K2.cwiseInverse().cwiseProduct(-wheel_accels -
+                                       K1 * ::std::pow(velocity_limit, 2));
+    const double accels1_err =
+        ::std::abs(implied_accels1(0) - implied_accels1(1));
+    const double accels2_err =
+        ::std::abs(implied_accels2(0) - implied_accels2(1));
+    EXPECT_LT(::std::min(accels1_err, accels2_err), 1e-10);
+    const double implied_accel =
+        (accels1_err < accels2_err) ? implied_accels1(0) : implied_accels2(0);
+    // Confirm that we are indeed on the edge of feasibility by testing that
+    // we can't accelerate any faste/slower at this velocity without violating
+    // acceleration constraints.
+    for (const double perturbed_accel :
+         {implied_accel - 1e-5, implied_accel + 1e-5}) {
+      const ::Eigen::Matrix<double, 2, 1> perturbed_wheel_accels =
+          K2 * perturbed_accel + K1 * velocity_limit * velocity_limit;
+      EXPECT_GT(perturbed_wheel_accels.lpNorm<::Eigen::Infinity>(),
+                allowed_lng_accel)
+          << "We were able to perturb the acceleration!!.";
+    }
+    {
+      const ::Eigen::Matrix<double, 2, 1> U =
+          B.inverse() * (wheel_accels - A * K2 * velocity_limit);
+      length_plan_vl_.push_back(U(0));
+      length_plan_vr_.push_back(U(1));
+    }
+
+    {
+      // Also test the utility function for determining the acceleration
+      // limits:
+      double min_accel, max_accel;
+      trajectory_->FrictionLngAccelLimits(distance, velocity_limit, &min_accel,
+                                          &max_accel);
+      // If on the limit, the min/max acceleration limits should be identical.
+      EXPECT_NEAR(min_accel, max_accel, 1e-10);
+      EXPECT_NEAR(implied_accel, min_accel, 1e-10);
+    }
+  }
+}
 
 // Tests that following a spline with feed forwards only gets pretty darn close
 // to the right point.
 TEST_P(ParameterizedSplineTest, FFSpline) {
   ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+  state = trajectory_->GoalState(0.0, 0.0);
 
   for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
     const double distance = length_plan_xva_[i](0);
@@ -223,11 +363,11 @@
           return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
                                     trajectory_->Tlr_to_la(), X, U);
         },
-        state, U, ::y2016::control_loops::drivetrain::kDt);
+        state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
   }
 
   EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
-            2e-2);
+            4e-2);
 }
 
 // Tests that following a spline with both feed forwards and feed back gets
@@ -244,7 +384,7 @@
     const ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
 
     const ::Eigen::Matrix<double, 2, 5> K =
-        trajectory_->KForState(state, ParameterizedSplineTest::kDt, Q, R);
+        trajectory_->KForState(state, dt_config_.dt, Q, R);
 
     const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
     const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
@@ -258,18 +398,22 @@
           return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
                                     trajectory_->Tlr_to_la(), X, U);
         },
-        state, U, ::y2016::control_loops::drivetrain::kDt);
+        state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
   }
 
   EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
-            1.2e-2);
+            2e-2);
 }
 
 // Tests that Iteratively computing the XVA plan is the same as precomputing it.
 TEST_P(ParameterizedSplineTest, IterativeXVA) {
+  for (double v : trajectory_->plan()) {
+    EXPECT_TRUE(::std::isfinite(v));
+  }
   ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
   for (size_t i = 1; i < length_plan_xva_.size(); ++i) {
-    ::Eigen::Matrix<double, 3, 1> xva = trajectory_->GetNextXVA(kDt, &state);
+    ::Eigen::Matrix<double, 3, 1> xva =
+        trajectory_->GetNextXVA(dt_config_.dt, &state);
     EXPECT_LT((length_plan_xva_[i] - xva).norm(), 1e-2);
   }
 }
@@ -289,54 +433,69 @@
 INSTANTIATE_TEST_CASE_P(
     SplineTest, ParameterizedSplineTest,
     ::testing::Values(
-        // Normal spline.
-        MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
-                               -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
-                                  .finished(),
-                              2.0 /*lateral acceleration*/,
-                              1.0 /*longitudinal acceleration*/,
-                              10.0 /* velocity limit */, 12.0 /* volts */,
-                              NullTrajectoryModificationFunction}),
+        MakeSplineTestParams(
+            {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2, -0.2, 1.0,
+                         0.0, 0.0, 1.0, 1.0)
+                            .finished()),
+             2.0 /*lateral acceleration*/, 1.0 /*longitudinal acceleration*/,
+             10.0 /* velocity limit */, 12.0 /* volts */,
+             NullTrajectoryModificationFunction}),
         // Be velocity limited.
-        MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
-                               -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
-                                  .finished(),
-                              2.0 /*lateral acceleration*/,
-                              1.0 /*longitudinal acceleration*/,
-                              0.5 /* velocity limit */, 12.0 /* volts */,
-                              NullTrajectoryModificationFunction}),
+        MakeSplineTestParams(
+            {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0, -1.0, 5.0,
+                         0.0, 0.0, 1.0, 1.0)
+                            .finished()),
+             2.0 /*lateral acceleration*/, 1.0 /*longitudinal acceleration*/,
+             0.5 /* velocity limit */, 12.0 /* volts */,
+             NullTrajectoryModificationFunction}),
         // Hit the voltage limit.
-        MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
-                               -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
-                                  .finished(),
-                              2.0 /*lateral acceleration*/,
-                              3.0 /*longitudinal acceleration*/,
-                              10.0 /* velocity limit */, 5.0 /* volts */,
-                              NullTrajectoryModificationFunction}),
+        MakeSplineTestParams(
+            {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0, -1.0, 5.0,
+                         0.0, 0.0, 1.0, 1.0)
+                            .finished()),
+             2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+             10.0 /* velocity limit */, 5.0 /* volts */,
+             NullTrajectoryModificationFunction}),
         // Hit the curvature limit.
-        MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
-                               -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
-                                  .finished(),
-                              1.0 /*lateral acceleration*/,
-                              3.0 /*longitudinal acceleration*/,
-                              10.0 /* velocity limit */, 12.0 /* volts */,
-                              NullTrajectoryModificationFunction}),
+        MakeSplineTestParams(
+            {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2, -0.2, 1.0,
+                         0.0, 0.0, 1.0, 1.0)
+                            .finished()),
+             1.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+             10.0 /* velocity limit */, 12.0 /* volts */,
+             NullTrajectoryModificationFunction}),
         // Add an artifical velocity limit in the middle.
-        MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
-                               -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
-                                  .finished(),
-                              2.0 /*lateral acceleration*/,
-                              3.0 /*longitudinal acceleration*/,
-                              10.0 /* velocity limit */, 12.0 /* volts */,
-                              LimitMiddleOfPathTrajectoryModificationFunction}),
+        MakeSplineTestParams(
+            {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0, -1.0, 5.0,
+                         0.0, 0.0, 1.0, 1.0)
+                            .finished()),
+             2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+             10.0 /* velocity limit */, 12.0 /* volts */,
+             LimitMiddleOfPathTrajectoryModificationFunction}),
         // Add a really short artifical velocity limit in the middle.
-        MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
-                               -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
-                                  .finished(),
-                              2.0 /*lateral acceleration*/,
-                              3.0 /*longitudinal acceleration*/,
-                              10.0 /* velocity limit */, 12.0 /* volts */,
-                              ShortLimitMiddleOfPathTrajectoryModificationFunction})));
+        MakeSplineTestParams(
+            {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0, -1.0, 5.0,
+                         0.0, 0.0, 1.0, 1.0)
+                            .finished()),
+             2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+             10.0 /* velocity limit */, 12.0 /* volts */,
+             ShortLimitMiddleOfPathTrajectoryModificationFunction}),
+        // Spline known to have caused issues in the past.
+        MakeSplineTestParams(
+            {(::Eigen::Matrix<double, 2, 6>() << 0.5f, 3.5f, 4.0f, 8.0f, 10.0f,
+              10.2f, 1.0f, 1.0f, -3.0f, -2.0f, -3.5f, -3.65f)
+                 .finished(),
+             2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+             200.0 /* velocity limit */, 12.0 /* volts */,
+             NullTrajectoryModificationFunction}),
+        // Perfectly straight line (to check corner cases).
+        MakeSplineTestParams(
+            {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.0, 2.0, 3.0,
+                         0.0, 0.0, 0.0, 0.0)
+                            .finished()),
+             2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+             200.0 /* velocity limit */, 12.0 /* volts */,
+             NullTrajectoryModificationFunction})));
 
 // 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