From 506d78b2f89bb8e50b9e54fbe51266653299abba Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Fri, 6 May 2016 11:03:56 -0700
Subject: [PATCH] Run the advance_isr faster instead of doing multiple e-steps
 per interrupt

---
 Marlin/stepper.cpp | 34 ++++++++++++++++++++++------------
 1 file changed, 22 insertions(+), 12 deletions(-)

diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 0ede6d3b7..bc070178e 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -506,7 +506,19 @@ void Stepper::isr() {
 
   void Stepper::advance_isr() {
 
-    old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
+    byte maxesteps = 0;
+    for (uint8_t i = 0; i < EXTRUDERS; i++)
+      if (abs(e_steps[i]) > maxesteps) maxesteps = abs(e_steps[i]);
+
+    if (maxesteps > 3)
+      old_OCR0A += 13;  // ~19kHz (250000/13 = 19230 Hz)
+    else if (maxesteps > 2)
+      old_OCR0A += 17;  // ~15kHz (250000/17 = 14705 Hz)
+    else if (maxesteps > 1)
+      old_OCR0A += 26;  // ~10kHz (250000/26 =  9615 Hz)
+    else
+      old_OCR0A += 52;  //  ~5kHz (250000/26 =  4807 Hz)
+
     OCR0A = old_OCR0A;
 
     #define STEP_E_ONCE(INDEX) \
@@ -523,19 +535,17 @@ void Stepper::isr() {
         E## INDEX ##_STEP_WRITE(!INVERT_E_STEP_PIN); \
       }
 
-    // Step all E steppers that have steps, up to 4 steps per interrupt
-    for (unsigned char i = 0; i < 4; i++) {
-      STEP_E_ONCE(0);
-      #if EXTRUDERS > 1
-        STEP_E_ONCE(1);
-        #if EXTRUDERS > 2
-          STEP_E_ONCE(2);
-          #if EXTRUDERS > 3
-            STEP_E_ONCE(3);
-          #endif
+    // Step all E steppers that have steps
+    STEP_E_ONCE(0);
+    #if EXTRUDERS > 1
+      STEP_E_ONCE(1);
+      #if EXTRUDERS > 2
+        STEP_E_ONCE(2);
+        #if EXTRUDERS > 3
+          STEP_E_ONCE(3);
         #endif
       #endif
-    }
+    #endif
 
   }