diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index a962a08267..e8989286b5 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -372,6 +372,7 @@ void Stepper::isr() { ) endstops.update(); // Take multiple steps per interrupt (For high speed moves) + bool all_steps_done = false; for (int8_t i = 0; i < step_loops; i++) { #ifndef USBCON customizedSerial.checkRx(); // Check for serial chars. @@ -524,8 +525,10 @@ void Stepper::isr() { #endif #endif // !ADVANCE && !LIN_ADVANCE - step_events_completed++; - if (step_events_completed >= current_block->step_event_count) break; + if (++step_events_completed >= current_block->step_event_count) { + all_steps_done = true; + break; + } } #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE) @@ -657,7 +660,7 @@ void Stepper::isr() { OCR1A = (OCR1A < (TCNT1 + 16)) ? (TCNT1 + 16) : OCR1A; // If current block is finished, reset pointer - if (step_events_completed >= current_block->step_event_count) { + if (all_steps_done) { current_block = NULL; planner.discard_current_block(); }