From 0d6e47db2b769b2f4f257610e9ff9718c60730af Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Tue, 6 Jul 2021 11:41:13 -1000 Subject: [PATCH] Big stepper / stepType cleanup Moved all the hardware dependencies out of Stepper.cpp mostly into the new file Stepping.cpp. stepType is now engine. There is a new section stepping: that holds the shared stepper delay parameters. The parameter name prefixes are now consistently either _us for microseconds or _ms for milliseconds. Nearly all the references to _engine are now encapsulated in Stepping.cpp, except for a couple that are still lurking in StandardStepper.cpp. I haven't figured out the best way to fix those. Ideally, the engine should be auto-selected based on the type of pins you use for step: . --- Grbl_Esp32/Custom/CoreXY.cpp | 9 +- Grbl_Esp32/data/6_pack_besc.yaml | 12 +- Grbl_Esp32/data/barts_3axis_test.yaml | 38 ++-- Grbl_Esp32/data/config.yaml | 17 +- Grbl_Esp32/data/spindles.yaml | 12 +- Grbl_Esp32/src/I2SOut.cpp | 60 ++--- Grbl_Esp32/src/I2SOut.h | 19 +- Grbl_Esp32/src/Limits.cpp | 6 +- Grbl_Esp32/src/Machine/Axes.cpp | 63 +----- Grbl_Esp32/src/Machine/Axes.h | 4 - Grbl_Esp32/src/Machine/MachineConfig.cpp | 11 +- Grbl_Esp32/src/Machine/MachineConfig.h | 23 +- Grbl_Esp32/src/MotionControl.cpp | 14 +- Grbl_Esp32/src/Motors/StandardStepper.cpp | 25 +- Grbl_Esp32/src/Motors/StandardStepper.h | 10 +- Grbl_Esp32/src/ProcessSettings.cpp | 4 +- Grbl_Esp32/src/Stepper.cpp | 182 ++------------- Grbl_Esp32/src/Stepper.h | 12 +- Grbl_Esp32/src/StepperPrivate.h | 7 +- Grbl_Esp32/src/Stepping.cpp | 213 ++++++++++++++++++ Grbl_Esp32/src/Stepping.h | 87 +++++++ .../test/Configuration/YamlComplete.cpp | 4 +- 22 files changed, 448 insertions(+), 384 deletions(-) create mode 100644 Grbl_Esp32/src/Stepping.cpp create mode 100644 Grbl_Esp32/src/Stepping.h diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp index b82136d7..5f6fc333 100644 --- a/Grbl_Esp32/Custom/CoreXY.cpp +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -196,13 +196,10 @@ bool user_defined_homing(AxisMask cycle_mask) { } } while (!switch_touched); -#ifdef USE_I2S_STEPS - if (config->_stepType == ST_I2S_STREAM) { - if (!approach) { - delay_ms(I2S_OUT_DELAY_MS); - } + if (!approach) { + config->_stepping->synchronize(); } -#endif + st_reset(); // Immediately force kill steppers and reset step segment buffer. delay_ms(config->_homing->_debounce); // Delay to allow transient dynamics to dissipate. diff --git a/Grbl_Esp32/data/6_pack_besc.yaml b/Grbl_Esp32/data/6_pack_besc.yaml index a6b7c21c..33d1dd9d 100644 --- a/Grbl_Esp32/data/6_pack_besc.yaml +++ b/Grbl_Esp32/data/6_pack_besc.yaml @@ -1,11 +1,13 @@ name: 6 Pack StepStick BESC board: 6-pack -idle_time: 250 -step_type: I2S_static -dir_delay_microseconds: 1 -pulse_microseconds: 2 -disable_delay_us: 0 +stepping: + engine: I2S_static + idle_ms: 250 + dir_delay_us: 1 + pulse_us: 2 + disable_delay_us: 0 + homing_init_lock: false i2so: diff --git a/Grbl_Esp32/data/barts_3axis_test.yaml b/Grbl_Esp32/data/barts_3axis_test.yaml index eb4ecd06..4ce854fa 100644 --- a/Grbl_Esp32/data/barts_3axis_test.yaml +++ b/Grbl_Esp32/data/barts_3axis_test.yaml @@ -6,6 +6,13 @@ i2so: ws: gpio.17 data: gpio.21 +stepping: + engine: I2S_STREAM + idle_ms: 250 + pulse_us: 2 + dir_delay_us: 1 + disable_delay_us: 0 + axes: number_axis: 3 @@ -19,13 +26,13 @@ axes: cycle: 1 positive_direction: false mpos: 10.000 - feed_rate: 500.000 - seek_rate: 100.000 debounce: 10 pulloff: 1.000 square: false - search_scaler: 1.100 - locate_scaler: 5.000 + seek_rate: 500.000 + feed_rate: 100.000 + seek_scaler: 1.100 + feed_scaler: 1.000 gang0: endstops: @@ -50,13 +57,13 @@ axes: cycle: 2 positive_direction: false mpos: 10.000 - feed_rate: 500.000 - seek_rate: 100.000 debounce: 10 pulloff: 1.000 square: false - search_scaler: 1.100 - locate_scaler: 5.000 + seek_rate: 500.000 + feed_rate: 100.000 + seek_scaler: 1.100 + feed_scaler: 1.000 gang0: endstops: @@ -80,13 +87,13 @@ axes: cycle: 2 positive_direction: false mpos: 10.000 - feed_rate: 500.000 - seek_rate: 100.000 debounce: 10 pulloff: 1.000 square: false - search_scaler: 1.100 - locate_scaler: 5.000 + seek_rate: 500.000 + feed_rate: 100.000 + seek_scaler: 1.100 + feed_scaler: 1.000 gang0: endstops: @@ -158,14 +165,7 @@ comms: dhcp: true channel: 1 -pulse_microseconds: 2 -dir_delay_microseconds: 1 -disable_delay_us: 0 -idle_time: 250 - - software_debounce_ms: 0 -step_type: I2S_STREAM laser_mode: false arc_tolerance: 0.002 junction_deviation: 0.010 diff --git a/Grbl_Esp32/data/config.yaml b/Grbl_Esp32/data/config.yaml index 6e29c8ab..c483afee 100644 --- a/Grbl_Esp32/data/config.yaml +++ b/Grbl_Esp32/data/config.yaml @@ -2,11 +2,13 @@ name: "ESP32 Dev Controller V4" board: "ESP32 Dev Controller V4" yaml_wiki: "https://github.com/bdring/Grbl_Esp32/wiki/YAML-Config-File" -idle_time: 250 -step_type: RMT -dir_delay_microseconds: 1 -pulse_microseconds: 2 -disable_delay_us: 0 +stepping: + engine: RMT + idle_ms: 250 + dir_delay_us: 1 + pulse_us: 2 + disable_delay_us: 0 + homing_init_lock: false axes: @@ -70,9 +72,10 @@ axes: cycle: 1 mpos: 10 positive_direction: true - feed_rate: 30.000 seek_rate: 300.000 - locate_scaler: 2.000 + feed_rate: 30.000 + seek_scaler: 2.000 + feed_scaler: 1.000 debounce: 500 pulloff: 1.000 diff --git a/Grbl_Esp32/data/spindles.yaml b/Grbl_Esp32/data/spindles.yaml index 21572037..0e905311 100644 --- a/Grbl_Esp32/data/spindles.yaml +++ b/Grbl_Esp32/data/spindles.yaml @@ -2,11 +2,13 @@ name: "ESP32 Dev Controller V4" board: "ESP32 Dev Controller V4" yaml_wiki: "https://github.com/bdring/Grbl_Esp32/wiki/YAML-Config-File" -idle_time: 250 -step_type: rmt -dir_delay_microseconds: 1 -pulse_microseconds: 2 -disable_delay_us: 0 +stepping: + engine: rmt + idle_ms: 250 + dir_delay_us: 1 + pulse_us: 2 + disable_delay_us: 0 + homing_init_lock: false axes: diff --git a/Grbl_Esp32/src/I2SOut.cpp b/Grbl_Esp32/src/I2SOut.cpp index d5ff116d..f28e9032 100644 --- a/Grbl_Esp32/src/I2SOut.cpp +++ b/Grbl_Esp32/src/I2SOut.cpp @@ -49,6 +49,7 @@ #include "Settings.h" #include "SettingsDefinitions.h" #include "Machine/MachineConfig.h" +#include "Stepper.h" #include #include @@ -124,9 +125,8 @@ static portMUX_TYPE i2s_out_spinlock = portMUX_INITIALIZER_UNLOCKED; static int i2s_out_initialized = 0; #ifdef USE_I2S_OUT_STREAM_IMPL -static volatile uint32_t i2s_out_pulse_period; -static uint32_t i2s_out_remain_time_until_next_pulse; // Time remaining until the next pulse (μsec) -static volatile i2s_out_pulse_func_t i2s_out_pulse_func; +static volatile uint32_t i2s_out_pulse_period; +static uint32_t i2s_out_remain_time_until_next_pulse; // Time remaining until the next pulse (μsec) #endif static uint8_t i2s_out_ws_pin = 255; @@ -374,29 +374,28 @@ static int IRAM_ATTR i2s_fillout_dma_buffer(lldesc_t* dma_desc) { // pulser status may change in pulse phase func, so I need to check it every time. if (i2s_out_pulser_status == STEPPING) { // fillout future DMA buffer (tail of the DMA buffer chains) - if (i2s_out_pulse_func != NULL) { - uint32_t old_rw_pos = o_dma.rw_pos; - I2S_OUT_PULSER_EXIT_CRITICAL(); // Temporarily unlocked status lock as it may be locked in pulse callback. - (*i2s_out_pulse_func)(); // should be pushed into buffer max DMA_SAMPLE_SAFE_COUNT - I2S_OUT_PULSER_ENTER_CRITICAL(); // Lock again. - // Calculate pulse period. - i2s_out_remain_time_until_next_pulse += i2s_out_pulse_period - I2S_OUT_USEC_PER_PULSE * (o_dma.rw_pos - old_rw_pos); - if (i2s_out_pulser_status == WAITING) { - // i2s_out_set_passthrough() has called from the pulse function. - // It needs to go into pass-through mode. - // This DMA descriptor must be a tail of the chain. - dma_desc->qe.stqe_next = NULL; // Cut the DMA descriptor ring. This allow us to identify the tail of the buffer. - } else if (i2s_out_pulser_status == PASSTHROUGH) { - // i2s_out_reset() has called during the execution of the pulse function. - // I2S has already in static mode, and buffers has cleared to zero. - // To prevent the pulse function from being called back, - // we assume that the buffer is already full. - i2s_out_remain_time_until_next_pulse = 0; // There is no need to fill the current buffer. - o_dma.rw_pos = DMA_SAMPLE_COUNT; // The buffer is full. - break; - } - continue; + uint32_t old_rw_pos = o_dma.rw_pos; + I2S_OUT_PULSER_EXIT_CRITICAL(); // Temporarily unlocked status lock as it may be locked in pulse callback. + Stepper::pulse_func(); + + I2S_OUT_PULSER_ENTER_CRITICAL(); // Lock again. + // Calculate pulse period. + i2s_out_remain_time_until_next_pulse += i2s_out_pulse_period - I2S_OUT_USEC_PER_PULSE * (o_dma.rw_pos - old_rw_pos); + if (i2s_out_pulser_status == WAITING) { + // i2s_out_set_passthrough() has called from the pulse function. + // It needs to go into pass-through mode. + // This DMA descriptor must be a tail of the chain. + dma_desc->qe.stqe_next = NULL; // Cut the DMA descriptor ring. This allow us to identify the tail of the buffer. + } else if (i2s_out_pulser_status == PASSTHROUGH) { + // i2s_out_reset() has called during the execution of the pulse function. + // I2S has already in static mode, and buffers has cleared to zero. + // To prevent the pulse function from being called back, + // we assume that the buffer is already full. + i2s_out_remain_time_until_next_pulse = 0; // There is no need to fill the current buffer. + o_dma.rw_pos = DMA_SAMPLE_COUNT; // The buffer is full. + break; } + continue; } } // no pulse data in push buffer (pulse off or idle or callback is not defined) @@ -678,13 +677,6 @@ int IRAM_ATTR i2s_out_set_pulse_period(uint32_t period) { return 0; } -int IRAM_ATTR i2s_out_set_pulse_callback(i2s_out_pulse_func_t func) { -#ifdef USE_I2S_OUT_STREAM_IMPL - i2s_out_pulse_func = func; -#endif - return 0; -} - int IRAM_ATTR i2s_out_reset() { I2S_OUT_PULSER_ENTER_CRITICAL(); i2s_out_stop(); @@ -705,7 +697,7 @@ int IRAM_ATTR i2s_out_reset() { } // -// Initialize funtion (external function) +// Initialize function (external function) // // XXX does this really need IRAM_ATTR ? It is not called from an ISR int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { @@ -917,7 +909,6 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { // default pulse callback period (μsec) i2s_out_pulse_period = init_param.pulse_period; - i2s_out_pulse_func = init_param.pulse_func; // Create the task that will feed the buffer xTaskCreatePinnedToCore(i2sOutTask, @@ -979,7 +970,6 @@ int IRAM_ATTR i2s_out_init() { default_param.ws_pin = wsPin.getNative(Pin::Capabilities::Output | Pin::Capabilities::Native); default_param.bck_pin = bckPin.getNative(Pin::Capabilities::Output | Pin::Capabilities::Native); default_param.data_pin = dataPin.getNative(Pin::Capabilities::Output | Pin::Capabilities::Native); - default_param.pulse_func = NULL; default_param.pulse_period = I2S_OUT_USEC_PER_PULSE; default_param.init_val = I2S_OUT_INIT_VAL; diff --git a/Grbl_Esp32/src/I2SOut.h b/Grbl_Esp32/src/I2SOut.h index 4a39cf8d..36bca3a2 100644 --- a/Grbl_Esp32/src/I2SOut.h +++ b/Grbl_Esp32/src/I2SOut.h @@ -64,8 +64,6 @@ const int I2S_OUT_DMABUF_LEN = 2000; /* maximum size in bytes (4092 is DMA's l const int I2S_OUT_DELAY_DMABUF_MS = (I2S_OUT_DMABUF_LEN / sizeof(uint32_t) * I2S_OUT_USEC_PER_PULSE / 1000); const int I2S_OUT_DELAY_MS = (I2S_OUT_DELAY_DMABUF_MS * (I2S_OUT_DMABUF_COUNT + 1)); -typedef void (*i2s_out_pulse_func_t)(void); - typedef struct { /* I2S bitstream (32-bits): Transfers from MSB(bit31) to LSB(bit0) in sequence @@ -80,12 +78,11 @@ typedef struct { If I2S_OUT_PIN_BASE is set to 128, bit0:Expanded GPIO 128, 1: Expanded GPIO 129, ..., v: Expanded GPIO 159 */ - uint8_t ws_pin; - uint8_t bck_pin; - uint8_t data_pin; - i2s_out_pulse_func_t pulse_func; - uint32_t pulse_period; // aka step rate. - uint32_t init_val; + uint8_t ws_pin; + uint8_t bck_pin; + uint8_t data_pin; + uint32_t pulse_period; // aka step rate. + uint32_t init_val; } i2s_out_init_t; /* @@ -100,7 +97,6 @@ int i2s_out_init(i2s_out_init_t& init_param); .ws_pin = I2S_OUT_WS, .bck_pin = I2S_OUT_BCK, .data_pin = I2S_OUT_DATA, - .pulse_func = NULL, .pulse_period = I2S_OUT_USEC_PER_PULSE, .init_val = I2S_OUT_INIT_VAL, }; @@ -160,11 +156,6 @@ void i2s_out_delay(); */ int i2s_out_set_pulse_period(uint32_t usec); -/* - Register a callback function to generate pulse data - */ -int i2s_out_set_pulse_callback(i2s_out_pulse_func_t func); - /* Get current pulser mode */ diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index f07a3d7f..70cd2ae3 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -260,10 +260,8 @@ static void limits_go_home(uint8_t cycle_mask, uint32_t n_locate_cycles) { // Keep trying until all axes have finished } while (axislock); - if (config->_stepType == ST_I2S_STREAM) { - if (!approach) { - delay_ms(I2S_OUT_DELAY_MS); - } + if (!approach) { + config->_stepping->synchronize(); } Stepper::reset(); // Immediately force kill steppers and reset step segment buffer. diff --git a/Grbl_Esp32/src/Machine/Axes.cpp b/Grbl_Esp32/src/Machine/Axes.cpp index aa5a481a..74c03cb4 100644 --- a/Grbl_Esp32/src/Machine/Axes.cpp +++ b/Grbl_Esp32/src/Machine/Axes.cpp @@ -6,7 +6,6 @@ #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 { @@ -17,7 +16,7 @@ namespace Machine { } void Axes::init() { - info_serial("Init Motors"); + info_serial("Axis count %d", config->_axes->_numberAxis); if (_sharedStepperDisable.defined()) { _sharedStepperDisable.setAttr(Pin::Attr::Output); @@ -130,34 +129,11 @@ namespace Machine { } } } - - auto wait_direction = config->_directionDelayMicroSeconds; - if (wait_direction > 0) { - // stepping->turnaround(wait_direction); - // 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; - } - } + config->_stepping->waitDirection(); } + config->_stepping->startPulseTimer(); + // Turn on step pulses for motors that are supposed to step now for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { if (bitnum_istrue(step_mask, axis)) { @@ -172,8 +148,10 @@ namespace Machine { } } } + // Turn all stepper pins off void IRAM_ATTR Axes::unstep() { + config->_stepping->waitPulse(); auto n_axis = _numberAxis; for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { for (uint8_t gang_index = 0; gang_index < Axis::MAX_NUMBER_GANGED; gang_index++) { @@ -214,35 +192,6 @@ namespace Machine { return SIZE_MAX; } - // Wait for motion to complete; the axes can still be moving - // after the data has been sent to the stepping engine, due - // to queuing delays. - void Axes::synchronize() { - if (config->_stepType == ST_I2S_STREAM) { - // XXX instead of a delay, we could sense when the DMA and - // FIFO have drained. It might be as simple as waiting for - // I2SO.conf1.tx_start == 0, while yielding. - delay_ms(I2S_OUT_DELAY_MS); - } - } - void Axes::beginLowLatency() { - _switchedStepper = config->_stepType == ST_I2S_STREAM; - if (_switchedStepper) { - config->_stepType = ST_I2S_STATIC; - } - } - void Axes::endLowLatency() { - if (_switchedStepper) { - if (i2s_out_get_pulser_status() != PASSTHROUGH) { - // Called during streaming. Stop streaming. - debug_serial("Stop the I2S streaming and switch to the passthrough mode."); - i2s_out_set_passthrough(); - i2s_out_delay(); // Wait for a change in mode. - } - config->_stepType = ST_I2S_STREAM; - } - } - // Configuration helpers: void Axes::group(Configuration::HandlerBase& handler) { diff --git a/Grbl_Esp32/src/Machine/Axes.h b/Grbl_Esp32/src/Machine/Axes.h index d470fb1a..331e6ebe 100644 --- a/Grbl_Esp32/src/Machine/Axes.h +++ b/Grbl_Esp32/src/Machine/Axes.h @@ -67,10 +67,6 @@ namespace Machine { return false; } - void synchronize(); // Wait for motion to complete - void beginLowLatency(); - void endLowLatency(); - // These are used for setup and to talk to the motors as a group. void init(); void read_settings(); // more like 'after read settings, before init'. Oh well... diff --git a/Grbl_Esp32/src/Machine/MachineConfig.cpp b/Grbl_Esp32/src/Machine/MachineConfig.cpp index 098c707c..5e3f14d9 100644 --- a/Grbl_Esp32/src/Machine/MachineConfig.cpp +++ b/Grbl_Esp32/src/Machine/MachineConfig.cpp @@ -47,6 +47,7 @@ namespace Machine { handler.item("board", _board); handler.item("name", _name); + handler.section("stepping", _stepping); handler.section("axes", _axes); handler.section("i2so", _i2so); handler.section("spi", _spi); @@ -56,15 +57,9 @@ namespace Machine { handler.section("comms", _comms); handler.section("macros", _macros); - handler.item("pulse_microseconds", _pulseMicroSeconds); - handler.item("dir_delay_microseconds", _directionDelayMicroSeconds); - handler.item("disable_delay_us", _disableDelayMicroSeconds); - handler.item("idle_time", _idleTime); handler.section("user_outputs", _userOutputs); handler.section("sdcard", _sdCard); handler.item("software_debounce_ms", _softwareDebounceMs); - handler.item("step_type", _stepType, stepTypes); - // TODO: Consider putting these under a gcode: hierarchy level? Or motion control? handler.item("laser_mode", _laserMode); handler.item("arc_tolerance", _arcTolerance); @@ -110,6 +105,10 @@ namespace Machine { _spi = new SPIBus(); } + if (_stepping == nullptr) { + _stepping = new Stepping(); + } + // We do not auto-create an I2SO bus config node // Only if an i2so section is present will config->_i2so be non-null diff --git a/Grbl_Esp32/src/Machine/MachineConfig.h b/Grbl_Esp32/src/Machine/MachineConfig.h index 8081ed4d..b2aec1a2 100644 --- a/Grbl_Esp32/src/Machine/MachineConfig.h +++ b/Grbl_Esp32/src/Machine/MachineConfig.h @@ -28,7 +28,7 @@ #include "../Probe.h" #include "../SDCard.h" #include "../Spindles/Spindle.h" -#include "../EnumItem.h" +#include "../Stepping.h" #include "../Stepper.h" #include "../Logging.h" #include "../Config.h" @@ -47,6 +47,7 @@ namespace Machine { Axes* _axes = nullptr; SPIBus* _spi = nullptr; I2SOBus* _i2so = nullptr; + Stepping* _stepping = nullptr; CoolantControl* _coolant = nullptr; Probe* _probe = nullptr; Communications* _comms = nullptr; @@ -57,19 +58,13 @@ namespace Machine { Spindles::SpindleList _spindles; - int _pulseMicroSeconds = 3; - int _directionDelayMicroSeconds = 0; - int _disableDelayMicroSeconds = 0; - - bool _laserMode = false; - float _arcTolerance = 0.002f; - float _junctionDeviation = 0.01f; - uint8_t _idleTime = 255; - bool _verboseErrors = false; - bool _reportInches = false; - bool _homingInitLock = true; - int _softwareDebounceMs = 0; - int _stepType = ST_RMT; + bool _laserMode = false; + float _arcTolerance = 0.002f; + float _junctionDeviation = 0.01f; + bool _verboseErrors = false; + bool _reportInches = false; + bool _homingInitLock = true; + int _softwareDebounceMs = 0; // Enables a special set of M-code commands that enables and disables the parking motion. // These are controlled by `M56`, `M56 P1`, or `M56 Px` to enable and `M56 P0` to disable. diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 11ef29b0..94c77392 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -329,9 +329,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par return GCUpdatePos::None; // Return if system reset has been issued. } - int save_stepper = config->_stepType; /* remember the stepper */ - - config->_axes->beginLowLatency(); + config->_stepping->beginLowLatency(); // Initialize probing control variables bool is_probe_away = bit_istrue(parser_flags, GCParserProbeIsAway); @@ -343,7 +341,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par if (config->_probe->tripped()) { sys_rt_exec_alarm = ExecAlarm::ProbeFailInitial; protocol_execute_realtime(); - config->_axes->endLowLatency(); + config->_stepping->endLowLatency(); return GCUpdatePos::None; // Nothing else to do but bail. } // Setup and queue probing motion. Auto cycle-start should not start the cycle. @@ -356,12 +354,12 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par do { protocol_execute_realtime(); if (sys.abort) { - config->_axes->endLowLatency(); + config->_stepping->endLowLatency(); return GCUpdatePos::None; // Check for system abort } } while (sys.state != State::Idle); - config->_axes->endLowLatency(); + config->_stepping->endLowLatency(); // Probing cycle complete! // Set state variables and error out, if the probe failed and cycle with error is enabled. @@ -467,8 +465,6 @@ void mc_reset() { } ganged_mode = gangDual; // in case an error occurred during squaring - if (config->_stepType == ST_I2S_STREAM) { - i2s_out_reset(); - } + config->_stepping->reset(); } } diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index 4f6fbc8b..2fe95df1 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -25,6 +25,10 @@ #include "../Report.h" #include "../Machine/MachineConfig.h" +#include "../Stepper.h" // ST_I2S_* +#include "../Stepping.h" // config->_stepping->_engine + +using namespace Machine; namespace Motors { rmt_item32_t StandardStepper::rmtItem[2]; @@ -57,8 +61,8 @@ namespace Motors { _dir_pin.setAttr(Pin::Attr::Output); - // stepping->init(_step_pin); - if (config->_stepType == ST_RMT) { + auto stepping = config->_stepping; + if (stepping->_engine == Stepping::RMT) { rmtConfig.rmt_mode = RMT_MODE_TX; rmtConfig.clk_div = 20; rmtConfig.mem_block_num = 2; @@ -69,8 +73,8 @@ namespace Motors { rmtConfig.tx_config.carrier_level = RMT_CARRIER_LEVEL_LOW; rmtConfig.tx_config.idle_output_en = true; - rmtItem[0].duration0 = config->_directionDelayMicroSeconds < 1 ? 1 : config->_directionDelayMicroSeconds * 4; - rmtItem[0].duration1 = 4 * config->_pulseMicroSeconds; + rmtItem[0].duration0 = stepping->_directionDelayUsecs < 1 ? 1 : stepping->_directionDelayUsecs * 4; + rmtItem[0].duration1 = 4 * stepping->_pulseUsecs; rmtItem[1].duration0 = 0; rmtItem[1].duration1 = 0; @@ -105,7 +109,7 @@ namespace Motors { } void IRAM_ATTR StandardStepper::step() { - if (config->_stepType == ST_RMT) { + if (config->_stepping->_engine == Stepping::RMT) { RMT.conf_ch[_rmt_chan_num].conf1.mem_rd_rst = 1; RMT.conf_ch[_rmt_chan_num].conf1.tx_start = 1; } else { @@ -114,7 +118,7 @@ namespace Motors { } void IRAM_ATTR StandardStepper::unstep() { - if (config->_stepType != ST_RMT) { + if (config->_stepping->_engine != Stepping::RMT) { _step_pin.off(); } } @@ -127,4 +131,13 @@ namespace Motors { namespace { MotorFactory::InstanceBuilder registration("standard_stepper"); } + + void StandardStepper::validate() const { + Assert(_step_pin.defined(), "Step pin should be configured."); + Assert(_dir_pin.defined(), "Direction pin should be configured."); + bool isI2SO = config->_stepping->_engine == Stepping::I2S_STREAM || config->_stepping->_engine == Stepping::I2S_STATIC; + Assert(!isI2SO || _step_pin.name().startsWith("I2SO"), "Step pin must be an I2SO pin"); + Assert(!isI2SO || _dir_pin.name().startsWith("I2SO"), "Direction pin must be an I2SO pin"); + } + } diff --git a/Grbl_Esp32/src/Motors/StandardStepper.h b/Grbl_Esp32/src/Motors/StandardStepper.h index f1a391e6..0fe1e792 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.h +++ b/Grbl_Esp32/src/Motors/StandardStepper.h @@ -1,7 +1,5 @@ #pragma once -#include "../Machine/MachineConfig.h" // config->_stepType -#include "../Stepper.h" // ST_I2S_* #include "Motor.h" #include @@ -34,13 +32,7 @@ namespace Motors { Pin _disable_pin; // Configuration handlers: - void validate() const override { - Assert(_step_pin.defined(), "Step pin should be configured."); - Assert(_dir_pin.defined(), "Direction pin should be configured."); - bool isI2SO = config->_stepType == ST_I2S_STREAM || config->_stepType == ST_I2S_STATIC; - Assert(!isI2SO || _step_pin.name().startsWith("I2SO"), "Step pin must be an I2SO pin"); - Assert(!isI2SO || _dir_pin.name().startsWith("I2SO"), "Direction pin must be an I2SO pin"); - } + void validate() const override; void group(Configuration::HandlerBase& handler) override { handler.item("step", _step_pin); diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index dfecf580..5708c7a1 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -272,11 +272,11 @@ Error home(int cycle) { } sys.state = State::Homing; // Set system state variable - config->_axes->beginLowLatency(); + config->_stepping->beginLowLatency(); mc_homing_cycle(cycle); - config->_axes->endLowLatency(); + config->_stepping->endLowLatency(); if (!sys.abort) { // Execute startup scripts after successful homing. sys.state = State::Idle; // Set to IDLE when complete. diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 2de9d4d4..5ca3ee50 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -26,12 +26,10 @@ #include "Stepper.h" #include "Machine/MachineConfig.h" +#include "Stepping.h" #include "StepperPrivate.h" #include "Planner.h" -#include "I2SOut.h" // i2s_out_push_sample -#include "Report.h" // info_serial - -#include +#include "Report.h" // info_serial #include // IRAM_ATTR using namespace Stepper; @@ -88,9 +86,6 @@ static volatile uint8_t segment_buffer_tail; static uint8_t segment_buffer_head; static uint8_t segment_next_head; -// Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static std::atomic busy; - // Pointers for the step segment being prepped from the planner buffer. Accessed only by the // main program. Pointers may be planning segments or planner blocks ahead of what being executed. static plan_block_t* pl_block; // Pointer to the planner block being prepped @@ -108,7 +103,7 @@ bool Stepper::shouldDisable() { // space. Using "timer() > EndTime" fails across the positive to // negative transition using signed comparison, and across the // negative to positive transition using unsigned. - return isIdle && config->_idleTime != 255 && (esp_timer_get_time() - idleEndTime) > 0; + return isIdle && config->_stepping->_idleMsecs != 255 && (esp_timer_get_time() - idleEndTime) > 0; } // Segment preparation data struct. Contains all the necessary information to compute new segments @@ -142,10 +137,6 @@ typedef struct { } st_prep_t; static st_prep_t prep; -EnumItem stepTypes[] = { - { ST_TIMED, "Timed" }, { ST_RMT, "RMT" }, { ST_I2S_STATIC, "I2S_static" }, { ST_I2S_STREAM, "I2S_stream" }, EnumItem(ST_RMT) -}; - /* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. Grbl employs the venerable Bresenham line algorithm to manage and exactly synchronize multi-axis moves. Unlike the popular DDA algorithm, the Bresenham algorithm is not susceptible to numerical @@ -202,58 +193,6 @@ EnumItem stepTypes[] = { */ -// Forward references to functions that are only used herein -static void timerWritePeriod(uint16_t timerTicks); -static void timerInit(); -static void timerStart(); -static void timerStop(); - -// Stepper timer configuration -const int stepTimerNumber = 0; -hw_timer_t* stepTimer = nullptr; // Handle -// autoReload true might give better step timing - but it also -// might cause problems if an interrupt takes too long -const bool autoReload = true; - -static void pulse_func(); - -// Counts stepper ISR invocations. This variable can be inspected -// from the mainline code to determine if the stepper ISR is running, -// since printing from the ISR is not a good idea. -uint32_t Stepper::isr_count = 0; - -// TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller -// int8 variables and update position counters only when a segment completes. This can get complicated -// with probing and homing cycles that require true real-time positions. -void IRAM_ATTR onStepperDriverTimer() { - // Timer ISR, normally takes a step. - - // The intermediate handler clears the timer interrupt so we need not do it here - - bool expected = false; - if (busy.compare_exchange_strong(expected, true)) { - ++isr_count; - - // Using autoReload results is less timing jitter so it is - // probably best to have it on. We keep the variable for - // convenience in debugging. - if (!autoReload) { - timerWrite(stepTimer, 0ULL); - } - - // It is tempting to defer this until after pulse_func(), - // but if pulse_func() determines that no more stepping - // is required and disables the timer, then that will be undone - // if the re-enable happens afterwards. - - timerAlarmEnable(stepTimer); - - pulse_func(); - - busy.store(false); - } -} - /** * This phase of the ISR should ONLY create the pulses for the steppers. * This prevents jitter caused by the interval between the start of the @@ -261,15 +200,9 @@ void IRAM_ATTR onStepperDriverTimer() { * call to this method that might cause variation in the timing. The aim * is to keep pulse timing as regular as possible. */ -static void IRAM_ATTR pulse_func() { +void IRAM_ATTR Stepper::pulse_func() { auto n_axis = config->_axes->_numberAxis; - // If we are using GPIO stepping as opposed to RMT, record the - // time that we turned on the step pins so we can turn them off - // at the end of this routine without incurring another interrupt. - // This is unnecessary with RMT and I2S stepping since both of - // those methods time the turn off automatically. - uint64_t step_pulse_start_time = esp_timer_get_time(); config->_axes->step(st.step_outbits, st.dir_outbits); // If there is no step segment, attempt to pop one from the stepper buffer @@ -279,7 +212,7 @@ static void IRAM_ATTR pulse_func() { // Initialize new step segment and load number of steps to execute st.exec_segment = &segment_buffer[segment_buffer_tail]; // Initialize step segment timing per step and load number of steps to execute. - timerWritePeriod(st.exec_segment->isrPeriod); + config->_stepping->setTimerPeriod(st.exec_segment->isrPeriod); st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. // If the new segment starts a new planner block, initialize stepper variables and counters. // NOTE: When the segment data index changes, this indicates a new planner block. @@ -348,41 +281,7 @@ static void IRAM_ATTR pulse_func() { } } - // stepping->unStep(pulseMicros); - uint64_t endMicros; - switch (config->_stepType) { - case ST_I2S_STREAM: - // Generate the number of pulses needed to span pulse_microseconds - i2s_out_push_sample(config->_pulseMicroSeconds); - config->_axes->unstep(); - break; - case ST_I2S_STATIC: - case ST_TIMED: - endMicros = config->_pulseMicroSeconds + step_pulse_start_time; - // wait for step pulse time to complete...some time expired during code above - while ((esp_timer_get_time() - endMicros) < 0) { - NOP(); // spin here until time to turn off step - } - config->_axes->unstep(); - break; - case ST_RMT: - break; - } -} - -void Stepper::init() { - busy.store(false); - - info_serial("Axis count %d", config->_axes->_numberAxis); - info_serial("Step type:%s Pulse:%dus Dsbl Delay:%dus Dir Delay:%dus", - // stepping->name(), - stepTypes[config->_stepType].name, - config->_pulseMicroSeconds, - config->_disableDelayMicroSeconds, - config->_directionDelayMicroSeconds); - - // Other stepper use timer interrupt - timerInit(); + config->_axes->unstep(); } // enabled. Startup init and limits call this function but shouldn't start the cycle. @@ -392,17 +291,14 @@ void Stepper::wake_up() { config->_axes->set_disable(false); isIdle = false; - // Enable Stepper Driver Interrupt - timerStart(); + // Enable Stepping Driver Interrupt + config->_stepping->startTimer(); } // Reset and clear stepper subsystem variables void Stepper::reset() { - // Initialize stepper driver idle state. - // stepping->reset(); - if (config->_stepType == ST_I2S_STREAM) { - i2s_out_reset(); - } + // Initialize Stepping driver idle state. + config->_stepping->reset(); go_idle(); // Initialize stepper algorithm variables. @@ -420,11 +316,12 @@ void Stepper::reset() { // Stepper shutdown void IRAM_ATTR Stepper::go_idle() { - // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active. - timerStop(); + // Disable Stepping Driver Interrupt. + config->_stepping->stopTimer(); // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances. - if (((config->_idleTime != 255) || sys_rt_exec_alarm != ExecAlarm::None || sys.state == State::Sleep) && sys.state != State::Homing) { + if (((config->_stepping->_idleMsecs != 255) || sys_rt_exec_alarm != ExecAlarm::None || sys.state == State::Sleep) && + sys.state != State::Homing) { // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete // stop and not drift from residual inertial forces at the end of the last movement. @@ -433,7 +330,7 @@ void IRAM_ATTR Stepper::go_idle() { } else { // Setup for shouldDisable() isIdle = true; - idleEndTime = esp_timer_get_time() + (config->_idleTime * 1000); // * 1000 because the time is in uSecs + idleEndTime = esp_timer_get_time() + (config->_stepping->_idleMsecs * 1000); // * 1000 because the time is in uSecs } } else { config->_axes->set_disable(false); @@ -846,7 +743,7 @@ void Stepper::prep_buffer() { // Compute CPU cycles per step for the prepped segment. // fStepperTimer is in units of timerTicks/sec, so the dimensional analysis is // timerTicks/sec * 60 sec/minute * minutes = timerTicks - uint32_t timerTicks = uint32_t(ceil((fStepperTimer * 60) * inv_rate)); // (timerTicks/step) + uint32_t timerTicks = uint32_t(ceil((Machine::Stepping::fStepperTimer * 60) * inv_rate)); // (timerTicks/step) int level; // Compute step timing and multi-axis smoothing level. @@ -913,49 +810,6 @@ float Stepper::get_realtime_rate() { } } -// The argument is in units of ticks of the timer that generates ISRs -static void IRAM_ATTR timerWritePeriod(uint16_t timerTicks) { - // stepping->setPeriod(uint16_t timerTicks); - if (config->_stepType == ST_I2S_STREAM) { - // 1 tick = fTimers / fStepperTimer - // Pulse ISR is called for each tick of alarm_val. - // The argument to i2s_out_set_pulse_period is in units of microseconds - i2s_out_set_pulse_period(((uint32_t)timerTicks) / ticksPerMicrosecond); - } else { - timerAlarmWrite(stepTimer, (uint64_t)timerTicks, autoReload); - } -} - -static void IRAM_ATTR timerInit() { - const bool isEdge = false; - const bool countUp = true; - - // Prepare stepping interrupt callbacks. The one that is actually - // used is determined by timerStart() and timerStop() - - // Register pulse_func with the I2S subsystem - i2s_out_set_pulse_callback(pulse_func); - - // Setup a timer for direct stepping - stepTimer = timerBegin(stepTimerNumber, fTimers / fStepperTimer, countUp); - timerAttachInterrupt(stepTimer, onStepperDriverTimer, isEdge); -} - -static void IRAM_ATTR timerStart() { - // stepping->start(); - if (config->_stepType == ST_I2S_STREAM) { - i2s_out_set_stepping(); - } else { - timerWrite(stepTimer, 0ULL); - timerAlarmEnable(stepTimer); - } -} - -static void IRAM_ATTR timerStop() { - // stepping->stop(); - if (config->_stepType == ST_I2S_STREAM) { - i2s_out_set_passthrough(); - } else if (stepTimer) { - timerAlarmDisable(stepTimer); - } +void Stepper::init() { + config->_stepping->init(); } diff --git a/Grbl_Esp32/src/Stepper.h b/Grbl_Esp32/src/Stepper.h index 551726be..408072a4 100644 --- a/Grbl_Esp32/src/Stepper.h +++ b/Grbl_Esp32/src/Stepper.h @@ -28,19 +28,13 @@ #include -enum stepper_id_t { - ST_TIMED = 0, - ST_RMT, - ST_I2S_STATIC, - ST_I2S_STREAM, -}; - namespace Stepper { // Is it time to disable the steppers? bool shouldDisable(); void init(); - void switch_mode(stepper_id_t new_stepper); + + void pulse_func(); // Enable steppers, but cycle does not start unless called by motion control or realtime command. void wake_up(); @@ -69,5 +63,3 @@ namespace Stepper { extern uint32_t isr_count; // for debugging only } // private - -extern EnumItem stepTypes[]; diff --git a/Grbl_Esp32/src/StepperPrivate.h b/Grbl_Esp32/src/StepperPrivate.h index 5e46b705..33964cac 100644 --- a/Grbl_Esp32/src/StepperPrivate.h +++ b/Grbl_Esp32/src/StepperPrivate.h @@ -23,11 +23,6 @@ struct PrepFlag { uint8_t decelOverride : 1; }; -// fStepperTimer should be an integer divisor of the bus speed, i.e. of fTimers -const uint32_t fTimers = 80000000; // the frequency of ESP32 timers -const uint32_t fStepperTimer = 20000000; // frequency of step pulse timer -const int ticksPerMicrosecond = fStepperTimer / 1000000; - // Define Adaptive Multi-Axis Step-Smoothing(AMASS) levels and cutoff frequencies. The highest level // frequency bin starts at 0Hz and ends at its cutoff frequency. The next lower level frequency bin // starts at the next higher cutoff frequency, and so on. The cutoff frequencies for each level must @@ -39,5 +34,5 @@ const int ticksPerMicrosecond = fStepperTimer / 1000000; // NOTE: Current settings are set to overdrive the ISR to no more than 16kHz, balancing CPU overhead // and timer accuracy. Do not alter these settings unless you know what you are doing. -const uint32_t amassThreshold = fStepperTimer / 8000; +const uint32_t amassThreshold = Machine::Stepping::fStepperTimer / 8000; const int maxAmassLevel = 3; // Each level increase doubles the threshold diff --git a/Grbl_Esp32/src/Stepping.cpp b/Grbl_Esp32/src/Stepping.cpp new file mode 100644 index 00000000..bec8f2c2 --- /dev/null +++ b/Grbl_Esp32/src/Stepping.cpp @@ -0,0 +1,213 @@ +#include +#include "I2SOut.h" +#include "EnumItem.h" +#include "Stepping.h" +#include "Stepper.h" +#include "Report.h" + +#include + +namespace Machine { + + EnumItem stepTypes[] = { { Stepping::TIMED, "Timed" }, + { Stepping::RMT, "RMT" }, + { Stepping::I2S_STATIC, "I2S_static" }, + { Stepping::I2S_STREAM, "I2S_stream" }, + EnumItem(Stepping::RMT) }; + + static std::atomic busy; + + void Stepping::init() { + info_serial("Step type:%s Pulse:%dus Dsbl Delay:%dus Dir Delay:%dus Idle Delay:%dms", + // stepping->name(), + stepTypes[_engine].name, + _pulseUsecs, + _disableDelayUsecs, + _directionDelayUsecs, + _idleMsecs); + + // Prepare stepping interrupt callbacks. The one that is actually + // used is determined by timerStart() and timerStop() + + const bool isEdge = false; + const bool countUp = true; + + // Setup a timer for direct stepping + stepTimer = timerBegin(stepTimerNumber, fTimers / fStepperTimer, countUp); + timerAttachInterrupt(stepTimer, onStepperDriverTimer, isEdge); + + // Register pulse_func with the I2S subsystem + // This could be done via the linker. + // i2s_out_set_pulse_callback(Stepper::pulse_func); + + busy.store(false); + } + + // Wait for motion to complete; the axes can still be moving + // after the data has been sent to the stepping engine, due + // to queuing delays. + void Stepping::synchronize() { + if (_engine == I2S_STREAM) { + i2s_out_delay(); + // XXX instead of a delay, we could sense when the DMA and + // FIFO have drained. It might be as simple as waiting for + // I2SO.conf1.tx_start == 0, while yielding. + // delay_ms(I2S_OUT_DELAY_MS); + } + } + void Stepping::reset() { + if (_engine == I2S_STREAM) { + i2s_out_reset(); + } + } + void Stepping::beginLowLatency() { + _switchedStepper = _engine == I2S_STREAM; + if (_switchedStepper) { + _engine = I2S_STATIC; + } + } + void Stepping::endLowLatency() { + if (_switchedStepper) { + if (i2s_out_get_pulser_status() != PASSTHROUGH) { + // Called during streaming. Stop streaming. + // debug_serial("Stop the I2S streaming and switch to the passthrough mode."); + i2s_out_set_passthrough(); + i2s_out_delay(); // Wait for a change in mode. + } + _engine = I2S_STREAM; + } + } + void Stepping::spinDelay(int64_t start_time, uint32_t durationUs) { + int64_t endTime = start_time + durationUs; + while ((esp_timer_get_time() - endTime) < 0) { + NOP(); + } + } + void Stepping::waitPulse() { + uint64_t pulseEndTime; + switch (_engine) { + case I2S_STREAM: + // Generate the number of pulses needed to span pulse_microseconds + i2s_out_push_sample(_pulseUsecs); + break; + case I2S_STATIC: + case TIMED: + spinDelay(_stepPulseStartTime, _pulseUsecs); + break; + case RMT: + // RMT generates the trailing edges in hardware + return; + } + } + void Stepping::waitDirection() { + if (_directionDelayUsecs) { + // Stepper drivers need some time between changing direction and doing a pulse. + switch (_engine) { + case stepper_id_t::I2S_STREAM: + i2s_out_push_sample(_directionDelayUsecs); + break; + case stepper_id_t::I2S_STATIC: + case stepper_id_t::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. + spinDelay(esp_timer_get_time(), _directionDelayUsecs); + break; + case stepper_id_t::RMT: + break; + } + } + } + } + + void Stepping::startPulseTimer() { + switch (_engine) { + case stepper_id_t::I2S_STREAM: + break; + case stepper_id_t::I2S_STATIC: + case stepper_id_t::TIMED: + _stepPulseStartTime = esp_timer_get_time(); + break; + case stepper_id_t::RMT: + break; + } + } + + // The argument is in units of ticks of the timer that generates ISRs + void IRAM_ATTR Stepping::setTimerPeriod(uint16_t timerTicks) { + if (_engine == I2S_STREAM) { + // 1 tick = fTimers / fStepperTimer + // Pulse ISR is called for each tick of alarm_val. + // The argument to i2s_out_set_pulse_period is in units of microseconds + i2s_out_set_pulse_period(((uint32_t)timerTicks) / ticksPerMicrosecond); + } else { + timerAlarmWrite(stepTimer, (uint64_t)timerTicks, autoReload); + } + } + void Stepping::startTimer() { + if (_engine == I2S_STREAM) { + i2s_out_set_stepping(); + } else { + timerWrite(stepTimer, 0ULL); + timerAlarmEnable(stepTimer); + } + } + void Stepping::stopTimer() { + if (_engine == I2S_STREAM) { + i2s_out_set_passthrough(); + } else if (stepTimer) { + timerAlarmDisable(stepTimer); + } + } + + // Stepper timer configuration + hw_timer_t* Stepping::stepTimer = nullptr; // Handle + + // Counts stepper ISR invocations. This variable can be inspected + // from the mainline code to determine if the stepper ISR is running, + // since printing from the ISR is not a good idea. + uint32_t Stepping::isr_count = 0; + + // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. + // TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller + // int8 variables and update position counters only when a segment completes. This can get complicated + // with probing and homing cycles that require true real-time positions. + void IRAM_ATTR Stepping::onStepperDriverTimer() { + // Timer ISR, normally takes a step. + + // The intermediate handler clears the timer interrupt so we need not do it here + + bool expected = false; + if (busy.compare_exchange_strong(expected, true)) { + ++isr_count; + + // Using autoReload results is less timing jitter so it is + // probably best to have it on. We keep the variable for + // convenience in debugging. + if (!autoReload) { + timerWrite(stepTimer, 0ULL); + } + + // It is tempting to defer this until after pulse_func(), + // but if pulse_func() determines that no more stepping + // is required and disables the timer, then that will be undone + // if the re-enable happens afterwards. + + timerAlarmEnable(stepTimer); + + Stepper::pulse_func(); + + busy.store(false); + } + } + + void Stepping::group(Configuration::HandlerBase& handler) { + handler.item("engine", _engine, stepTypes); + handler.item("idle_ms", _idleMsecs); + handler.item("pulse_us", _pulseUsecs); + handler.item("dir_delay_us", _directionDelayUsecs); + handler.item("disable_delay_us", _disableDelayUsecs); + } +} diff --git a/Grbl_Esp32/src/Stepping.h b/Grbl_Esp32/src/Stepping.h new file mode 100644 index 00000000..ddaed45d --- /dev/null +++ b/Grbl_Esp32/src/Stepping.h @@ -0,0 +1,87 @@ +#pragma once + +/* + Part of Grbl_ESP32 + 2021 - Stefan de Bruijn, Mitch Bradley + + Grbl_ESP32 is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl_ESP32 is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl_ESP32. If not, see . +*/ + +#include "Configuration/Configurable.h" +// #include + +namespace Machine { + class Stepping : public Configuration::Configurable { + public: + // fStepperTimer should be an integer divisor of the bus speed, i.e. of fTimers + static const uint32_t fStepperTimer = 20000000; // frequency of step pulse timer + + private: + static const int stepTimerNumber = 0; + static const bool autoReload = true; + static hw_timer_t* stepTimer; + static void onStepperDriverTimer(); + + static const uint32_t fTimers = 80000000; // the frequency of ESP32 timers + static const int ticksPerMicrosecond = fStepperTimer / 1000000; + + bool _switchedStepper = false; + int64_t _stepPulseStartTime; + + static void spinDelay(int64_t start_time, uint32_t duration); + + public: + // Counts stepper ISR invocations. This variable can be inspected + // from the mainline code to determine if the stepper ISR is running, + // since printing from the ISR is not a good idea. + static uint32_t isr_count; + + enum stepper_id_t { + TIMED = 0, + RMT, + I2S_STATIC, + I2S_STREAM, + }; + + Stepping() = default; + + uint8_t _idleMsecs = 255; + uint32_t _pulseUsecs = 3; + uint32_t _directionDelayUsecs = 0; + uint32_t _disableDelayUsecs = 0; + + int _engine = RMT; + + // Interfaces to stepping engine + void init(); + + void synchronize(); // Wait for motion to complete + void reset(); // Clean up old state and start fresh + void beginLowLatency(); + void endLowLatency(); + void startPulseTimer(); + void waitPulse(); // Wait for pulse length + void waitDirection(); // Wait for direction delay + void waitMotion(); // Wait for motion to complete + + // Timers + void setTimerPeriod(uint16_t timerTicks); + void startTimer(); + void stopTimer(); + + // Configuration system helpers: + void group(Configuration::HandlerBase& handler) override; + }; +} +extern EnumItem stepTypes[]; diff --git a/Grbl_Esp32/test/Configuration/YamlComplete.cpp b/Grbl_Esp32/test/Configuration/YamlComplete.cpp index 58ee8569..798528a4 100644 --- a/Grbl_Esp32/test/Configuration/YamlComplete.cpp +++ b/Grbl_Esp32/test/Configuration/YamlComplete.cpp @@ -10,7 +10,7 @@ namespace Configuration { "yaml_wiki: \"https://github.com/bdring/Grbl_Esp32/wiki/YAML-Config-File\"\n" "\n" "idle_time: 250\n" - "step_type: rmt\n" + "engine: rmt\n" "dir_delay_microseconds: 1\n" "pulse_microseconds: 2\n" "disable_delay_us: 0\n" @@ -63,7 +63,7 @@ namespace Configuration { p.Tokenize(); Assert(!p.Eof(), "No EOF expected"); - Assert(p.key().equals("step_type"), "Expected 'step_type'"); + Assert(p.key().equals("engine"), "Expected 'engine'"); p.Tokenize(); Assert(!p.Eof(), "No EOF expected");