diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
index cbfc8c1e75..e5d5b118f8 100644
--- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -166,7 +166,12 @@ SplineHelper::QuinticControlVectorsFromWaypoints(
     const auto scalar =
         1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
 
-    vectors.push_back(QuinticControlVector(scalar, p0));
+    // 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, 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 a419bf9d9e..d41048a0bf 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
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -69,7 +69,13 @@ public final class SplineHelper {
       // This just makes the splines look better.
       final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
 
-      vectors.add(getQuinticControlVector(scalar, p0));
+      // 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, p1));
     }
     return vectors;