Set Spark to desired position once smooth motion is finished

lasarobotics · Aug 30, 2024 · 85de332 · 85de332
1 parent ecd9a14
commit 85de332
Showing 1 changed file with 9 additions and 0 deletions.
diff --git 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
+      );
+    }
   }
 
   /**