diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
index e5d5b118f8..a6b6a6996c 100644
--- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
@@ -123,7 +123,9 @@ std::vector<QuinticHermiteSpline>
 SplineHelper::QuinticSplinesFromControlVectors(
     const std::vector<Spline<5>::ControlVector>& controlVectors) {
   std::vector<QuinticHermiteSpline> splines;
-  for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
+  // There are twice as many control vectors are there are splines,
+  // so we increment the counter by 2.
+  for (size_t i = 0; i < controlVectors.size() - 1; i += 2) {
     auto& xInitial = controlVectors[i].x;
     auto& yInitial = controlVectors[i].y;
     auto& xFinal = controlVectors[i + 1].x;
@@ -166,12 +168,7 @@ SplineHelper::QuinticControlVectorsFromWaypoints(
     const auto scalar =
         1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
 
-    // Only add the first control vector if this is the first iteration. The
-    // last control vector of this iteration is the first control vector of
-    // the next iteration.
-    if (i == 0) {
-      vectors.push_back(QuinticControlVector(scalar, p0));
-    }
+    vectors.push_back(QuinticControlVector(scalar, p0));
     vectors.push_back(QuinticControlVector(scalar, p1));
   }
   return vectors;
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
index d41048a0bf..e6ca75d03f 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
@@ -69,13 +69,7 @@ public final class SplineHelper {
       // This just makes the splines look better.
       final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
 
-      // Only add the first control vector if this is the first iteration. The
-      // last control vector of this iteration is the first control vector of
-      // the next iteration.
-      if (i == 0) {
-        vectors.add(getQuinticControlVector(scalar, p0));
-      }
-
+      vectors.add(getQuinticControlVector(scalar, p0));
       vectors.add(getQuinticControlVector(scalar, p1));
     }
     return vectors;
@@ -219,14 +213,15 @@ public final class SplineHelper {
   @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
   public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
       Spline.ControlVector[] controlVectors) {
-    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
-    for (int i = 0; i < controlVectors.length - 1; i++) {
+    // There are twice as many control vectors are there are splines.
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length / 2];
+    for (int i = 0; i < controlVectors.length - 1; i += 2) {
       var xInitial = controlVectors[i].x;
       var xFinal = controlVectors[i + 1].x;
       var yInitial = controlVectors[i].y;
       var yFinal = controlVectors[i + 1].y;
-      splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
-                                            yInitial, yFinal);
+      splines[i / 2] = new QuinticHermiteSpline(xInitial, xFinal,
+          yInitial, yFinal);
     }
     return splines;
   }