diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index abc8aaf..6dbee52 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -10,8 +10,8 @@
     ],
     deps = [
         ":drivetrain_queue",
-        "//aos/controls:replay_control_loop",
         "//aos:init",
+        "//aos/controls:replay_control_loop",
         "//frc971/queues:gyro",
     ],
 )
@@ -66,7 +66,7 @@
         "//aos/controls:polytope",
         "//aos/logging:matrix_logging",
         "//aos/logging:queue_logging",
-        "//aos/robot_state:robot_state",
+        "//aos/robot_state",
         "//aos/util:log_interval",
         "//aos/util:trapezoid_profile",
         "//frc971:shifter_hall_effect",
@@ -91,7 +91,7 @@
         "//aos/controls:polytope",
         "//aos/logging:matrix_logging",
         "//aos/logging:queue_logging",
-        "//aos/robot_state:robot_state",
+        "//aos/robot_state",
         "//aos/util:log_interval",
         "//frc971/control_loops:coerce_goal",
         "//frc971/control_loops:state_feedback_loop",
@@ -257,3 +257,26 @@
         "//frc971/control_loops:state_feedback_loop",
     ],
 )
+
+cc_library(
+    name = "spline",
+    srcs = ["spline.cc"],
+    hdrs = ["spline.h"],
+    deps = [
+        "//third_party/eigen",
+    ],
+)
+
+cc_test(
+    name = "spline_test",
+    srcs = [
+        "spline_test.cc",
+    ],
+    restricted_to = ["//tools:k8"],
+    deps = [
+        ":spline",
+        "//aos/testing:googletest",
+        "//third_party/matplotlib-cpp",
+        "@com_github_gflags_gflags//:gflags",
+    ],
+)
diff --git a/frc971/control_loops/drivetrain/spline.cc b/frc971/control_loops/drivetrain/spline.cc
new file mode 100644
index 0000000..f75c6eb
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline.cc
@@ -0,0 +1,48 @@
+#include "frc971/control_loops/drivetrain/spline.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// TODO(austin): We are re-computing dpoints a lot here.  Figure out how to pass
+// them in.
+double Spline::Theta(double alpha) const {
+  const ::Eigen::Matrix<double, 2, 1> dp = DPoint(alpha);
+  return ::std::atan2(dp(1), dp(0));
+}
+
+double Spline::DTheta(double alpha) const {
+  const ::Eigen::Matrix<double, 2, 1> dp = DPoint(alpha);
+  const ::Eigen::Matrix<double, 2, 1> ddp = DDPoint(alpha);
+  const double dx = dp(0);
+  const double dy = dp(1);
+
+  const double ddx = ddp(0);
+  const double ddy = ddp(1);
+
+  return 1.0 / (::std::pow(dx, 2) + ::std::pow(dy, 2)) * (dx * ddy - dy * ddx);
+}
+
+double Spline::DDTheta(double alpha) const {
+  const ::Eigen::Matrix<double, 2, 1> dp = DPoint(alpha);
+  const ::Eigen::Matrix<double, 2, 1> ddp = DDPoint(alpha);
+  const ::Eigen::Matrix<double, 2, 1> dddp = DDDPoint(alpha);
+  const double dx = dp(0);
+  const double dy = dp(1);
+
+  const double ddx = ddp(0);
+  const double ddy = ddp(1);
+
+  const double dddx = dddp(0);
+  const double dddy = dddp(1);
+
+  const double magdxy2 = ::std::pow(dx, 2) + ::std::pow(dy, 2);
+
+  return -1.0 / (::std::pow(magdxy2, 2)) * (dx * ddy - dy * ddx) * 2.0 *
+             (dy * ddy + dx * ddx) +
+         1.0 / magdxy2 * (dx * dddy - dy * dddx);
+}
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/spline.h b/frc971/control_loops/drivetrain/spline.h
new file mode 100644
index 0000000..4d8533f
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -0,0 +1,82 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINE_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINE_H_
+
+#include "Eigen/Dense"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// Class to hold a spline as a function of alpha.  Alpha can only range between
+// 0.0 and 1.0.
+// TODO(austin): Need to be able to represent splines which have more than 2
+// control points at some point.  Or splines chained together.  This is close
+// enough for now.
+class Spline {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  // Constructs a spline.  control_points is a matrix of start, control1,
+  // control2, end.
+  Spline(::Eigen::Matrix<double, 2, 4> control_points)
+      : control_points_(control_points) {}
+
+  // Returns the xy coordiate of the spline for a given alpha.
+  ::Eigen::Matrix<double, 2, 1> Point(double alpha) const {
+    return control_points_ *
+           (::Eigen::Matrix<double, 4, 1>() << ::std::pow((1.0 - alpha), 3),
+            3.0 * ::std::pow((1.0 - alpha), 2) * alpha,
+            3.0 * (1.0 - alpha) * ::std::pow(alpha, 2), ::std::pow(alpha, 3))
+               .finished();
+  }
+
+  // Returns the dspline/dalpha for a given alpha.
+  ::Eigen::Matrix<double, 2, 1> DPoint(double alpha) const {
+    return control_points_ *
+           (::Eigen::Matrix<double, 4, 1>()
+                << -3.0 * ::std::pow((1.0 - alpha), 2),
+            3.0 * ::std::pow((1.0 - alpha), 2) +
+                -2.0 * 3.0 * (1.0 - alpha) * alpha,
+            -3.0 * ::std::pow(alpha, 2) + 2.0 * 3.0 * (1.0 - alpha) * alpha,
+            3.0 * ::std::pow(alpha, 2))
+               .finished();
+  }
+
+  // Returns the d^2spline/dalpha^2 for a given alpha.
+  ::Eigen::Matrix<double, 2, 1> DDPoint(double alpha) const {
+    return control_points_ *
+           (::Eigen::Matrix<double, 4, 1>() << 2.0 * 3.0 * (1.0 - alpha),
+            -2.0 * 3.0 * (1.0 - alpha) + -2.0 * 3.0 * (1.0 - alpha) +
+                2.0 * 3.0 * alpha,
+            -2.0 * 3.0 * alpha + 2.0 * 3.0 * (1.0 - alpha) - 2.0 * 3.0 * alpha,
+            2.0 * 3.0 * alpha)
+               .finished();
+  }
+
+  // Returns the d^3spline/dalpha^3 for a given alpha.
+  ::Eigen::Matrix<double, 2, 1> DDDPoint(double /*alpha*/) const {
+    return control_points_ *
+           (::Eigen::Matrix<double, 4, 1>() << -2.0 * 3.0,
+            2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
+            -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0, 2.0 * 3.0)
+               .finished();
+  }
+
+  // Returns theta for a given alpha.
+  double Theta(double alpha) const;
+
+  // Returns dtheta/dalpha for a given alpha.
+  double DTheta(double alpha) const;
+
+  // Returns d^2 theta/dalpha^2 for a given alpha.
+  double DDTheta(double alpha) const;
+
+ private:
+  ::Eigen::Matrix<double, 2, 4> control_points_;
+};
+
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINE_H_
diff --git a/frc971/control_loops/drivetrain/spline_test.cc b/frc971/control_loops/drivetrain/spline_test.cc
new file mode 100644
index 0000000..9aa3325
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline_test.cc
@@ -0,0 +1,148 @@
+#include "frc971/control_loops/drivetrain/spline.h"
+
+#include <vector>
+
+#include "gflags/gflags.h"
+#include "gtest/gtest.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
+DEFINE_bool(plot, false, "If true, plot");
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+// Test fixture with a spline from 0, 0 to 1, 1
+class SplineTest : public ::testing::Test {
+ protected:
+  SplineTest()
+      : spline_((::Eigen::Matrix<double, 2, 4>() << 0.0, 0.5, 0.5, 1.0, 0.0,
+                 0.0, 1.0, 1.0)
+                    .finished()) {}
+  Spline spline_;
+};
+
+// Tests that the derivitives of xy integrate back up to the position.
+TEST_F(SplineTest, XYIntegral) {
+  ::std::vector<double> alphas_plot;
+  ::std::vector<double> x_plot;
+  ::std::vector<double> y_plot;
+  ::std::vector<double> ix_plot;
+  ::std::vector<double> iy_plot;
+  ::std::vector<double> dx_plot;
+  ::std::vector<double> dy_plot;
+  ::std::vector<double> idx_plot;
+  ::std::vector<double> idy_plot;
+
+  const int num_points = 10000;
+  ::Eigen::Matrix<double, 2, 1> point = spline_.Point(0.0);
+  ::Eigen::Matrix<double, 2, 1> dpoint = spline_.DPoint(0.0);
+  ::Eigen::Matrix<double, 2, 1> ddpoint = spline_.DDPoint(0.0);
+
+  const double dalpha = 1.0 / static_cast<double>(num_points - 1);
+  for (int i = 0; i < num_points; ++i) {
+    const double alpha =
+        1.0 * static_cast<double>(i) / static_cast<double>(num_points - 1);
+    const ::Eigen::Matrix<double, 2, 1> expected_point = spline_.Point(alpha);
+    const ::Eigen::Matrix<double, 2, 1> expected_dpoint = spline_.DPoint(alpha);
+    const ::Eigen::Matrix<double, 2, 1> expected_ddpoint =
+        spline_.DDPoint(alpha);
+
+    alphas_plot.push_back(alpha);
+    x_plot.push_back(expected_point(0));
+    y_plot.push_back(expected_point(1));
+    ix_plot.push_back(point(0));
+    iy_plot.push_back(point(1));
+    dx_plot.push_back(expected_dpoint(0));
+    dy_plot.push_back(expected_dpoint(1));
+    idx_plot.push_back(dpoint(0));
+    idy_plot.push_back(dpoint(1));
+
+    EXPECT_LT((point - expected_point).norm(), 1e-2) << ": At alpha " << alpha;
+    EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2) << ": At alpha "
+                                                       << alpha;
+    EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-2) << ": At alpha "
+                                                         << alpha;
+
+    // We need to record the starting state without integrating.
+    if (i == 0) {
+      continue;
+    }
+
+    point += dpoint * dalpha;
+    dpoint += ddpoint * dalpha;
+    ddpoint += spline_.DDDPoint(alpha) * dalpha;
+  }
+
+  // Conditionally plot the functions and their integrals to aid debugging.
+  if (FLAGS_plot) {
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(alphas_plot, x_plot, {{"label", "x"}});
+    matplotlibcpp::plot(alphas_plot, ix_plot, {{"label", "ix"}});
+    matplotlibcpp::plot(alphas_plot, y_plot, {{"label", "y"}});
+    matplotlibcpp::plot(alphas_plot, iy_plot, {{"label", "iy"}});
+    matplotlibcpp::plot(alphas_plot, dx_plot, {{"label", "dx"}});
+    matplotlibcpp::plot(alphas_plot, idx_plot, {{"label", "idx"}});
+    matplotlibcpp::plot(alphas_plot, dy_plot, {{"label", "dy"}});
+    matplotlibcpp::plot(alphas_plot, idy_plot, {{"label", "idy"}});
+    matplotlibcpp::legend();
+
+    matplotlibcpp::show();
+  }
+}
+
+// Tests that the derivitives of theta integrate back up to the angle.
+TEST_F(SplineTest, ThetaIntegral) {
+  ::std::vector<double> alphas_plot;
+  ::std::vector<double> theta_plot;
+  ::std::vector<double> itheta_plot;
+  ::std::vector<double> dtheta_plot;
+  ::std::vector<double> idtheta_plot;
+
+  const int num_points = 10000;
+  double theta = spline_.Theta(0.0);
+  double dtheta = spline_.DTheta(0.0);
+
+  const double dalpha = 1.0 / static_cast<double>(num_points - 1);
+  for (int i = 0; i < num_points; ++i) {
+    const double alpha =
+        1.0 * static_cast<double>(i) / static_cast<double>(num_points - 1);
+    const double expected_theta = spline_.Theta(alpha);
+    const double expected_dtheta = spline_.DTheta(alpha);
+
+    alphas_plot.push_back(alpha);
+    theta_plot.push_back(expected_theta);
+    itheta_plot.push_back(theta);
+    dtheta_plot.push_back(expected_dtheta);
+    idtheta_plot.push_back(dtheta);
+
+    EXPECT_NEAR(expected_theta, theta, 1e-2) << ": At alpha " << alpha;
+    EXPECT_NEAR(expected_dtheta, dtheta, 1e-2) << ": At alpha " << alpha;
+
+    // We need to record the starting state without integrating.
+    if (i == 0) {
+      continue;
+    }
+
+    theta += dtheta * dalpha;
+    dtheta += spline_.DDTheta(alpha) * dalpha;
+  }
+
+  // Conditionally plot the functions and their integrals to aid debugging.
+  if (FLAGS_plot) {
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(alphas_plot, theta_plot, {{"label", "theta"}});
+    matplotlibcpp::plot(alphas_plot, itheta_plot, {{"label", "itheta"}});
+    matplotlibcpp::plot(alphas_plot, dtheta_plot, {{"label", "dtheta"}});
+    matplotlibcpp::plot(alphas_plot, idtheta_plot, {{"label", "idtheta"}});
+    matplotlibcpp::legend();
+
+    matplotlibcpp::show();
+  }
+}
+
+}  // namespace testing
+}  // namespace drivetrain
+}  // namespace control_loops
+}  // namespace frc971