diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 577250a..ac6e2d3 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -2,6 +2,7 @@
 
 load("//aos/build:queues.bzl", "queue_library")
 load("//tools:environments.bzl", "mcu_cpus")
+load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
 
 cc_binary(
     name = "replay_drivetrain",
@@ -346,11 +347,24 @@
     srcs = [
         "trajectory_test.cc",
     ],
+    defines =
+        cpu_select({
+            "amd64": [
+                "SUPPORT_PLOT=1",
+            ],
+            "arm": [],
+        }),
+    linkstatic = True,
     deps = [
         ":trajectory",
         "//aos/testing:googletest",
         "//aos/testing:test_shm",
         "//y2016:constants",
         "//y2016/control_loops/drivetrain:polydrivetrain_plants",
-    ],
+    ] + cpu_select({
+        "amd64": [
+            "//third_party/matplotlib-cpp",
+        ],
+        "arm": [],
+    }),
 )
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index 520ee1a9..8ba9fca 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -4,13 +4,19 @@
 #include <vector>
 
 #include "aos/testing/test_shm.h"
+#include "gflags/gflags.h"
 #include "gtest/gtest.h"
+#if defined(SUPPORT_PLOT)
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
 #include "y2016/constants.h"
 #include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 #include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
 #include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
 #include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
 
+DEFINE_bool(plot, false, "If true, plot");
+
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
@@ -66,28 +72,80 @@
   return kDrivetrainConfig;
 };
 
-class SplineTest : public ::testing::Test {
- protected:
+struct SplineTestParams {
+  ::Eigen::Matrix<double, 2, 4> control_points;
+  double lateral_acceleration;
+  double longitudal_acceleration;
+  double velocity_limit;
+  double voltage_limit;
+};
+
+class ParameterizedSplineTest
+    : 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));
 
-  DistanceSpline distance_spline_;
-  Trajectory trajectory_;
+  ::std::unique_ptr<DistanceSpline> distance_spline_;
+  ::std::unique_ptr<Trajectory> trajectory_;
   ::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
 
-  SplineTest()
-      : distance_spline_(Spline((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
-                                 -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
-                                    .finished())),
-        trajectory_(&distance_spline_, GetDrivetrainConfig()) {
-    trajectory_.set_lateral_acceleration(2.0);
-    trajectory_.set_longitudal_acceleration(1.0);
-    trajectory_.LateralAccelPass();
-    trajectory_.ForwardPass();
-    trajectory_.BackwardPass();
-    length_plan_xva_ = trajectory_.PlanXVA(kDt);
+  ParameterizedSplineTest() {}
+
+  void SetUp() {
+    distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+        new DistanceSpline(Spline(GetParam().control_points)));
+    trajectory_ = ::std::unique_ptr<Trajectory>(
+        new Trajectory(distance_spline_.get(), GetDrivetrainConfig(),
+                       GetParam().velocity_limit));
+    trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
+    trajectory_->set_longitudal_acceleration(GetParam().longitudal_acceleration);
+    trajectory_->set_longitudal_acceleration(GetParam().longitudal_acceleration);
+    trajectory_->set_voltage_limit(GetParam().voltage_limit);
+
+    initial_plan_ = trajectory_->plan();
+    trajectory_->LateralAccelPass();
+    curvature_plan_ = trajectory_->plan();
+    trajectory_->ForwardPass();
+    forward_plan_ = trajectory_->plan();
+    trajectory_->BackwardPass();
+    backward_plan_ = trajectory_->plan();
+
+    length_plan_xva_ = trajectory_->PlanXVA(kDt);
+  }
+
+  void TearDown() {
+#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_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));
+      }
+
+      matplotlibcpp::figure();
+      matplotlibcpp::plot(distances, backward_plan_, {{"label", "backward"}});
+      matplotlibcpp::plot(distances, forward_plan_, {{"label", "forward"}});
+      matplotlibcpp::plot(distances, curvature_plan_, {{"label", "lateral"}});
+      matplotlibcpp::plot(distances, initial_plan_, {{"label", "initial"}});
+      matplotlibcpp::legend();
+
+      matplotlibcpp::figure();
+      matplotlibcpp::plot(length_plan_t_, length_plan_x_, {{"label", "x"}});
+      matplotlibcpp::plot(length_plan_t_, length_plan_v_, {{"label", "v"}});
+      matplotlibcpp::plot(length_plan_t_, length_plan_a_, {{"label", "a"}});
+      matplotlibcpp::plot(length_plan_t_, length_plan_vl_, {{"label", "Vl"}});
+      matplotlibcpp::plot(length_plan_t_, length_plan_vr_, {{"label", "Vr"}});
+      matplotlibcpp::legend();
+
+      matplotlibcpp::show();
+    }
+#endif
   }
 
   static constexpr double kXPos = 0.05;
@@ -108,65 +166,120 @@
        1.0 / ::std::pow(12.0, 2))
           .finished()
           .asDiagonal();
+
+  ::std::vector<double> initial_plan_;
+  ::std::vector<double> curvature_plan_;
+  ::std::vector<double> forward_plan_;
+  ::std::vector<double> backward_plan_;
+
+  ::std::vector<double> length_plan_t_;
+  ::std::vector<double> length_plan_x_;
+  ::std::vector<double> length_plan_v_;
+  ::std::vector<double> length_plan_a_;
+  ::std::vector<double> length_plan_vl_;
+  ::std::vector<double> length_plan_vr_;
+
 };
 
-constexpr chrono::nanoseconds SplineTest::kDt;
+constexpr chrono::nanoseconds ParameterizedSplineTest::kDt;
 
 // Tests that following a spline with feed forwards only gets pretty darn close
 // to the right point.
-TEST_F(SplineTest, FFSpline) {
+TEST_P(ParameterizedSplineTest, FFSpline) {
   ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
 
   for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
     const double distance = length_plan_xva_[i](0);
 
-    const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_.FFVoltage(distance);
+    const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
     const ::Eigen::Matrix<double, 2, 1> U = U_ff;
+
+    length_plan_vl_.push_back(U(0));
+    length_plan_vr_.push_back(U(1));
     state = RungeKuttaU(
         [this](const ::Eigen::Matrix<double, 5, 1> &X,
                const ::Eigen::Matrix<double, 2, 1> &U) {
-          return ContinuousDynamics(trajectory_.velocity_drivetrain().plant(),
-                                    trajectory_.Tlr_to_la(), X, U);
+          return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
+                                    trajectory_->Tlr_to_la(), X, U);
         },
         state, U, ::y2016::control_loops::drivetrain::kDt);
   }
 
-  EXPECT_LT((state - trajectory_.GoalState(trajectory_.length(), 0.0)).norm(),
+  EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
             2e-2);
 }
 
 // Tests that following a spline with both feed forwards and feed back gets
 // pretty darn close to the right point.
-TEST_F(SplineTest, FBSpline) {
+TEST_P(ParameterizedSplineTest, FBSpline) {
   ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
 
   for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
     const double distance = length_plan_xva_[i](0);
     const double velocity = length_plan_xva_[i](1);
     const ::Eigen::Matrix<double, 5, 1> goal_state =
-        trajectory_.GoalState(distance, velocity);
+        trajectory_->GoalState(distance, velocity);
 
     const ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
 
     const ::Eigen::Matrix<double, 2, 5> K =
-        trajectory_.KForState(state, SplineTest::kDt, Q, R);
+        trajectory_->KForState(state, ParameterizedSplineTest::kDt, Q, R);
 
-    const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_.FFVoltage(distance);
+    const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
     const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
     const ::Eigen::Matrix<double, 2, 1> U = U_ff + U_fb;
+
+    length_plan_vl_.push_back(U(0));
+    length_plan_vr_.push_back(U(1));
     state = RungeKuttaU(
         [this](const ::Eigen::Matrix<double, 5, 1> &X,
                const ::Eigen::Matrix<double, 2, 1> &U) {
-          return ContinuousDynamics(trajectory_.velocity_drivetrain().plant(),
-                                    trajectory_.Tlr_to_la(), X, U);
+          return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
+                                    trajectory_->Tlr_to_la(), X, U);
         },
         state, U, ::y2016::control_loops::drivetrain::kDt);
   }
 
-  EXPECT_LT((state - trajectory_.GoalState(trajectory_.length(), 0.0)).norm(),
+  EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
             1.1e-2);
 }
 
+SplineTestParams MakeSplineTestParams(struct SplineTestParams params) {
+  return params;
+}
+
+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 */}),
+        // 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 */}),
+        // 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 */}),
+        // 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 */})));
+
 // TODO(austin): Handle saturation.  254 does this by just not going that
 // fast...  We want to maybe replan when we get behind, or something.  Maybe
 // stop moving the setpoint like our 2018 arm?