From 4a2241b07d2d9a29439c33c39dc4958bcdd750ad Mon Sep 17 00:00:00 2001
From: Stefan de Bruijn <stefan@nubilosoft.com>
Date: Thu, 1 Jul 2021 22:33:31 +0200
Subject: [PATCH] Fixed a few bugs in steppping: direction delays and step
 pulse initialization.

---
 Grbl_Esp32/src/I2SOut.h         |  2 +-
 Grbl_Esp32/src/Machine/Axes.cpp | 28 ++++++++++++++++++++++++++++
 Grbl_Esp32/src/Stepper.cpp      | 17 +++++++++++------
 3 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/Grbl_Esp32/src/I2SOut.h b/Grbl_Esp32/src/I2SOut.h
index 95661fd0..4a39cf8d 100644
--- a/Grbl_Esp32/src/I2SOut.h
+++ b/Grbl_Esp32/src/I2SOut.h
@@ -56,7 +56,7 @@
 
 /* 16-bit mode: 1000000 usec / ((160000000 Hz) / 10 / 2) x 16 bit/pulse x 2(stereo) = 4 usec/pulse */
 /* 32-bit mode: 1000000 usec / ((160000000 Hz) /  5 / 2) x 32 bit/pulse x 2(stereo) = 4 usec/pulse */
-const int I2S_OUT_USEC_PER_PULSE = 4;
+const uint32_t I2S_OUT_USEC_PER_PULSE = 4;
 
 const int I2S_OUT_DMABUF_COUNT = 5;    /* number of DMA buffers to store data */
 const int I2S_OUT_DMABUF_LEN   = 2000; /* maximum size in bytes (4092 is DMA's limit) */
diff --git a/Grbl_Esp32/src/Machine/Axes.cpp b/Grbl_Esp32/src/Machine/Axes.cpp
index 01f64664..5caa2192 100644
--- a/Grbl_Esp32/src/Machine/Axes.cpp
+++ b/Grbl_Esp32/src/Machine/Axes.cpp
@@ -5,6 +5,9 @@
 #include "../Motors/NullMotor.h"
 #include "../NutsBolts.h"
 #include "../MotionControl.h"
+#include "../Stepper.h"     // stepper_id_t
+#include "../I2SOut.h"      // i2s_out_push_sample
+#include "MachineConfig.h"  // config->
 
 namespace Machine {
     Axes::Axes() : _axis() {
@@ -127,6 +130,31 @@ namespace Machine {
                     }
                 }
             }
+
+            auto wait_direction = config->_directionDelayMicroSeconds;
+            if (wait_direction > 0) {
+                // Stepper drivers need some time between changing direction and doing a pulse.
+                switch (config->_stepType) {
+                    case stepper_id_t::ST_I2S_STREAM:
+                        i2s_out_push_sample(wait_direction);
+                        break;
+                    case stepper_id_t::ST_I2S_STATIC:
+                    case stepper_id_t::ST_TIMED: {
+                        // wait for step pulse time to complete...some time expired during code above
+                        //
+                        // If we are using GPIO stepping as opposed to RMT, record the
+                        // time that we turned on the direction pins so we can delay a bit.
+                        // If we are using RMT, we can't delay here.
+                        auto direction_pulse_start_time = esp_timer_get_time() + wait_direction;
+                        while ((esp_timer_get_time() - direction_pulse_start_time) < 0) {
+                            NOP();  // spin here until time to turn off step
+                        }
+                        break;
+                    }
+                    case stepper_id_t::ST_RMT:
+                        break;
+                }
+            }
         }
 
         // Turn on step pulses for motors that are supposed to step now
diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp
index fe2ca8eb..6c7f8c2f 100644
--- a/Grbl_Esp32/src/Stepper.cpp
+++ b/Grbl_Esp32/src/Stepper.cpp
@@ -366,8 +366,13 @@ void stepper_init() {
                 config->_disableDelayMicroSeconds,
                 config->_directionDelayMicroSeconds);
 
-    // I2S stepper stream mode use callback but timer interrupt
-    i2s_out_set_pulse_callback(stepper_pulse_func);
+    switch (config->_stepType) {
+        case ST_I2S_STREAM:
+        case ST_I2S_STATIC:
+            // I2S stepper stream mode use callback but timer interrupt
+            i2s_out_set_pulse_callback(stepper_pulse_func);
+            break;
+    }
 
     // Other stepper use timer interrupt
     Stepper_Timer_Init();
@@ -398,13 +403,13 @@ void st_wake_up() {
     // Enable stepper drivers.
     config->_axes->set_disable(false);
     stepper_idle = false;
-    // Initialize step pulse timing from settings. Here to ensure updating after re-writing.
 
-    // Step pulse delay handling is not require with RMT
-    if (config->_disableDelayMicroSeconds < 1) {
+    // Initialize step pulse timing from settings. Here to ensure updating after re-writing.
+    // Step pulse delay handling is not require with ESP32 RMT...the RMT function does it.
+    if (config->_stepType != ST_RMT || config->_directionDelayMicroSeconds < 1) {
         // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement.
         st.step_pulse_time = -(((config->_pulseMicroSeconds - 2) * ticksPerMicrosecond) >> 3);
-    }
+    } 
 
     // Enable Stepper Driver Interrupt
     Stepper_Timer_Start();