From a132919bdbb59dd913654f19149f282b3021a362 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <thinkyhead@users.noreply.github.com>
Date: Sun, 7 Jan 2024 21:24:31 -0600
Subject: [PATCH] Move to ftMotion_stepper

---
 Marlin/src/HAL/ESP32/i2s.cpp  |  8 +-------
 Marlin/src/module/stepper.cpp | 10 ++++++++--
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/Marlin/src/HAL/ESP32/i2s.cpp b/Marlin/src/HAL/ESP32/i2s.cpp
index 9fb5fe924b112..2fa2eaf8e06cd 100644
--- a/Marlin/src/HAL/ESP32/i2s.cpp
+++ b/Marlin/src/HAL/ESP32/i2s.cpp
@@ -164,13 +164,7 @@ void stepperTask(void *parameter) {
       #if ENABLED(FT_MOTION)
 
         if (using_ftMotion) {
-          if (!nextMainISR) {
-            stepper.ftMotion_stepper();
-            IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, endstops.update()); // Check endstops now
-            TERN_(BABYSTEPPING, if (babystep.has_steps()) stepper.babystepping_isr());
-          }
-          else
-            i2s_push_sample();
+          if (!nextMainISR) stepper.ftMotion_stepper();
           nextMainISR = 0;
         }
 
diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
index 4a0ad11d6602b..3c44ff5a4ac1f 100644
--- a/Marlin/src/module/stepper.cpp
+++ b/Marlin/src/module/stepper.cpp
@@ -1511,8 +1511,6 @@ void Stepper::isr() {
         if (!nextMainISR) {               // Main ISR is ready to fire during this iteration?
           nextMainISR = FTM_MIN_TICKS;    // Set to minimum interval (a limit on the top speed)
           ftMotion_stepper();             // Run FTM Stepping
-          IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, endstops.update());         // Check endstops now
-          TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());  // Babystep if needed
         }
         interval = nextMainISR;           // Interval is either some old nextMainISR or FTM_MIN_TICKS
         nextMainISR = 0;                  // For FT Motion fire again ASAP
@@ -3493,6 +3491,8 @@ void Stepper::report_positions() {
       U_APPLY_STEP(axis_did_move.u, false), V_APPLY_STEP(axis_did_move.v, false), W_APPLY_STEP(axis_did_move.w, false)
     );
 
+    TERN_(I2S_STEPPER_STREAM, i2s_push_sample());
+
     // Begin waiting for the minimum pulse duration
     START_TIMED_PULSE();
 
@@ -3534,6 +3534,12 @@ void Stepper::report_positions() {
       U_APPLY_STEP(!STEP_STATE_U, false), V_APPLY_STEP(!STEP_STATE_V, false), W_APPLY_STEP(!STEP_STATE_W, false)
     );
 
+    // Check endstops on every step
+    IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, endstops.update());
+
+    // Also handle babystepping here
+    TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
+
   } // Stepper::ftMotion_stepper
 
   void Stepper::ftMotion_blockQueueUpdate() {