From 85de33268d9df6619ed9a143402a32cdd50f98d2 Mon Sep 17 00:00:00 2001
From: Vignesh Balasubramaniam <vigneshb.hp@gmail.com>
Date: Fri, 30 Aug 2024 17:43:36 -0500
Subject: [PATCH] Set Spark to desired position once smooth motion is finished

---
 .../org/lasarobotics/hardware/revrobotics/Spark.java     | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
index 6a2fc87e..7a462dc3 100644
--- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
+++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
@@ -588,6 +588,15 @@ private void handleSmoothMotion() {
     );
 
     m_isSmoothMotionEnabled = !isSmoothMotionFinished();
+
+    if (!m_isSmoothMotionEnabled) {
+      set(
+        m_desiredState.position,
+        ControlType.kPosition,
+        m_feedforwardSupplier.apply(m_desiredState),
+        SparkPIDController.ArbFFUnits.kVoltage
+      );
+    }
   }
 
   /**