diff --git a/frc971/control_loops/drivetrain/spline.cc b/frc971/control_loops/drivetrain/spline.cc
index 941d520..609d3b1 100644
--- a/frc971/control_loops/drivetrain/spline.cc
+++ b/frc971/control_loops/drivetrain/spline.cc
@@ -4,6 +4,38 @@
 namespace control_loops {
 namespace drivetrain {
 
+::Eigen::Matrix<double, 2, 6> Spline4To6(
+    const ::Eigen::Matrix<double, 2, 4> &control_points) {
+  ::Eigen::Matrix<double, 2, 6> new_control_points;
+  // a' = a
+  // b' = (2a + 3b) / 5
+  // c' = (a + 6b + 3c) / 10
+  // d' = (d + 6c + 3b) / 10
+  // e' = (2d + 3c) / 5
+  // f' = d
+  new_control_points.block<2, 1>(0, 0) = control_points.block<2, 1>(0, 0);
+  new_control_points.block<2, 1>(0, 1) =
+      (2.0 * control_points.block<2, 1>(0, 0) +
+       3.0 * control_points.block<2, 1>(0, 1)) /
+      5.0;
+  new_control_points.block<2, 1>(0, 2) =
+      (control_points.block<2, 1>(0, 0) +
+       6.0 * control_points.block<2, 1>(0, 1) +
+       3.0 * control_points.block<2, 1>(0, 2)) /
+      10.0;
+  new_control_points.block<2, 1>(0, 3) =
+      (control_points.block<2, 1>(0, 3) +
+       6.0 * control_points.block<2, 1>(0, 2) +
+       3.0 * control_points.block<2, 1>(0, 1)) /
+      10.0;
+  new_control_points.block<2, 1>(0, 4) =
+      (2.0 * control_points.block<2, 1>(0, 3) +
+       3.0 * control_points.block<2, 1>(0, 2)) /
+      5.0;
+  new_control_points.block<2, 1>(0, 5) = control_points.block<2, 1>(0, 3);
+  return new_control_points;
+}
+
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/spline.h b/frc971/control_loops/drivetrain/spline.h
index 7a77082..c5b94cc 100644
--- a/frc971/control_loops/drivetrain/spline.h
+++ b/frc971/control_loops/drivetrain/spline.h
@@ -150,6 +150,10 @@
            1.0 / magdxy2 * (dx * dddy - dy * dddx);
   }
 
+  const ::Eigen::Matrix<double, 2, N> &control_points() const {
+    return control_points_;
+  }
+
  private:
   const ::Eigen::Matrix<double, 2, N> control_points_;
 
@@ -163,6 +167,10 @@
 
 typedef NSpline<4> Spline;
 
+// Converts a 4 control point spline into
+::Eigen::Matrix<double, 2, 6> Spline4To6(
+    const ::Eigen::Matrix<double, 2, 4> &control_points);
+
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/spline_test.cc b/frc971/control_loops/drivetrain/spline_test.cc
index 9aa3325..3ba856d 100644
--- a/frc971/control_loops/drivetrain/spline_test.cc
+++ b/frc971/control_loops/drivetrain/spline_test.cc
@@ -17,10 +17,15 @@
 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_;
+      : control_points_((::Eigen::Matrix<double, 2, 4>() << 0.0, 0.5, 0.5, 1.0,
+                         0.0, 0.0, 1.0, 1.0)
+                            .finished()),
+        spline4_(control_points_),
+        spline6_(Spline4To6(control_points_)) {}
+
+  ::Eigen::Matrix<double, 2, 4> control_points_;
+  NSpline<4> spline4_;
+  NSpline<6> spline6_;
 };
 
 // Tests that the derivitives of xy integrate back up to the position.
@@ -36,18 +41,18 @@
   ::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);
+  ::Eigen::Matrix<double, 2, 1> point = spline6_.Point(0.0);
+  ::Eigen::Matrix<double, 2, 1> dpoint = spline6_.DPoint(0.0);
+  ::Eigen::Matrix<double, 2, 1> ddpoint = spline6_.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_point = spline6_.Point(alpha);
+    const ::Eigen::Matrix<double, 2, 1> expected_dpoint = spline6_.DPoint(alpha);
     const ::Eigen::Matrix<double, 2, 1> expected_ddpoint =
-        spline_.DDPoint(alpha);
+        spline6_.DDPoint(alpha);
 
     alphas_plot.push_back(alpha);
     x_plot.push_back(expected_point(0));
@@ -72,7 +77,7 @@
 
     point += dpoint * dalpha;
     dpoint += ddpoint * dalpha;
-    ddpoint += spline_.DDDPoint(alpha) * dalpha;
+    ddpoint += spline6_.DDDPoint(alpha) * dalpha;
   }
 
   // Conditionally plot the functions and their integrals to aid debugging.
@@ -88,6 +93,10 @@
     matplotlibcpp::plot(alphas_plot, idy_plot, {{"label", "idy"}});
     matplotlibcpp::legend();
 
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(x_plot, y_plot, {{"label", "spline"}});
+    matplotlibcpp::legend();
+
     matplotlibcpp::show();
   }
 }
@@ -101,15 +110,15 @@
   ::std::vector<double> idtheta_plot;
 
   const int num_points = 10000;
-  double theta = spline_.Theta(0.0);
-  double dtheta = spline_.DTheta(0.0);
+  double theta = spline6_.Theta(0.0);
+  double dtheta = spline6_.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);
+    const double expected_theta = spline6_.Theta(alpha);
+    const double expected_dtheta = spline6_.DTheta(alpha);
 
     alphas_plot.push_back(alpha);
     theta_plot.push_back(expected_theta);
@@ -126,7 +135,7 @@
     }
 
     theta += dtheta * dalpha;
-    dtheta += spline_.DDTheta(alpha) * dalpha;
+    dtheta += spline6_.DDTheta(alpha) * dalpha;
   }
 
   // Conditionally plot the functions and their integrals to aid debugging.
@@ -142,6 +151,75 @@
   }
 }
 
+// Tests that a 4 point spline has the same points as a 6 point spline built
+// with Spline4To6.
+TEST_F(SplineTest, FourToSixSpline) {
+  const int num_points = 10000;
+
+  ::std::vector<double> alphas_plot;
+  ::std::vector<double> x_plot;
+  ::std::vector<double> y_plot;
+
+  const double dalpha = 1.0 / static_cast<double>(num_points - 1);
+  for (int i = 0; i < num_points; ++i) {
+    const double alpha = dalpha * static_cast<double>(i);
+
+    const ::Eigen::Matrix<double, 2, 1> expected_point = spline4_.Point(alpha);
+    const ::Eigen::Matrix<double, 2, 1> expected_dpoint =
+        spline4_.DPoint(alpha);
+    const ::Eigen::Matrix<double, 2, 1> expected_ddpoint =
+        spline4_.DDPoint(alpha);
+    const ::Eigen::Matrix<double, 2, 1> expected_dddpoint =
+        spline4_.DDDPoint(alpha);
+
+    const ::Eigen::Matrix<double, 2, 1> point = spline6_.Point(alpha);
+    const ::Eigen::Matrix<double, 2, 1> dpoint = spline6_.DPoint(alpha);
+    const ::Eigen::Matrix<double, 2, 1> ddpoint = spline6_.DDPoint(alpha);
+    const ::Eigen::Matrix<double, 2, 1> dddpoint = spline6_.DDDPoint(alpha);
+
+    alphas_plot.push_back(alpha);
+    x_plot.push_back(point(0));
+    y_plot.push_back(point(1));
+
+    EXPECT_LT((point - expected_point).norm(), 1e-9) << ": At alpha " << alpha;
+    EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-9) << ": At alpha "
+                                                       << alpha;
+    EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-9) << ": At alpha "
+                                                         << alpha;
+    EXPECT_LT((dddpoint - expected_dddpoint).norm(), 1e-9) << ": At alpha "
+                                                           << alpha;
+  }
+
+  // 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, y_plot, {{"label", "y"}});
+    matplotlibcpp::legend();
+
+    ::std::vector<double> control4x;
+    ::std::vector<double> control4y;
+    ::std::vector<double> control6x;
+    ::std::vector<double> control6y;
+    for (int i = 0; i < 4; ++i) {
+      control4x.push_back(spline4_.control_points()(0, i));
+      control4y.push_back(spline4_.control_points()(1, i));
+    }
+    for (int i = 0; i < 6; ++i) {
+      control6x.push_back(spline6_.control_points()(0, i));
+      control6y.push_back(spline6_.control_points()(1, i));
+    }
+
+    matplotlibcpp::figure();
+    matplotlibcpp::plot(x_plot, y_plot, {{"label", "spline"}});
+    matplotlibcpp::plot(control4x, control4y, {{"label", "4 control points"}});
+    matplotlibcpp::plot(control6x, control6y, {{"label", "6 control points"}});
+    matplotlibcpp::legend();
+
+    matplotlibcpp::show();
+  }
+}
+
 }  // namespace testing
 }  // namespace drivetrain
 }  // namespace control_loops