From 192e9a3bc3cc27870361098cdb23332d93c31cf7 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Fri, 1 Sep 2023 17:16:38 +0200 Subject: [PATCH 01/85] Issue #199: add initialization of _enablePinHighActive and _enablePinLowActive --- src/FastAccelStepper.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index a36520b8..93dc5c5a 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -529,6 +529,8 @@ void FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num, _stepPin = step_pin; _dirHighCountsUp = true; _dirPin = PIN_UNDEFINED; + _enablePinHighActive = PIN_UNDEFINED; + _enablePinLowActive = PIN_UNDEFINED; _rg.init(); _queue_num = num; From ea40b6b1efb07972c98595743edb9266630b4b65 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Mon, 4 Sep 2023 20:15:54 +0200 Subject: [PATCH 02/85] bump version --- CHANGELOG.md | 3 ++- .../test_sd_04_timing_2560/expect.txt | 20 +++++++++---------- .../test_sd_04_timing_328p/expect.txt | 14 ++++++------- .../test_sd_04_timing_328p_37k/expect.txt | 8 ++++---- .../simavr_based/test_sd_11_328p/expect.txt | 8 ++++---- library.properties | 2 +- 6 files changed, 28 insertions(+), 27 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 32b544da..5584f6a0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,8 +5,9 @@ TODO: - rename PoorManFloat to e.g. Log2Representation - rename RampConstAcceleration to e.g. RampControl -pre-0.30.5: +0.30.5: - Fix target position for a move() interrupting the keep running mode +- Fix issue #199: add initialization of `_enablePinHighActive` and `_enablePinLowActive` 0.30.4: - Fix for issue #178: speed does not decelerate but jumps to lower value diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 03a797e5..1d84d735 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,26 +4,26 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=521431us - StepB: 64000*L->H, 64000*H->L, Max High=33us Total High=452082us - StepC: 64000*L->H, 64000*H->L, Max High=37us Total High=486045us + StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=576437us + StepB: 64000*L->H, 64000*H->L, Max High=30us Total High=393762us + StepC: 64000*L->H, 64000*H->L, Max High=37us Total High=429542us Position[A]=64000 Position[B]=64000 Position[C]=64000 -Time in EnableA max=233599 us, total=233599 us +Time in EnableA max=233598 us, total=233598 us -Time in EnableB max=246083 us, total=246083 us +Time in EnableB max=246081 us, total=246081 us -Time in EnableC max=254232 us, total=254232 us +Time in EnableC max=254229 us, total=254229 us -Time in FillISR max=2754 us, total=2090247 us +Time in FillISR max=2697 us, total=2091083 us -Time in StepA max=28 us, total=521431 us +Time in StepA max=28 us, total=576437 us -Time in StepB max=33 us, total=452082 us +Time in StepB max=30 us, total=393762 us -Time in StepC max=37 us, total=486045 us +Time in StepC max=37 us, total=429542 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 34bca0eb..2aa1c005 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,21 +2,21 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5599us - StepB: 1000*L->H, 1000*H->L, Max High=19us Total High=5436us + StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5618us + StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5419us Position[A]=1000 Position[B]=1000 -Time in EnableA max=225399 us, total=225399 us +Time in EnableA max=225398 us, total=225398 us -Time in EnableB max=238111 us, total=238111 us +Time in EnableB max=238110 us, total=238110 us -Time in FillISR max=2638 us, total=47571 us +Time in FillISR max=2638 us, total=47569 us -Time in StepA max=14 us, total=5599 us +Time in StepA max=14 us, total=5618 us -Time in StepB max=19 us, total=5436 us +Time in StepB max=16 us, total=5419 us Time in StepISR max=7 us, total=9224 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index eb1b86d1..ac107712 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5285us + StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5286us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 -Time in EnableA max=225400 us, total=225400 us +Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2015 us, total=27510 us +Time in FillISR max=2015 us, total=27518 us -Time in StepA max=11 us, total=5285 us +Time in StepA max=11 us, total=5286 us Time in StepISR max=7 us, total=4581 us diff --git a/extras/tests/simavr_based/test_sd_11_328p/expect.txt b/extras/tests/simavr_based/test_sd_11_328p/expect.txt index 95fbbeaa..fdf0457e 100644 --- a/extras/tests/simavr_based/test_sd_11_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_11_328p/expect.txt @@ -2,11 +2,11 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 99708*L->H, 99708*H->L, Max High=13us Total High=514351us + StepA: 99808*L->H, 99808*H->L, Max High=13us Total High=514674us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us -Position[A]=50436 +Position[A]=50536 -Time in EnableA max=225396 us, total=225396 us +Time in EnableA max=225395 us, total=225395 us -Time in StepA max=13 us, total=514351 us +Time in StepA max=13 us, total=514674 us diff --git a/library.properties b/library.properties index 11dc91bb..617a1ae9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.4 +version=0.30.5 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes From 1935ae1794599d1e6347450e55c62648e4b0cebc Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Thu, 12 Oct 2023 22:50:04 +0200 Subject: [PATCH 03/85] intermediate (broken) version with conf_update --- extras/ci/platformio.ini | 5 +- src/StepperISR_esp32_rmt.cpp | 2 +- src/StepperISR_esp32c3_rmt.cpp | 588 +++++++++++++++++++++++++++++++++ src/common.h | 7 +- src/test_probe.h | 7 + 5 files changed, 604 insertions(+), 5 deletions(-) create mode 100644 src/StepperISR_esp32c3_rmt.cpp diff --git a/extras/ci/platformio.ini b/extras/ci/platformio.ini index e4131fbe..f5aeeb5f 100644 --- a/extras/ci/platformio.ini +++ b/extras/ci/platformio.ini @@ -65,10 +65,13 @@ lib_extra_dirs = ../../.. [env:esp32c3] board = esp32-c3-devkitm-1 framework = arduino -platform = https://github.com/tasmota/platform-espressif32/releases/download/v.2.0.3/platform-espressif32-v.2.0.3.zip +platform = espressif32 +#platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.10.03/platform-espressif32.zip build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. +board_build.flash_mode = dio +upload_port = /dev/ttyACM0 [env:nanoatmega168] platform = atmelavr diff --git a/src/StepperISR_esp32_rmt.cpp b/src/StepperISR_esp32_rmt.cpp index 113c7704..5ee62b9b 100644 --- a/src/StepperISR_esp32_rmt.cpp +++ b/src/StepperISR_esp32_rmt.cpp @@ -1,5 +1,5 @@ #include "StepperISR.h" -#ifdef SUPPORT_ESP32_RMT +#if defined(SUPPORT_ESP32_RMT) && !defined(SUPPORT_ESP32C3_RMT) //#define TEST_MODE diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp new file mode 100644 index 00000000..0d886474 --- /dev/null +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -0,0 +1,588 @@ +#include "StepperISR.h" +#ifdef SUPPORT_ESP32C3_RMT + +#define TEST_MODE + +#include "test_probe.h" + +// The following concept is in use: +// +// The buffer of 64Bytes is split into two parts à 31 words. +// Each part will hold one command (or part of). +// After the 2*31 words an end marker is placed. +// The threshold is set to 31. +// +// This way, the threshold interrupt occurs after the first part +// and the end interrupt after the second part. +// +// Of these 32 bits, the low 16-bit entry is sent first and the high entry +// second. +// Every 16 bit entry defines with MSB the output level and the lower 15 bits +// the ticks. +#ifdef SUPPORT_ESP32C3_RMT +#define PART_SIZE 23 +#define MEM_SIZE 48 +#else +#define PART_SIZE 31 +#define MEM_SIZE 64 +#endif + +void IRAM_ATTR StepperQueue::stop_rmt(bool both) { + // We are stopping the rmt by letting it run into the end at high speed. + // + // disable the interrupts + // rmt_set_tx_intr_en(channel, false); + // rmt_set_tx_thr_intr_en(channel, false, 0); + + // stop esp32 rmt, by let it hit the end + RMT.tx_conf[channel].tx_conti_mode = 0; + + // replace second part of buffer with pauses + uint32_t *data = FAS_RMT_MEM(channel); + uint8_t start = both ? 0 : PART_SIZE; + data = &data[start]; + for (uint8_t i = start; i < 2 * PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); + } + *data = 0; + + // as the rmt is not running anymore, mark it as stopped + _rmtStopped = true; +} + +static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, + uint32_t *data) { + if (!fill_part_one) { + data += PART_SIZE; + } + uint8_t rp = q->read_idx; + if (rp == q->next_write_idx) { + // no command in queue + if (fill_part_one) { + q->bufferContainsSteps[0] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // make a pause with approx. 1ms + // 258 ticks * 2 * 31 = 15996 @ 16MHz + *data++ = 0x01020102; + } + } else { + q->stop_rmt(false); + } + return; + } + // Process command + struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; + + if (e_curr->toggle_dir) { + // the command requests dir pin toggle + // This is ok only, if the ongoing command does not contain steps + if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { + // So we need a pause. change the finished read entry into a pause + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2*PART_SIZE)); + } + return; + } + // The ongoing command does not contain steps, so change dir here should be + // ok + gpio_num_t dirPin = (gpio_num_t)q->dirPin; + gpio_set_level(dirPin, gpio_get_level(dirPin) ^ 1); + // and delete the request + e_curr->toggle_dir = 0; + } + + uint8_t steps = e_curr->steps; + uint16_t ticks = e_curr->ticks; + // if (steps != 0) { + // PROBE_2_TOGGLE; + //} + uint32_t last_entry; + if (steps == 0) { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + // two pauses à 3 ticks. the 2 for debugging + *data++ = 0x00010002; + ticks -= 3; + } + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + last_entry = ticks_l; + last_entry <<= 16; + last_entry |= ticks_r; + } else { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; + if (ticks == 0xffff) { + // special treatment for this case, because an rmt entry can only cover up + // to 0xfffe ticks every step must be minimum split into two rmt entries, + // so at max PART/2 steps can be done. + if (steps < PART_SIZE / 2) { + for (uint8_t i = 1; i < steps; i++) { + // steps-1 iterations + *data++ = 0x40017fff | 0x8000; + *data++ = 0x20002000; + } + *data++ = 0x40017fff | 0x8000; + uint16_t delta = PART_SIZE - 2 * steps; + delta <<= 5; + *data++ = 0x20002000 - delta; + // 2*(steps - 1) + 1 already stored => 2*steps - 1 + // and after this for loop one entry added => 2*steps + for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { + *data++ = 0x00100010; + } + last_entry = 0x00100010; + steps = 0; + } else { + steps -= PART_SIZE / 2; + for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { + *data++ = 0x40017fff | 0x8000; + *data++ = 0x20002000; + } + *data++ = 0x40017fff | 0x8000; + last_entry = 0x20002000; + } + } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { + uint8_t steps_to_do = steps; + if (steps > PART_SIZE) { + steps_to_do /= 2; + } + + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 1; i < steps_to_do; i++) { + *data++ = rmt_entry; + } + uint32_t delta = PART_SIZE - steps_to_do; + delta <<= 18; // shift in upper 16bit and multiply with 4 + *data++ = rmt_entry - delta; + for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { + *data++ = 0x00020002; + } + last_entry = 0x00020002; + steps -= steps_to_do; + } else { + // either >= 2*PART_SIZE or = PART_SIZE + // every entry one step + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + *data++ = rmt_entry; + } + last_entry = rmt_entry; + steps -= PART_SIZE; + } + } + if (!fill_part_one) { + // Note: When enabling the continuous transmission mode by setting + // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the + // channel continuously, that is, from the first byte to the last one, + // then from the first to the last again, and so on. In this mode, there + // will be an idle level lasting one clk_div cycle between N and N+1 + // transmissions. + last_entry -= 1; + } + *data = last_entry; + + // Data is complete + if (steps == 0) { + // The command has been completed + if (e_curr->repeat_entry == 0) { + q->read_idx = rp + 1; + } + } else { + e_curr->steps = steps; + } +} + +#if !defined(RMT_CHANNEL_MEM) && !defined(SUPPORT_ESP32C3_RMT) +#define RMT_LIMIT tx_lim +#define RMT_FIFO apb_fifo_mask +#else +#define RMT_LIMIT limit +#define RMT_FIFO fifo_mask +#endif + +#ifdef TEST_MODE +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_2_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + PROBE_3_TOGGLE; \ + /* now repeat the interrupt at buffer size + end marker */ \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ + } +#else +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_2_TOGGLE; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + if (q->_rmtStopped) { \ + rmt_set_tx_intr_en(q->channel, false); \ + rmt_set_tx_thr_intr_en(q->channel, false, PART_SIZE + 1); \ + q->_isRunning = false; \ + PROBE_1_TOGGLE; \ + } else { \ + apply_command(q, false, FAS_RMT_MEM(ch)); \ + } \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + PROBE_3_TOGGLE; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + if (!q->_rmtStopped) { \ + apply_command(q, true, FAS_RMT_MEM(ch)); \ + /* now repeat the interrupt at buffer size + end marker */ \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ + } \ + } +#endif + +static void IRAM_ATTR tx_intr_handler(void *arg) { + PROBE_1_TOGGLE; + uint32_t mask = RMT.int_st.val; + RMT.int_clr.val = mask; + PROCESS_CHANNEL(0); + PROCESS_CHANNEL(1); +#if QUEUES_RMT >= 4 + PROCESS_CHANNEL(2); + PROCESS_CHANNEL(3); +#endif +#if QUEUES_RMT == 8 + PROCESS_CHANNEL(4); + PROCESS_CHANNEL(5); + PROCESS_CHANNEL(6); + PROCESS_CHANNEL(7); +#endif +} + +void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { +#ifdef TEST_PROBE_1 + if (channel_num == 0) { + pinMode(TEST_PROBE_1, OUTPUT); + PROBE_1(HIGH); + delay(10); + PROBE_1(LOW); + delay(5); + PROBE_1(HIGH); + delay(5); + PROBE_1(LOW); + delay(5); + } +#endif +#ifdef TEST_PROBE_2 + pinMode(TEST_PROBE_2, OUTPUT); + PROBE_2(LOW); +#endif +#ifdef TEST_PROBE_3 + pinMode(TEST_PROBE_3, OUTPUT); + PROBE_3(LOW); +#endif + + _initVars(); + _step_pin = step_pin; +// digitalWrite(step_pin, LOW); +// pinMode(step_pin, OUTPUT); + + channel = (rmt_channel_t)channel_num; + + if (channel_num == 0) { + periph_module_enable(PERIPH_RMT_MODULE); + } + // 80 MHz/5 = 16 MHz + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); + rmt_set_source_clk(channel, RMT_BASECLK_APB); + rmt_set_clk_div(channel, 5); + rmt_set_mem_block_num(channel, 1); + rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); + rmt_tx_stop(channel); + rmt_memory_rw_rst(channel); + //rmt_rx_stop(channel); TX only channel ! + // rmt_tx_memory_reset is not defined in arduino V340 and based on test result + // not needed rmt_tx_memory_reset(channel); + if (channel_num == 0) { + rmt_isr_register(tx_intr_handler, NULL, + ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); + RMT.sys_conf.fifo_mask = 1; // disable fifo mode + } + + _isRunning = false; + _rmtStopped = true; + + connect_rmt(); + +#ifdef TEST_MODE + if (channel == 0) { + RMT.tx_conf[channel].mem_rd_rst = 1; + uint32_t *mem = FAS_RMT_MEM(channel); + // Fill the buffer with a significant pattern for debugging + for (uint8_t i = 0; i < MEM_SIZE/2; i += 2) { + *mem++ = 0x7fffffff; + *mem++ = 0x7fff7fff; + } + for (uint8_t i = MEM_SIZE/2; i < MEM_SIZE-2; i += 2) { + *mem++ = 0x3fffdfff; + *mem++ = 0x3fff3fff; + } + // without end marker it does not loop + *mem++ = 0; + *mem++ = 0; + // rmt_tx_start(channel, true); + PROBE_2_TOGGLE; + + RMT.tx_conf[channel].idle_out_lv = 1; + delay(1); + RMT.tx_conf[channel].idle_out_lv = 0; + delay(1); + + PROBE_3_TOGGLE; + + RMT.tx_conf[channel].tx_conti_mode = 1; + rmt_set_tx_intr_en(channel, true); + rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + RMT.tx_conf[channel].tx_start = 1; + + delay(1000); + if (false) { + mem--; + mem--; + // destroy end marker => no end interrupt, no repeat + *mem++ = 0x00010001; + *mem = 0x00010001; + } + if (true) { + // just clear conti mode => causes end interrupt, no repeat + RMT.tx_conf[channel].tx_conti_mode = 0; + } + delay(1000); + // actually no need to enable/disable interrupts. + // and this seems to avoid some pitfalls + + // This runs the RMT buffer once + RMT.tx_conf[channel].tx_conti_mode = 1; + delay(1); + RMT.tx_conf[channel].tx_conti_mode = 0; + while (true) { + delay(1000); + PROBE_1_TOGGLE; + } + } +#endif +} + +void StepperQueue::connect_rmt() { + // rmt_set_tx_intr_en(channel, true); + // rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + RMT.tx_conf[channel].idle_out_lv = 0; + RMT.tx_conf[channel].idle_out_en = 1; + //RMT.tx_conf[channel].mem_tx_wrap_en = 0; +#ifndef __ESP32_IDF_V44__ + rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); +#else + rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false); +#endif + +#ifdef TEST_MODE + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 1; + RMT.tx_conf[channel].idle_out_en = 1; + RMT.tx_conf[channel].conf_update = 1; + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 0; + RMT.tx_conf[channel].idle_out_en = 0; + RMT.tx_conf[channel].conf_update = 1; + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 1; + RMT.tx_conf[channel].idle_out_en = 0; + RMT.tx_conf[channel].conf_update = 1; + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 0; + RMT.tx_conf[channel].idle_out_en = 1; + RMT.tx_conf[channel].conf_update = 1; + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 0; + RMT.tx_conf[channel].idle_out_en = 1; + RMT.tx_conf[channel].conf_update = 1; + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 1; + RMT.tx_conf[channel].idle_out_en = 0; + RMT.tx_conf[channel].conf_update = 1; + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 0; + RMT.tx_conf[channel].idle_out_en = 0; + RMT.tx_conf[channel].conf_update = 1; + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 1; + RMT.tx_conf[channel].idle_out_en = 1; + RMT.tx_conf[channel].conf_update = 1; + delay(2); +#endif +} + +void StepperQueue::disconnect_rmt() { + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); +#ifndef __ESP32_IDF_V44__ +// rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1); +#else +// rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); +#endif + RMT.tx_conf[channel].idle_out_en = 0; +} + +void StepperQueue::startQueue_rmt() { +#define TRACE +#ifdef TRACE + USBSerial.println("START"); +#endif +#ifdef TEST_PROBE_2 + PROBE_2(LOW); +#endif +#ifdef TEST_PROBE_3 + PROBE_3(LOW); +#endif +#ifdef TEST_PROBE_1 + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_2_TOGGLE; + delay(2); + PROBE_2_TOGGLE; + delay(2); + PROBE_3_TOGGLE; + delay(3); + PROBE_3_TOGGLE; + delay(3); +#endif + rmt_tx_stop(channel); + // rmt_rx_stop(channel); + rmt_memory_rw_rst(channel); + // the following assignment should not be needed; + // RMT.data_ch[channel] = 0; + uint32_t *mem = FAS_RMT_MEM(channel); + // Fill the buffer with a significant pattern for debugging + // Keep it for now + for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { + mem[i] = 0x0fff8fff; + mem[i + 1] = 0x7fff8fff; + } + mem[2 * PART_SIZE] = 0; + mem[2 * PART_SIZE + 1] = 0; + _isRunning = true; + _rmtStopped = false; + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, 0); + RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.tx_conf[channel].mem_rd_rst = 1; + // RMT.apb_conf.mem_tx_wrap_en = 0; + +//#define TRACE +#ifdef TRACE + USBSerial.print("Queue:"); + USBSerial.print(read_idx); + USBSerial.print('/'); + USBSerial.println(next_write_idx); + for (uint8_t i = 0; i < MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } +#endif + + // set dirpin toggle here + uint8_t rp = read_idx; + if (rp == next_write_idx) { + // nothing to do ? + // Should not happen, so bail + return; + } + if (entry[rp & QUEUE_LEN_MASK].toggle_dir) { + gpio_set_level((gpio_num_t)dirPin, gpio_get_level((gpio_num_t)dirPin) ^ 1); + entry[rp & QUEUE_LEN_MASK].toggle_dir = false; + } + + bufferContainsSteps[0] = true; + bufferContainsSteps[1] = true; + apply_command(this, true, mem); + +#ifdef TRACE + USBSerial.print(RMT.tx_conf[channel].val, BIN); + USBSerial.println(' '); + USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en); + USBSerial.println(' '); + for (uint8_t i = 0; i < MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } + if (!isRunning()) { + USBSerial.println("STOPPED 1"); + } +#endif + + apply_command(this, false, mem); + +#ifdef TRACE + USBSerial.print(RMT.tx_conf[channel].val, BIN); + USBSerial.print(' '); + USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en); + USBSerial.println(' '); + + for (uint8_t i = 0; i < MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } +#endif + rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + rmt_set_tx_intr_en(channel, true); + _rmtStopped = false; + +#ifdef TRACE + USBSerial.print("Interrupt enable:"); + USBSerial.println(RMT.int_ena.val, BIN); + USBSerial.print("Interrupt status:"); + USBSerial.println(RMT.int_st.val, BIN); + USBSerial.print("Threshold: 0x"); + USBSerial.println(RMT.tx_lim[channel].val, HEX); +#endif + + // This starts the rmt module + RMT.tx_conf[channel].tx_conti_mode = 1; + + RMT.tx_conf[channel].tx_start = 1; +} +void StepperQueue::forceStop_rmt() { + stop_rmt(true); + + // and empty the buffer + read_idx = next_write_idx; +} +bool StepperQueue::isReadyForCommands_rmt() { + if (_isRunning) { + return !_rmtStopped; + } + return true; +} +uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; } +#endif diff --git a/src/common.h b/src/common.h index 5a2d0c80..9d095196 100644 --- a/src/common.h +++ b/src/common.h @@ -140,7 +140,7 @@ struct queue_end_s { //========================================================================== // -// ESP32 derivate - ESP32S3 - NOT SUPPORTED +// ESP32 derivate - ESP32S3 // //========================================================================== #elif CONFIG_IDF_TARGET_ESP32S3 @@ -162,12 +162,12 @@ struct queue_end_s { //========================================================================== // -// ESP32 derivate - ESP32C3 - NOT SUPPORTED +// ESP32 derivate - ESP32C3 // //========================================================================== #elif CONFIG_IDF_TARGET_ESP32C3 -#error "esp32c3 is not supported" #define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32C3_RMT #include #include #include @@ -175,6 +175,7 @@ struct queue_end_s { #include #define QUEUES_MCPWM_PCNT 0 #define QUEUES_RMT 2 +#define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) //========================================================================== // diff --git a/src/test_probe.h b/src/test_probe.h index bb2f2ee4..23f24cd9 100644 --- a/src/test_probe.h +++ b/src/test_probe.h @@ -18,6 +18,13 @@ #define TEST_PROBE_3 4 #endif +#define ESP32C3_TEST_PROBE +#ifdef ESP32C3_TEST_PROBE +#define TEST_PROBE_1 1 +#define TEST_PROBE_2 2 +#define TEST_PROBE_3 3 +#endif + #ifdef TEST_PROBE_1 #define PROBE_1(x) digitalWrite(TEST_PROBE_1, x) #define PROBE_1_TOGGLE \ From 8302ae550f077130ce07df2c5847fba78d40ac15 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Thu, 12 Oct 2023 23:00:47 +0200 Subject: [PATCH 04/85] pulses are finally generated and immediate stops works --- src/StepperISR_esp32c3_rmt.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp index 0d886474..d5ba6dff 100644 --- a/src/StepperISR_esp32c3_rmt.cpp +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -1,7 +1,7 @@ #include "StepperISR.h" #ifdef SUPPORT_ESP32C3_RMT -#define TEST_MODE +//#define TEST_MODE #include "test_probe.h" @@ -36,6 +36,7 @@ void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // stop esp32 rmt, by let it hit the end RMT.tx_conf[channel].tx_conti_mode = 0; + RMT.tx_conf[channel].conf_update = 1; // replace second part of buffer with pauses uint32_t *data = FAS_RMT_MEM(channel); @@ -349,6 +350,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { RMT.tx_conf[channel].tx_conti_mode = 1; rmt_set_tx_intr_en(channel, true); rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].tx_start = 1; delay(1000); @@ -362,6 +364,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { if (true) { // just clear conti mode => causes end interrupt, no repeat RMT.tx_conf[channel].tx_conti_mode = 0; + RMT.tx_conf[channel].conf_update = 1; } delay(1000); // actually no need to enable/disable interrupts. @@ -369,8 +372,10 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { // This runs the RMT buffer once RMT.tx_conf[channel].tx_conti_mode = 1; + RMT.tx_conf[channel].conf_update = 1; delay(1); RMT.tx_conf[channel].tx_conti_mode = 0; + RMT.tx_conf[channel].conf_update = 1; while (true) { delay(1000); PROBE_1_TOGGLE; @@ -569,6 +574,7 @@ void StepperQueue::startQueue_rmt() { // This starts the rmt module RMT.tx_conf[channel].tx_conti_mode = 1; + RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].tx_start = 1; } From 9c51893d5e3d6f3ec444caaa2907dd94ad85bdca Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 13 Oct 2023 00:55:55 +0200 Subject: [PATCH 05/85] better understanding of esp32c3 rmt using TEST_MODE --- src/StepperISR_esp32c3_rmt.cpp | 120 +++++++++++++++++++++------------ 1 file changed, 76 insertions(+), 44 deletions(-) diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp index d5ba6dff..cb32da35 100644 --- a/src/StepperISR_esp32c3_rmt.cpp +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -1,15 +1,15 @@ #include "StepperISR.h" #ifdef SUPPORT_ESP32C3_RMT -//#define TEST_MODE +#define TEST_MODE #include "test_probe.h" // The following concept is in use: // -// The buffer of 64Bytes is split into two parts à 31 words. +// The buffer of 48 Bytes is split into two parts à 31 words. // Each part will hold one command (or part of). -// After the 2*31 words an end marker is placed. +// After the 2*23 words an end marker is placed. // The threshold is set to 31. // // This way, the threshold interrupt occurs after the first part @@ -19,6 +19,14 @@ // second. // Every 16 bit entry defines with MSB the output level and the lower 15 bits // the ticks. +// +// +// Important difference of esp32c3 (compared to esp32): +// - configuration updates need an conf_update strobe +// (apparently the manual is not correct by mentioning to set conf_update first) +// - if the end marker is hit in continuous mode, there is no end interrupt +// +// #ifdef SUPPORT_ESP32C3_RMT #define PART_SIZE 23 #define MEM_SIZE 48 @@ -35,8 +43,8 @@ void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // rmt_set_tx_thr_intr_en(channel, false, 0); // stop esp32 rmt, by let it hit the end - RMT.tx_conf[channel].tx_conti_mode = 0; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].tx_conti_mode = 0; // replace second part of buffer with pauses uint32_t *data = FAS_RMT_MEM(channel); @@ -212,15 +220,34 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, #define RMT_FIFO fifo_mask #endif +// The threshold interrupts are happening in the "middle" of the previous entry. +// Best strategy: +// 1. enable threshold interrupt with PART_SIZE+2 +// 2. On every threshold interrupt toggle PART_SIZE and PART_SIZE+1 +// This way the first interrupt happens on the first entry of second half, +// and the second interrupt on the first entry of the first half. +// Afterwards alternating. This way the end interrupt is always "half buffer away" +// from the threshold interrupt #ifdef TEST_MODE #define PROCESS_CHANNEL(ch) \ if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_2_TOGGLE; \ + PROBE_1_TOGGLE; \ } \ if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - PROBE_3_TOGGLE; \ - /* now repeat the interrupt at buffer size + end marker */ \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ + uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ + if (old_limit == PART_SIZE+1) { \ + /* first half of buffer sent */ \ + PROBE_2_TOGGLE; \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } \ + else { \ + /* second half of buffer sent */ \ + PROBE_3_TOGGLE; \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + /* */ \ + RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 0; \ } #else #define PROCESS_CHANNEL(ch) \ @@ -242,13 +269,13 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, if (!q->_rmtStopped) { \ apply_command(q, true, FAS_RMT_MEM(ch)); \ /* now repeat the interrupt at buffer size + end marker */ \ + RMT.tx_conf[ch].conf_update = 1; \ RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ } \ } #endif static void IRAM_ATTR tx_intr_handler(void *arg) { - PROBE_1_TOGGLE; uint32_t mask = RMT.int_st.val; RMT.int_clr.val = mask; PROCESS_CHANNEL(0); @@ -324,33 +351,35 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { #ifdef TEST_MODE if (channel == 0) { RMT.tx_conf[channel].mem_rd_rst = 1; + RMT.tx_conf[channel].mem_rd_rst = 0; uint32_t *mem = FAS_RMT_MEM(channel); // Fill the buffer with a significant pattern for debugging - for (uint8_t i = 0; i < MEM_SIZE/2; i += 2) { + for (uint8_t i = 0; i < PART_SIZE; i++) { *mem++ = 0x7fffffff; - *mem++ = 0x7fff7fff; } - for (uint8_t i = MEM_SIZE/2; i < MEM_SIZE-2; i += 2) { + for (uint8_t i = PART_SIZE; i < 2*PART_SIZE; i++) { *mem++ = 0x3fffdfff; - *mem++ = 0x3fff3fff; } // without end marker it does not loop *mem++ = 0; *mem++ = 0; - // rmt_tx_start(channel, true); - PROBE_2_TOGGLE; - - RMT.tx_conf[channel].idle_out_lv = 1; - delay(1); - RMT.tx_conf[channel].idle_out_lv = 0; - delay(1); - - PROBE_3_TOGGLE; + // conti mode is accepted with the conf_update 1 strobe + // but still do not see interrupts RMT.tx_conf[channel].tx_conti_mode = 1; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; rmt_set_tx_intr_en(channel, true); - rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + // In order to avoid threshold/end interrupt on end, add one + rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 2); + // intr enable needs conf_update strobe RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // tx_start does not need conf_update + PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 RMT.tx_conf[channel].tx_start = 1; delay(1000); @@ -365,6 +394,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { // just clear conti mode => causes end interrupt, no repeat RMT.tx_conf[channel].tx_conti_mode = 0; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; } delay(1000); // actually no need to enable/disable interrupts. @@ -373,9 +403,11 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { // This runs the RMT buffer once RMT.tx_conf[channel].tx_conti_mode = 1; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; delay(1); RMT.tx_conf[channel].tx_conti_mode = 0; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; while (true) { delay(1000); PROBE_1_TOGGLE; @@ -389,6 +421,8 @@ void StepperQueue::connect_rmt() { // rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); RMT.tx_conf[channel].idle_out_lv = 0; RMT.tx_conf[channel].idle_out_en = 1; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; //RMT.tx_conf[channel].mem_tx_wrap_en = 0; #ifndef __ESP32_IDF_V44__ rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); @@ -397,46 +431,40 @@ void StepperQueue::connect_rmt() { #endif #ifdef TEST_MODE + // here gpio is 0 + delay(1); PROBE_3_TOGGLE; RMT.tx_conf[channel].idle_out_lv = 1; - RMT.tx_conf[channel].idle_out_en = 1; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 1 delay(2); PROBE_3_TOGGLE; RMT.tx_conf[channel].idle_out_lv = 0; - RMT.tx_conf[channel].idle_out_en = 0; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 0 delay(2); PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv = 1; RMT.tx_conf[channel].idle_out_en = 0; RMT.tx_conf[channel].conf_update = 1; - delay(2); - PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv = 0; - RMT.tx_conf[channel].idle_out_en = 1; - RMT.tx_conf[channel].conf_update = 1; - delay(2); - PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv = 0; - RMT.tx_conf[channel].idle_out_en = 1; - RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 0 delay(2); PROBE_3_TOGGLE; RMT.tx_conf[channel].idle_out_lv = 1; - RMT.tx_conf[channel].idle_out_en = 0; + RMT.tx_conf[channel].idle_out_en = 1; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 1 delay(2); PROBE_3_TOGGLE; RMT.tx_conf[channel].idle_out_lv = 0; - RMT.tx_conf[channel].idle_out_en = 0; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 0 delay(2); PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv = 1; - RMT.tx_conf[channel].idle_out_en = 1; - RMT.tx_conf[channel].conf_update = 1; - delay(2); #endif } @@ -448,6 +476,7 @@ void StepperQueue::disconnect_rmt() { #else // rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); #endif + RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].idle_out_en = 0; } @@ -462,7 +491,7 @@ void StepperQueue::startQueue_rmt() { #ifdef TEST_PROBE_3 PROBE_3(LOW); #endif -#ifdef TEST_PROBE_1 +#if defined(TEST_PROBE_1) && defined(TEST_PROBE_2) && defined(TEST_PROBE_3) delay(1); PROBE_1_TOGGLE; delay(1); @@ -497,8 +526,11 @@ void StepperQueue::startQueue_rmt() { _rmtStopped = false; rmt_set_tx_intr_en(channel, false); rmt_set_tx_thr_intr_en(channel, false, 0); + RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].mem_tx_wrap_en = 0; RMT.tx_conf[channel].mem_rd_rst = 1; + RMT.tx_conf[channel].mem_rd_rst = 0; + RMT.tx_conf[channel].conf_update = 0; // RMT.apb_conf.mem_tx_wrap_en = 0; //#define TRACE @@ -573,8 +605,8 @@ void StepperQueue::startQueue_rmt() { #endif // This starts the rmt module - RMT.tx_conf[channel].tx_conti_mode = 1; RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].tx_conti_mode = 1; RMT.tx_conf[channel].tx_start = 1; } From 776e0aef403b88c9e5ea41ab6d790d2b7525e394 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 11:07:05 +0200 Subject: [PATCH 06/85] format code --- src/StepperISR_esp32c3_rmt.cpp | 105 +++++++++++++++++---------------- 1 file changed, 53 insertions(+), 52 deletions(-) diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp index cb32da35..55c75b82 100644 --- a/src/StepperISR_esp32c3_rmt.cpp +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -23,7 +23,8 @@ // // Important difference of esp32c3 (compared to esp32): // - configuration updates need an conf_update strobe -// (apparently the manual is not correct by mentioning to set conf_update first) +// (apparently the manual is not correct by mentioning to set conf_update +// first) // - if the end marker is hit in continuous mode, there is no end interrupt // // @@ -91,7 +92,8 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; for (uint8_t i = 0; i < PART_SIZE; i++) { // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2*PART_SIZE)); + *data++ = 0x00010001 * + ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE)); } return; } @@ -226,28 +228,27 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, // 2. On every threshold interrupt toggle PART_SIZE and PART_SIZE+1 // This way the first interrupt happens on the first entry of second half, // and the second interrupt on the first entry of the first half. -// Afterwards alternating. This way the end interrupt is always "half buffer away" -// from the threshold interrupt +// Afterwards alternating. This way the end interrupt is always "half buffer +// away" from the threshold interrupt #ifdef TEST_MODE -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_1_TOGGLE; \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ - if (old_limit == PART_SIZE+1) { \ - /* first half of buffer sent */ \ - PROBE_2_TOGGLE; \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ - } \ - else { \ - /* second half of buffer sent */ \ - PROBE_3_TOGGLE; \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ - } \ - /* */ \ - RMT.tx_conf[ch].conf_update = 1; \ - RMT.tx_conf[ch].conf_update = 0; \ +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_1_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ + if (old_limit == PART_SIZE + 1) { \ + /* first half of buffer sent */ \ + PROBE_2_TOGGLE; \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* second half of buffer sent */ \ + PROBE_3_TOGGLE; \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + /* */ \ + RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 0; \ } #else #define PROCESS_CHANNEL(ch) \ @@ -269,7 +270,7 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, if (!q->_rmtStopped) { \ apply_command(q, true, FAS_RMT_MEM(ch)); \ /* now repeat the interrupt at buffer size + end marker */ \ - RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 1; \ RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ } \ } @@ -317,8 +318,8 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { _initVars(); _step_pin = step_pin; -// digitalWrite(step_pin, LOW); -// pinMode(step_pin, OUTPUT); + // digitalWrite(step_pin, LOW); + // pinMode(step_pin, OUTPUT); channel = (rmt_channel_t)channel_num; @@ -334,9 +335,9 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); rmt_tx_stop(channel); rmt_memory_rw_rst(channel); - //rmt_rx_stop(channel); TX only channel ! - // rmt_tx_memory_reset is not defined in arduino V340 and based on test result - // not needed rmt_tx_memory_reset(channel); + // rmt_rx_stop(channel); TX only channel ! + // rmt_tx_memory_reset is not defined in arduino V340 and based on test + // result not needed rmt_tx_memory_reset(channel); if (channel_num == 0) { rmt_isr_register(tx_intr_handler, NULL, ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); @@ -357,30 +358,30 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { for (uint8_t i = 0; i < PART_SIZE; i++) { *mem++ = 0x7fffffff; } - for (uint8_t i = PART_SIZE; i < 2*PART_SIZE; i++) { + for (uint8_t i = PART_SIZE; i < 2 * PART_SIZE; i++) { *mem++ = 0x3fffdfff; } // without end marker it does not loop *mem++ = 0; *mem++ = 0; - // conti mode is accepted with the conf_update 1 strobe - // but still do not see interrupts + // conti mode is accepted with the conf_update 1 strobe + // but still do not see interrupts RMT.tx_conf[channel].tx_conti_mode = 1; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; rmt_set_tx_intr_en(channel, true); - // In order to avoid threshold/end interrupt on end, add one + // In order to avoid threshold/end interrupt on end, add one rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 2); - // intr enable needs conf_update strobe - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; - RMT.tx_conf[channel].mem_tx_wrap_en = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; - // tx_start does not need conf_update - PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 - RMT.tx_conf[channel].tx_start = 1; + // intr enable needs conf_update strobe + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // tx_start does not need conf_update + PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 + RMT.tx_conf[channel].tx_start = 1; delay(1000); if (false) { @@ -393,8 +394,8 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { if (true) { // just clear conti mode => causes end interrupt, no repeat RMT.tx_conf[channel].tx_conti_mode = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; } delay(1000); // actually no need to enable/disable interrupts. @@ -402,15 +403,15 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { // This runs the RMT buffer once RMT.tx_conf[channel].tx_conti_mode = 1; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; delay(1); RMT.tx_conf[channel].tx_conti_mode = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; while (true) { delay(1000); - PROBE_1_TOGGLE; + PROBE_1_TOGGLE; } } #endif @@ -423,7 +424,7 @@ void StepperQueue::connect_rmt() { RMT.tx_conf[channel].idle_out_en = 1; RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].conf_update = 0; - //RMT.tx_conf[channel].mem_tx_wrap_en = 0; + // RMT.tx_conf[channel].mem_tx_wrap_en = 0; #ifndef __ESP32_IDF_V44__ rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); #else From f540a5e7f00d9be1121506444b4ad60a3a7b5fbb Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 11:14:03 +0200 Subject: [PATCH 07/85] StepperDemo can work with USBSerial of esp32c3 --- CHANGELOG.md | 4 + examples/StepperDemo/StepperDemo.ino | 199 ++++++++++++++++----------- examples/StepperDemo/generic.h | 11 ++ examples/StepperDemo/test_seq.h | 2 + examples/StepperDemo/test_seq_07.cpp | 6 +- examples/StepperDemo/test_seq_08.cpp | 22 +-- examples/StepperDemo/test_seq_09.cpp | 16 +-- examples/StepperDemo/test_seq_10.cpp | 6 +- examples/StepperDemo/test_seq_11.cpp | 32 ++--- examples/StepperDemo/test_seq_12.cpp | 6 +- examples/StepperDemo/test_seq_13.cpp | 6 +- 11 files changed, 180 insertions(+), 130 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5584f6a0..35bf386c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,10 @@ TODO: - setCurrentPosition() should adjust the maintained last position bits in queue. can be called a bug - rename PoorManFloat to e.g. Log2Representation - rename RampConstAcceleration to e.g. RampControl +- merge the two esp32 rmt drivers as soon as esp32c3 works + +pre-0.30.6: +- Untested support for ESP32C3 0.30.5: - Fix target position for a move() interrupting the keep running mode diff --git a/examples/StepperDemo/StepperDemo.ino b/examples/StepperDemo/StepperDemo.ino index d32759d4..e0d661e9 100644 --- a/examples/StepperDemo/StepperDemo.ino +++ b/examples/StepperDemo/StepperDemo.ino @@ -124,9 +124,13 @@ const struct stepper_config_s stepper_config[MAX_STEPPER] = { #elif defined(ARDUINO_ARCH_ESP32) // Example hardware configuration for esp32 board. // Please adapt to your configuration +#ifdef CONFIG_IDF_TARGET_ESP32C3 +const uint8_t led_pin = 8; +#else const uint8_t led_pin = PIN_UNDEFINED; +#endif const struct stepper_config_s stepper_config[MAX_STEPPER] = { - // clang-format off +// clang-format off // Test-HW // Position 01 linked to atmega nano // 2: Enable Left Pin 13 GPIO13 , DIR Right Pin 7 GPIO18, Step Right Pin 13 GPIO15 @@ -137,7 +141,31 @@ const struct stepper_config_s stepper_config[MAX_STEPPER] = { // 7: Enable Left Pin 8 GPIO25 A8, DIR Right Pin 2 GPIO22, Step Right Pin 8 GPIO5 // ALL Enable: Right Pin 1 GPIO23 // Left Pin 15: +5V - // clang-format on +// clang-format on +#ifdef CONFIG_IDF_TARGET_ESP32C3 + { + step : 6, + enable_low_active : PIN_UNDEFINED, + enable_high_active : PIN_UNDEFINED, + direction : PIN_UNDEFINED, + dir_change_delay : 0, + direction_high_count_up : true, + auto_enable : true, + on_delay_us : 50, + off_delay_ms : 1000 + }, + { + step : 7, + enable_low_active : PIN_UNDEFINED, + enable_high_active : PIN_UNDEFINED, + direction : PIN_UNDEFINED, + dir_change_delay : 0, + direction_high_count_up : true, + auto_enable : true, + on_delay_us : 500, + off_delay_ms : 1000 + } +#else { // position 01.234567 => 2 step : 17, @@ -163,6 +191,7 @@ const struct stepper_config_s stepper_config[MAX_STEPPER] = { on_delay_us : 500, off_delay_ms : 1000 } +#endif #if MAX_STEPPER > 2 , { @@ -794,7 +823,7 @@ void output_msg(uint8_t x) { if (y <= MSG_USAGE_CONFIG) { output_msg(y); } else { - Serial.print(ch); + SerialInterface.print(ch); } } } @@ -822,7 +851,7 @@ void setDirectionPin(uint8_t direction, bool polarity) { delay10us(); if (digitalRead(direction) != polarity) { output_msg(MSG_CANNOT_SET_DIRECTION_PIN); - Serial.println(polarity ? "HIGH" : "LOW"); + SerialInterface.println(polarity ? "HIGH" : "LOW"); } } } @@ -853,9 +882,11 @@ void test_direct_drive(const struct stepper_config_s *stepper) { output_msg(MSG_ENABLE_HIGH_PIN_IS_NOT_HIGH); } } - setDirectionPin(direction, direction_high_count_up); - do3200Steps(step); - setDirectionPin(direction, !direction_high_count_up); + if (direction != PIN_UNDEFINED) { + setDirectionPin(direction, direction_high_count_up); + do3200Steps(step); + setDirectionPin(direction, !direction_high_count_up); + } do3200Steps(step); if (enableLow != PIN_UNDEFINED) { digitalWrite(enableLow, HIGH); @@ -876,12 +907,14 @@ void test_direct_drive(const struct stepper_config_s *stepper) { #endif void setup() { - Serial.begin(115200); + SerialInterface.begin(115200); + delay(2000); output_msg(MSG_STEPPER_VERSION); - Serial.print(" F_CPU="); - Serial.println(F_CPU); - Serial.print(" TICKS_PER_S="); - Serial.println(TICKS_PER_S); + SerialInterface.print(" F_CPU="); + SerialInterface.println(F_CPU); + SerialInterface.print(" TICKS_PER_S="); + SerialInterface.println(TICKS_PER_S); + SerialInterface.println(MAX_STEPPER); // If you are not sure, that the stepper hardware is working, // then try first direct port manipulation and uncomment the next line. @@ -939,86 +972,86 @@ bool simulate_digitalRead_error = false; bool simulate_blocked_ISR = false; void info(FastAccelStepper *s, bool long_info) { - Serial.print('@'); + SerialInterface.print('@'); #if defined(SUPPORT_ESP32_PULSE_COUNTER) if (s->pulseCounterAttached()) { int16_t pcnt_pos_1 = s->readPulseCounter(); int32_t pos = s->getCurrentPosition(); int16_t pcnt_pos_2 = s->readPulseCounter(); - Serial.print(pos); - Serial.print(" ["); - Serial.print(pcnt_pos_1); - Serial.print(']'); + SerialInterface.print(pos); + SerialInterface.print(" ["); + SerialInterface.print(pcnt_pos_1); + SerialInterface.print(']'); if (pcnt_pos_1 != pcnt_pos_2) { - Serial.print(" ["); - Serial.print(pcnt_pos_2); - Serial.print(']'); + SerialInterface.print(" ["); + SerialInterface.print(pcnt_pos_2); + SerialInterface.print(']'); } } else { int32_t pos = s->getCurrentPosition(); - Serial.print(pos); + SerialInterface.print(pos); } #else int32_t pos = s->getCurrentPosition(); - Serial.print(pos); + SerialInterface.print(pos); #endif if (s->isRunning()) { if (s->isRunningContinuously()) { - Serial.print(" nonstop"); + SerialInterface.print(" nonstop"); } else { - Serial.print(" => "); - Serial.print(s->targetPos()); + SerialInterface.print(" => "); + SerialInterface.print(s->targetPos()); } - Serial.print(" QueueEnd="); - Serial.print(s->getPositionAfterCommandsCompleted()); + SerialInterface.print(" QueueEnd="); + SerialInterface.print(s->getPositionAfterCommandsCompleted()); if (speed_in_milli_hz) { - Serial.print(" v="); - Serial.print(s->getCurrentSpeedInMilliHz()); - Serial.print("mSteps/s"); + SerialInterface.print(" v="); + SerialInterface.print(s->getCurrentSpeedInMilliHz()); + SerialInterface.print("mSteps/s"); } else { - Serial.print('/'); - Serial.print(s->getPeriodInUsAfterCommandsCompleted()); - Serial.print("us/"); - Serial.print(s->getPeriodInTicksAfterCommandsCompleted()); - Serial.print("ticks"); + SerialInterface.print('/'); + SerialInterface.print(s->getPeriodInUsAfterCommandsCompleted()); + SerialInterface.print("us/"); + SerialInterface.print(s->getPeriodInTicksAfterCommandsCompleted()); + SerialInterface.print("ticks"); } if (s->isRampGeneratorActive()) { switch (s->rampState() & RAMP_STATE_MASK) { case RAMP_STATE_IDLE: - Serial.print(" IDLE "); + SerialInterface.print(" IDLE "); break; case RAMP_STATE_ACCELERATE: - Serial.print(" ACC "); + SerialInterface.print(" ACC "); break; case RAMP_STATE_DECELERATE: - Serial.print(" RED "); // Reduce + SerialInterface.print(" RED "); // Reduce break; case RAMP_STATE_COAST: - Serial.print(" COAST"); + SerialInterface.print(" COAST"); break; case RAMP_STATE_REVERSE: - Serial.print(" REV "); + SerialInterface.print(" REV "); break; default: - Serial.print(s->rampState()); + SerialInterface.print(s->rampState()); } } else { - Serial.print(" MANU"); + SerialInterface.print(" MANU"); } } else { if (long_info) { output_msg(MSG_ACCELERATION_STATUS); - Serial.print(s->getAcceleration()); + SerialInterface.print(s->getAcceleration()); if (speed_in_milli_hz) { output_msg(MSG_SPEED_STATUS_FREQ); - Serial.print(s->getSpeedInMilliHz()); + SerialInterface.print(s->getSpeedInMilliHz()); } else { output_msg(MSG_SPEED_STATUS_TIME); - Serial.print(s->getSpeedInUs()); + SerialInterface.print(s->getSpeedInUs()); } } } - Serial.print(' '); + SerialInterface.print(' '); } void stepper_info() { @@ -1054,18 +1087,18 @@ void output_info(bool only_running) { if (stepper[i]) { if (!only_running) { if (i == selected) { - Serial.print(">> "); + SerialInterface.print(">> "); } else { - Serial.print(" "); + SerialInterface.print(" "); } } if (!only_running || stepper[i]->isRunning()) { - Serial.print('M'); - Serial.print(i + 1); - Serial.print(": "); + SerialInterface.print('M'); + SerialInterface.print(i + 1); + SerialInterface.print(": "); info(stepper[i], !only_running); if (!only_running) { - Serial.println(); + SerialInterface.println(); } else { need_ln = true; } @@ -1073,7 +1106,7 @@ void output_info(bool only_running) { } } if (need_ln) { - Serial.println(); + SerialInterface.println(); } } @@ -1112,7 +1145,7 @@ bool process_cmd(char *cmd) { if ((val_n[0] > 0) && (val_n[0] <= MAX_STEPPER)) { output_msg(MSG_SELECT_STEPPER); selected = val_n[0] - 1; - Serial.println(selected + 1); + SerialInterface.println(selected + 1); return true; } } @@ -1120,13 +1153,13 @@ bool process_cmd(char *cmd) { case MODE(normal, 'r'): #if defined(ARDUINO_ARCH_ESP32) if (strcmp(cmd, "eset") == 0) { - Serial.println("ESP reset"); + SerialInterface.println("ESP reset"); esp_task_wdt_init(1, true); esp_task_wdt_add(NULL); while (true) ; } - Serial.println("ESP restart"); + SerialInterface.println("ESP restart"); ESP.restart(); #endif #if defined(ARDUINO_ARCH_AVR) @@ -1188,7 +1221,7 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_SET_ACCELERATION_TO); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); res = stepper_selected->setAcceleration(val_n[0]); if (res < 0) { output_msg(MSG_ERROR_INVALID_VALUE); @@ -1200,7 +1233,7 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_LINEAR_ACCELERATION); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); stepper_selected->setLinearAcceleration(val_n[0]); return true; } @@ -1209,7 +1242,7 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_JUMP_START); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); stepper_selected->setJumpStart(val_n[0]); return true; } @@ -1219,7 +1252,7 @@ bool process_cmd(char *cmd) { if (*endptr == 0) { speed_in_milli_hz = false; output_msg(MSG_SET_SPEED_TO_US); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); res = stepper_selected->setSpeedInUs(val_n[0]); if (res < 0) { output_msg(MSG_ERROR_INVALID_VALUE); @@ -1232,7 +1265,7 @@ bool process_cmd(char *cmd) { if (*endptr == 0) { speed_in_milli_hz = true; output_msg(MSG_SET_SPEED_TO_HZ); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); res = stepper_selected->setSpeedInHz(val_n[0]); if (res < 0) { output_msg(MSG_ERROR_INVALID_VALUE); @@ -1245,7 +1278,7 @@ bool process_cmd(char *cmd) { if (*endptr == 0) { speed_in_milli_hz = true; output_msg(MSG_SET_SPEED_TO_MILLI_HZ); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); res = stepper_selected->setSpeedInMilliHz(val_n[0]); if (res < 0) { output_msg(MSG_ERROR_INVALID_VALUE); @@ -1257,7 +1290,7 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_SET_ACCELERATION_TO); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); res = stepper_selected->moveByAcceleration(val_n[0]); output_msg(MSG_MOVE_OK + res); return true; @@ -1267,7 +1300,7 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_MOVE_STEPS); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); res = stepper_selected->move(val_n[0]); output_msg(MSG_MOVE_OK + res); return true; @@ -1277,7 +1310,7 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_MOVE_TO_POSITION); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); res = stepper_selected->moveTo(val_n[0]); output_msg(MSG_MOVE_OK + res); return true; @@ -1287,7 +1320,7 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_SET_POSITION); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); stepper_selected->setCurrentPosition(val_n[0]); return true; } @@ -1296,10 +1329,10 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_SET_ENABLE_TIME); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); res = stepper_selected->setDelayToEnable(val_n[0]); output_msg(MSG_RETURN_CODE); - Serial.println(res); + SerialInterface.println(res); return true; } break; @@ -1307,7 +1340,7 @@ bool process_cmd(char *cmd) { val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { output_msg(MSG_SET_DISABLE_TIME); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); stepper_selected->setDelayToDisable(val_n[0]); return true; } @@ -1315,7 +1348,7 @@ bool process_cmd(char *cmd) { case MODE(normal, 'w'): val_n[0] = strtol(cmd, &endptr, 10); if (*endptr == 0) { - Serial.print(val_n[0]); + SerialInterface.print(val_n[0]); output_msg(MSG_WAIT_MS); pause_ms = val_n[0]; pause_start = millis(); @@ -1333,26 +1366,26 @@ bool process_cmd(char *cmd) { case 1: output_msg(MSG_DIRECTION_PIN); output_msg(MSG_SET_TO_PIN); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); stepper_selected->setDirectionPin(val_n[0]); return true; case 2: case 3: output_msg(MSG_DIRECTION_PIN); output_msg(MSG_SET_TO_PIN); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); output_msg(MSG_DIRECTION_PIN); if (val_n[1] != 0) { output_msg(MSG_HIGH_COUNT_DOWN); } else { output_msg(MSG_HIGH_COUNT_UP); } - Serial.println(); + SerialInterface.println(); if (gv == 2) { stepper_selected->setDirectionPin(val_n[0], val_n[1]); } else { output_msg(MSG_DELAY); - Serial.println(val_n[2]); + SerialInterface.println(val_n[2]); stepper_selected->setDirectionPin(val_n[0], val_n[1], val_n[2]); } return true; @@ -1401,7 +1434,7 @@ bool process_cmd(char *cmd) { output_msg(MSG_RUN_FORWARD); res = stepper_selected->runForward(); output_msg(MSG_RETURN_CODE); - Serial.println(res); + SerialInterface.println(res); return true; } break; @@ -1410,7 +1443,7 @@ bool process_cmd(char *cmd) { output_msg(MSG_RUN_BACKWARD); res = stepper_selected->runBackward(); output_msg(MSG_RETURN_CODE); - Serial.println(res); + SerialInterface.println(res); return true; } break; @@ -1450,7 +1483,7 @@ bool process_cmd(char *cmd) { while (stepper_selected->isRunning()) { // do nothing } - Serial.println("STOPPED"); + SerialInterface.println("STOPPED"); } #endif return true; @@ -1513,7 +1546,7 @@ bool process_cmd(char *cmd) { } if (get_val1_val2_val3(cmd) == 3) { output_msg(MSG_ATTACH_PULSE_COUNTER); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); if (!stepper_selected->attachToPulseCounter(val_n[0], val_n[1], val_n[2])) { output_msg(MSG_ERROR_ATTACH_PULSE_COUNTER); @@ -1522,7 +1555,7 @@ bool process_cmd(char *cmd) { } if (get_val1_val2_val3(cmd) == 1) { output_msg(MSG_ATTACH_PULSE_COUNTER); - Serial.println(val_n[0]); + SerialInterface.println(val_n[0]); if (!stepper_selected->attachToPulseCounter(val_n[0])) { output_msg(MSG_ERROR_ATTACH_PULSE_COUNTER); } @@ -1536,7 +1569,7 @@ bool process_cmd(char *cmd) { const struct test_seq_def_s *ts = &test_sequence[i]; if (strcmp(out_buffer, ts->code) == 0) { output_msg(MSG_SELECT_TEST_SEQUENCE); - Serial.println(out_buffer); + SerialInterface.println(out_buffer); test_seq[selected].test = ts->test; test_seq[selected].state = 0; return true; @@ -1551,8 +1584,8 @@ bool process_cmd(char *cmd) { void loop() { char ch = 0; if (input == NULL) { - if (Serial.available()) { - ch = Serial.read(); + if (SerialInterface.available()) { + ch = SerialInterface.read(); } } else { ch = *input; @@ -1594,7 +1627,7 @@ void loop() { if (out_ptr > 0) { if (!process_cmd(out_buffer)) { output_msg(MSG_UNKNOWN_COMMAND); - Serial.println(out_buffer); + SerialInterface.println(out_buffer); } } out_ptr = 0; diff --git a/examples/StepperDemo/generic.h b/examples/StepperDemo/generic.h index d4154d96..c9e9873a 100644 --- a/examples/StepperDemo/generic.h +++ b/examples/StepperDemo/generic.h @@ -1,3 +1,6 @@ +#ifndef GENERIC_H +#define GENERIC_H + #if defined(ARDUINO_ARCH_AVR) #define get_char(x) pgm_read_byte(x) #define MSG_TYPE PGM_P @@ -7,3 +10,11 @@ #else #error "Unsupported derivate" #endif + +#if defined(CONFIG_IDF_TARGET_ESP32C3) +#define SerialInterface USBSerial +#else +#define SerialInterface Serial +#endif + +#endif diff --git a/examples/StepperDemo/test_seq.h b/examples/StepperDemo/test_seq.h index 514e4f62..0dd79794 100644 --- a/examples/StepperDemo/test_seq.h +++ b/examples/StepperDemo/test_seq.h @@ -1,4 +1,6 @@ #include "FastAccelStepper.h" +#include "generic.h" + #define TEST_STATE_ERROR 0xffff struct test_seq_s { bool (*test)(FastAccelStepper *stepper, struct test_seq_s *, diff --git a/examples/StepperDemo/test_seq_07.cpp b/examples/StepperDemo/test_seq_07.cpp index f469a47a..f393f959 100644 --- a/examples/StepperDemo/test_seq_07.cpp +++ b/examples/StepperDemo/test_seq_07.cpp @@ -14,7 +14,7 @@ bool test_seq_07(FastAccelStepper *stepper, struct test_seq_s *seq, case 1: if ((stepper->rampState() & RAMP_STATE_MASK) == RAMP_STATE_COAST) { int32_t dt = time_ms - seq->u32_1; - Serial.println(dt); + SerialInterface.println(dt); // 779 esp, 805 avr (neu 810 avr), 820: esp32 with rmt, 812: esp32 with // rmt second channel if (abs(dt - 792) > 30) { @@ -43,11 +43,11 @@ bool test_seq_07(FastAccelStepper *stepper, struct test_seq_s *seq, case 4: if (!stepper->isRunning()) { int32_t dt = time_ms - seq->u32_1; - Serial.println(dt); + SerialInterface.println(dt); if (abs(dt - 1495) > 30) { seq->state = TEST_STATE_ERROR; } - Serial.println(stepper->getPositionAfterCommandsCompleted()); + SerialInterface.println(stepper->getPositionAfterCommandsCompleted()); if (stepper->getPositionAfterCommandsCompleted() != 0) { seq->state = TEST_STATE_ERROR; } diff --git a/examples/StepperDemo/test_seq_08.cpp b/examples/StepperDemo/test_seq_08.cpp index 9c034b59..239726b1 100644 --- a/examples/StepperDemo/test_seq_08.cpp +++ b/examples/StepperDemo/test_seq_08.cpp @@ -15,7 +15,7 @@ bool test_seq_08(FastAccelStepper *stepper, struct test_seq_s *seq, case 0: // INIT srand(135); if (!stepper->attachToPulseCounter(7)) { - Serial.println("Error attaching to pulse counter"); + SerialInterface.println("Error attaching to pulse counter"); seq->state = TEST_STATE_ERROR; return true; } @@ -27,10 +27,10 @@ bool test_seq_08(FastAccelStepper *stepper, struct test_seq_s *seq, int16_t pcnt = stepper->readPulseCounter(); int32_t spos = stepper->getPositionAfterCommandsCompleted(); if ((pcnt & 0x3fff) != (spos & 0x3fff)) { - Serial.print("stepper pos="); - Serial.print(spos); - Serial.print(" real pos="); - Serial.println(pcnt); + SerialInterface.print("stepper pos="); + SerialInterface.print(spos); + SerialInterface.print(" real pos="); + SerialInterface.println(pcnt); seq->state = TEST_STATE_ERROR; return true; @@ -69,12 +69,12 @@ bool test_seq_08(FastAccelStepper *stepper, struct test_seq_s *seq, move = -move; } - Serial.print("speed="); - Serial.print(speed); - Serial.print(" accel="); - Serial.print(accel); - Serial.print(" move="); - Serial.println(move); + SerialInterface.print("speed="); + SerialInterface.print(speed); + SerialInterface.print(" accel="); + SerialInterface.print(accel); + SerialInterface.print(" move="); + SerialInterface.println(move); stepper->setSpeedInUs(speed); stepper->setAcceleration(accel); stepper->move(move); diff --git a/examples/StepperDemo/test_seq_09.cpp b/examples/StepperDemo/test_seq_09.cpp index 8105cbfc..f84302c7 100644 --- a/examples/StepperDemo/test_seq_09.cpp +++ b/examples/StepperDemo/test_seq_09.cpp @@ -7,7 +7,7 @@ bool test_seq_09(FastAccelStepper *stepper, struct test_seq_s *seq, srand(135); #if defined(SUPPORT_ESP32_PULSE_COUNTER) if (!stepper->attachToPulseCounter(7)) { - Serial.println("Error attaching to pulse counter"); + SerialInterface.println("Error attaching to pulse counter"); seq->state = TEST_STATE_ERROR; return true; } @@ -28,10 +28,10 @@ bool test_seq_09(FastAccelStepper *stepper, struct test_seq_s *seq, uint32_t accel = rand() % (AMAX * 4); accel = accel >> ((accel % 4) + 2); accel = accel + AMIN; - Serial.print("speed="); - Serial.print(speed); - Serial.print(" accel="); - Serial.println(accel); + SerialInterface.print("speed="); + SerialInterface.print(speed); + SerialInterface.print(" accel="); + SerialInterface.println(accel); stepper->setSpeedInUs(speed); stepper->setAcceleration(accel); if (rand() & 1) { @@ -58,8 +58,8 @@ bool test_seq_09(FastAccelStepper *stepper, struct test_seq_s *seq, #if defined(SUPPORT_ESP32_PULSE_COUNTER) int16_t old = seq->s16_1; seq->s16_1 = stepper->readPulseCounter(); - Serial.print("Steps needed for stop="); - Serial.println(old - seq->s16_1); + SerialInterface.print("Steps needed for stop="); + SerialInterface.println(old - seq->s16_1); #endif seq->state++; } @@ -68,7 +68,7 @@ bool test_seq_09(FastAccelStepper *stepper, struct test_seq_s *seq, if (time_ms - seq->u32_1 >= 100) { #if defined(SUPPORT_ESP32_PULSE_COUNTER) if (seq->s16_1 != stepper->readPulseCounter()) { - Serial.println("Step AFTER stop"); + SerialInterface.println("Step AFTER stop"); } #endif seq->u32_1 = time_ms; diff --git a/examples/StepperDemo/test_seq_10.cpp b/examples/StepperDemo/test_seq_10.cpp index cf99f56e..d5c2ffbe 100644 --- a/examples/StepperDemo/test_seq_10.cpp +++ b/examples/StepperDemo/test_seq_10.cpp @@ -50,9 +50,9 @@ bool test_seq_10(FastAccelStepper *stepper, struct test_seq_s *seq, stepper->applySpeedAcceleration(); stepper->stopMove(); } else { - Serial.print("Speed changes 64us <=> "); - Serial.print(64 + seq->s16_1); - Serial.println("us"); + SerialInterface.print("Speed changes 64us <=> "); + SerialInterface.print(64 + seq->s16_1); + SerialInterface.println("us"); seq->state = 2; } } diff --git a/examples/StepperDemo/test_seq_11.cpp b/examples/StepperDemo/test_seq_11.cpp index 8f158084..4a91649a 100644 --- a/examples/StepperDemo/test_seq_11.cpp +++ b/examples/StepperDemo/test_seq_11.cpp @@ -28,16 +28,16 @@ bool test_seq_11(FastAccelStepper *stepper, struct test_seq_s *seq, for (uint16_t i = 0; i <= 1000; i++) { int32_t pos = stepper->getCurrentPosition(); if (pos < seq->s32_1) { - Serial.print("i="); - Serial.print(i); + SerialInterface.print("i="); + SerialInterface.print(i); if (i != 0) { - Serial.print(" prev pos="); - Serial.print(prev_pos); + SerialInterface.print(" prev pos="); + SerialInterface.print(prev_pos); } - Serial.print(" old pos="); - Serial.print(seq->s32_1); - Serial.print(" curr pos="); - Serial.println(pos); + SerialInterface.print(" old pos="); + SerialInterface.print(seq->s32_1); + SerialInterface.print(" curr pos="); + SerialInterface.println(pos); stepper->stopMove(); seq->state = TEST_STATE_ERROR; return true; @@ -54,16 +54,16 @@ bool test_seq_11(FastAccelStepper *stepper, struct test_seq_s *seq, for (uint16_t i = 0; i <= 1000; i++) { int32_t pos = stepper->getCurrentPosition(); if (pos > seq->s32_1) { - Serial.print("i="); - Serial.print(i); + SerialInterface.print("i="); + SerialInterface.print(i); if (i != 0) { - Serial.print(" prev pos="); - Serial.print(prev_pos); + SerialInterface.print(" prev pos="); + SerialInterface.print(prev_pos); } - Serial.print(" old pos="); - Serial.print(seq->s32_1); - Serial.print(" curr pos="); - Serial.println(pos); + SerialInterface.print(" old pos="); + SerialInterface.print(seq->s32_1); + SerialInterface.print(" curr pos="); + SerialInterface.println(pos); stepper->stopMove(); seq->state = TEST_STATE_ERROR; return true; diff --git a/examples/StepperDemo/test_seq_12.cpp b/examples/StepperDemo/test_seq_12.cpp index 1d718b0d..ef00127e 100644 --- a/examples/StepperDemo/test_seq_12.cpp +++ b/examples/StepperDemo/test_seq_12.cpp @@ -51,9 +51,9 @@ bool test_seq_12(FastAccelStepper *stepper, struct test_seq_s *seq, stepper->applySpeedAcceleration(); stepper->stopMove(); } else { - Serial.print("Speed changes 64us <=> "); - Serial.print(64 + seq->s16_1); - Serial.println("us"); + SerialInterface.print("Speed changes 64us <=> "); + SerialInterface.print(64 + seq->s16_1); + SerialInterface.println("us"); seq->state = 2; } } diff --git a/examples/StepperDemo/test_seq_13.cpp b/examples/StepperDemo/test_seq_13.cpp index c619de24..37a90d3d 100644 --- a/examples/StepperDemo/test_seq_13.cpp +++ b/examples/StepperDemo/test_seq_13.cpp @@ -18,8 +18,8 @@ bool test_seq_13(FastAccelStepper *stepper, struct test_seq_s *seq, case 3: res = stepper->addQueueEntry(&cmd_step); if (res > 1) { - Serial.print(res); - Serial.print(' '); + SerialInterface.print(res); + SerialInterface.print(' '); } else { seq->u32_1 = time_ms; seq->state++; @@ -28,7 +28,7 @@ bool test_seq_13(FastAccelStepper *stepper, struct test_seq_s *seq, case 4: if (time_ms - seq->u32_1 >= 20) { if (stepper->getCurrentPosition() != 4) { - Serial.println("not all raw commands executed"); + SerialInterface.println("not all raw commands executed"); seq->state = TEST_STATE_ERROR; } return true; From 3d6ddea75fb57b168dd912a8d6364fec31683e26 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 18:02:38 +0200 Subject: [PATCH 08/85] first version with esp32c3 having started, coasted and stopped once --- CHANGELOG.md | 2 +- src/StepperISR_esp32_rmt.cpp | 8 +- src/StepperISR_esp32c3_rmt.cpp | 194 +++++++++++++++++++-------------- src/test_probe.h | 6 +- 4 files changed, 118 insertions(+), 92 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 35bf386c..434eaafb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,7 +7,7 @@ TODO: - merge the two esp32 rmt drivers as soon as esp32c3 works pre-0.30.6: -- Untested support for ESP32C3 +- Partly tested support for ESP32C3 0.30.5: - Fix target position for a move() interrupting the keep running mode diff --git a/src/StepperISR_esp32_rmt.cpp b/src/StepperISR_esp32_rmt.cpp index 5ee62b9b..ce4371ec 100644 --- a/src/StepperISR_esp32_rmt.cpp +++ b/src/StepperISR_esp32_rmt.cpp @@ -115,10 +115,10 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, if (steps < PART_SIZE / 2) { for (uint8_t i = 1; i < steps; i++) { // steps-1 iterations - *data++ = 0x40017fff | 0x8000; + *data++ = 0x40007fff | 0x8000; *data++ = 0x20002000; } - *data++ = 0x40017fff | 0x8000; + *data++ = 0x40007fff | 0x8000; uint16_t delta = PART_SIZE - 2 * steps; delta <<= 5; *data++ = 0x20002000 - delta; @@ -132,10 +132,10 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, } else { steps -= PART_SIZE / 2; for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - *data++ = 0x40017fff | 0x8000; + *data++ = 0x40007fff | 0x8000; *data++ = 0x20002000; } - *data++ = 0x40017fff | 0x8000; + *data++ = 0x40007fff | 0x8000; last_entry = 0x20002000; } } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp index 55c75b82..105b9c52 100644 --- a/src/StepperISR_esp32c3_rmt.cpp +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -1,41 +1,51 @@ #include "StepperISR.h" #ifdef SUPPORT_ESP32C3_RMT -#define TEST_MODE +//#define TEST_MODE #include "test_probe.h" // The following concept is in use: // -// The buffer of 48 Bytes is split into two parts à 31 words. +// The rmt buffer is split into two parts. // Each part will hold one command (or part of). -// After the 2*23 words an end marker is placed. -// The threshold is set to 31. -// -// This way, the threshold interrupt occurs after the first part -// and the end interrupt after the second part. +// After the two parts an end marker is placed. // // Of these 32 bits, the low 16-bit entry is sent first and the high entry // second. // Every 16 bit entry defines with MSB the output level and the lower 15 bits // the ticks. // -// // Important difference of esp32c3 (compared to esp32): // - configuration updates need an conf_update strobe // (apparently the manual is not correct by mentioning to set conf_update // first) // - if the end marker is hit in continuous mode, there is no end interrupt -// +// - there is no tick lost with the end marker +// - minimum periods as per relation 1 and 2 to be adhered to +// // #ifdef SUPPORT_ESP32C3_RMT #define PART_SIZE 23 #define MEM_SIZE 48 #else +#error #define PART_SIZE 31 #define MEM_SIZE 64 #endif +// In order to avoid threshold/end interrupt on end, add one +#define enable_rmt_interrupts(channel) { \ + RMT.tx_lim[channel].RMT_LIMIT = PART_SIZE + 2;\ + RMT.tx_conf[channel].conf_update = 1; \ + RMT.tx_conf[channel].conf_update = 0; \ + RMT.int_clr.val |= 0x101 << channel; \ + RMT.int_ena.val |= 0x101 << channel; \ +} +#define disable_rmt_interrupts(channel) { \ + RMT.int_ena.val &= ~(0x101 << channel); \ +} + void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // We are stopping the rmt by letting it run into the end at high speed. // @@ -44,8 +54,9 @@ void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // rmt_set_tx_thr_intr_en(channel, false, 0); // stop esp32 rmt, by let it hit the end - RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].tx_conti_mode = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; // replace second part of buffer with pauses uint32_t *data = FAS_RMT_MEM(channel); @@ -74,7 +85,8 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, for (uint8_t i = 0; i < PART_SIZE; i++) { // make a pause with approx. 1ms // 258 ticks * 2 * 31 = 15996 @ 16MHz - *data++ = 0x01020102; + // 347 ticks * 2 * 23 = 15962 @ 16MHz + *data++ = 0x010001 * (16000/2/PART_SIZE); } } else { q->stop_rmt(false); @@ -108,21 +120,23 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, uint8_t steps = e_curr->steps; uint16_t ticks = e_curr->ticks; // if (steps != 0) { - // PROBE_2_TOGGLE; + // PROBE_1_TOGGLE; //} uint32_t last_entry; if (steps == 0) { q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - // two pauses à 3 ticks. the 2 for debugging - *data++ = 0x00010002; - ticks -= 3; - } + // Perhaps the rmt performs look ahead + ticks -= (PART_SIZE-2) * 4 + 8; uint16_t ticks_l = ticks >> 1; uint16_t ticks_r = ticks - ticks_l; last_entry = ticks_l; last_entry <<= 16; last_entry |= ticks_r; + *data++ = last_entry; + for (uint8_t i = 1; i < PART_SIZE - 1; i++) { + *data++ = 0x00020002; + } + last_entry = 0x00040004; } else { q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; if (ticks == 0xffff) { @@ -132,10 +146,10 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, if (steps < PART_SIZE / 2) { for (uint8_t i = 1; i < steps; i++) { // steps-1 iterations - *data++ = 0x40017fff | 0x8000; + *data++ = 0x40007fff | 0x8000; *data++ = 0x20002000; } - *data++ = 0x40017fff | 0x8000; + *data++ = 0x40007fff | 0x8000; uint16_t delta = PART_SIZE - 2 * steps; delta <<= 5; *data++ = 0x20002000 - delta; @@ -149,10 +163,10 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, } else { steps -= PART_SIZE / 2; for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - *data++ = 0x40017fff | 0x8000; + *data++ = 0x40007fff | 0x8000; *data++ = 0x20002000; } - *data++ = 0x40017fff | 0x8000; + *data++ = 0x40007fff | 0x8000; last_entry = 0x20002000; } } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { @@ -169,13 +183,13 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, for (uint8_t i = 1; i < steps_to_do; i++) { *data++ = rmt_entry; } - uint32_t delta = PART_SIZE - steps_to_do; - delta <<= 18; // shift in upper 16bit and multiply with 4 + uint32_t delta = (PART_SIZE - steps_to_do) * 4 + 8; + delta <<= 16; // shift in upper 16bit *data++ = rmt_entry - delta; for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { *data++ = 0x00020002; } - last_entry = 0x00020002; + last_entry = 0x00040004; steps -= steps_to_do; } else { // either >= 2*PART_SIZE or = PART_SIZE @@ -192,15 +206,16 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, steps -= PART_SIZE; } } - if (!fill_part_one) { + // No tick lost mentioned for esp32c3 + // if (!fill_part_one) { // Note: When enabling the continuous transmission mode by setting // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the // channel continuously, that is, from the first byte to the last one, // then from the first to the last again, and so on. In this mode, there // will be an idle level lasting one clk_div cycle between N and N+1 // transmissions. - last_entry -= 1; - } + // last_entry -= 1; + // } *data = last_entry; // Data is complete @@ -238,41 +253,49 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ if (old_limit == PART_SIZE + 1) { \ - /* first half of buffer sent */ \ + /* second half of buffer sent */ \ PROBE_2_TOGGLE; \ + /* demonstrate modification of RAM */ \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + mem[PART_SIZE] = 0x33ff33ff; \ RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ } else { \ - /* second half of buffer sent */ \ + /* first half of buffer sent */ \ PROBE_3_TOGGLE; \ RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ } \ - /* */ \ RMT.tx_conf[ch].conf_update = 1; \ RMT.tx_conf[ch].conf_update = 0; \ } #else #define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_2_TOGGLE; \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - if (q->_rmtStopped) { \ - rmt_set_tx_intr_en(q->channel, false); \ - rmt_set_tx_thr_intr_en(q->channel, false, PART_SIZE + 1); \ - q->_isRunning = false; \ - PROBE_1_TOGGLE; \ - } else { \ - apply_command(q, false, FAS_RMT_MEM(ch)); \ - } \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - PROBE_3_TOGGLE; \ + disable_rmt_interrupts(q->channel); \ + q->_isRunning = false; \ + PROBE_1_TOGGLE; \ + PROBE_2_TOGGLE; \ + PROBE_3_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - if (!q->_rmtStopped) { \ - apply_command(q, true, FAS_RMT_MEM(ch)); \ - /* now repeat the interrupt at buffer size + end marker */ \ - RMT.tx_conf[ch].conf_update = 1; \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ - } \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + apply_command(q, false, mem); \ + /* demonstrate modification of RAM */ \ + /*mem[PART_SIZE] = 0x33fff3ff; */ \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + apply_command(q, true, mem); \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 0; \ } #endif @@ -326,9 +349,22 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { if (channel_num == 0) { periph_module_enable(PERIPH_RMT_MODULE); } - // 80 MHz/5 = 16 MHz - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); + // APB_CLOCK=80 MHz + // CLK_DIV = APB_CLOCK/5 = 16 MHz + // + // Relation 1 in esp32c3 technical reference: + // 3 * T_APB + 5 * T_RMT_CLK < period * T_CLK_DIV + // => 8 * T_APB < period * T_APB*5 + // => period > 8/5 + // => period >= 2 + // + // Relation 2 in esp32c3 technical reference before end marker: + // 6 * T_APB + 12 * T_RMT_CLK < period * T_CLK_DIV + // => 18 * T_APB < period * T_APB*5 + // => period > 18/5 + // => period >= 4 + // + disable_rmt_interrupts(channel); rmt_set_source_clk(channel, RMT_BASECLK_APB); rmt_set_clk_div(channel, 5); rmt_set_mem_block_num(channel, 1); @@ -366,19 +402,13 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { *mem++ = 0; // conti mode is accepted with the conf_update 1 strobe - // but still do not see interrupts RMT.tx_conf[channel].tx_conti_mode = 1; RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].conf_update = 0; - rmt_set_tx_intr_en(channel, true); - // In order to avoid threshold/end interrupt on end, add one - rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 2); - // intr enable needs conf_update strobe - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; RMT.tx_conf[channel].mem_tx_wrap_en = 0; RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].conf_update = 0; + enable_rmt_interrupts(channel); // tx_start does not need conf_update PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 RMT.tx_conf[channel].tx_start = 1; @@ -402,13 +432,10 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { // and this seems to avoid some pitfalls // This runs the RMT buffer once - RMT.tx_conf[channel].tx_conti_mode = 1; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; - delay(1); RMT.tx_conf[channel].tx_conti_mode = 0; RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].tx_start = 1; while (true) { delay(1000); PROBE_1_TOGGLE; @@ -418,8 +445,6 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { } void StepperQueue::connect_rmt() { - // rmt_set_tx_intr_en(channel, true); - // rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); RMT.tx_conf[channel].idle_out_lv = 0; RMT.tx_conf[channel].idle_out_en = 1; RMT.tx_conf[channel].conf_update = 1; @@ -470,15 +495,15 @@ void StepperQueue::connect_rmt() { } void StepperQueue::disconnect_rmt() { - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); + disable_rmt_interrupts(channel); #ifndef __ESP32_IDF_V44__ // rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1); #else // rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); #endif - RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].idle_out_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; } void StepperQueue::startQueue_rmt() { @@ -511,30 +536,28 @@ void StepperQueue::startQueue_rmt() { #endif rmt_tx_stop(channel); // rmt_rx_stop(channel); - rmt_memory_rw_rst(channel); - // the following assignment should not be needed; - // RMT.data_ch[channel] = 0; + RMT.tx_conf[channel].mem_rd_rst = 1; + RMT.tx_conf[channel].mem_rd_rst = 0; uint32_t *mem = FAS_RMT_MEM(channel); +//#define TRACE +#ifdef TRACE // Fill the buffer with a significant pattern for debugging // Keep it for now for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { - mem[i] = 0x0fff8fff; - mem[i + 1] = 0x7fff8fff; +// mem[i] = 0x0fff8fff; +// mem[i + 1] = 0x7fff8fff; } +#endif + // Write end marker mem[2 * PART_SIZE] = 0; mem[2 * PART_SIZE + 1] = 0; _isRunning = true; _rmtStopped = false; - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, 0); - RMT.tx_conf[channel].conf_update = 1; + disable_rmt_interrupts(channel); RMT.tx_conf[channel].mem_tx_wrap_en = 0; - RMT.tx_conf[channel].mem_rd_rst = 1; - RMT.tx_conf[channel].mem_rd_rst = 0; + RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].conf_update = 0; - // RMT.apb_conf.mem_tx_wrap_en = 0; -//#define TRACE #ifdef TRACE USBSerial.print("Queue:"); USBSerial.print(read_idx); @@ -592,9 +615,7 @@ void StepperQueue::startQueue_rmt() { USBSerial.println(mem[i], HEX); } #endif - rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); - rmt_set_tx_intr_en(channel, true); - _rmtStopped = false; + enable_rmt_interrupts(channel); #ifdef TRACE USBSerial.print("Interrupt enable:"); @@ -606,9 +627,14 @@ void StepperQueue::startQueue_rmt() { #endif // This starts the rmt module - RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].tx_conti_mode = 1; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + PROBE_1_TOGGLE; RMT.tx_conf[channel].tx_start = 1; } void StepperQueue::forceStop_rmt() { diff --git a/src/test_probe.h b/src/test_probe.h index 23f24cd9..5ccbfab2 100644 --- a/src/test_probe.h +++ b/src/test_probe.h @@ -5,20 +5,20 @@ // code to investigate rmt module functioning //#define ESP32_TEST_PROBE - -#ifdef ESP32_TEST_PROBE +//#define ESP32C3_TEST_PROBE // in rmt: // TEST_PROBE_1 on startQueue and queue stop, with double toggle at // startQueue TEST_PROBE_2 end interrupt, when rmt transmission hits buffer // end TEST_PROBE_3 threshold interrupt, after first buffer half transmission // is complete + +#ifdef ESP32_TEST_PROBE #define TEST_PROBE_1 18 #define TEST_PROBE_2 5 #define TEST_PROBE_3 4 #endif -#define ESP32C3_TEST_PROBE #ifdef ESP32C3_TEST_PROBE #define TEST_PROBE_1 1 #define TEST_PROBE_2 2 From 099084c2ff1c26a9a7a387a76eda626052b7334f Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 18:03:15 +0200 Subject: [PATCH 09/85] disable trace --- src/StepperISR_esp32c3_rmt.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp index 105b9c52..ebb0df9b 100644 --- a/src/StepperISR_esp32c3_rmt.cpp +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -2,6 +2,7 @@ #ifdef SUPPORT_ESP32C3_RMT //#define TEST_MODE +//#define TRACE #include "test_probe.h" @@ -507,7 +508,6 @@ void StepperQueue::disconnect_rmt() { } void StepperQueue::startQueue_rmt() { -#define TRACE #ifdef TRACE USBSerial.println("START"); #endif From dfad0ddb3f923bb5c4ec4d20bb7948b9efa5159d Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 18:39:07 +0200 Subject: [PATCH 10/85] adjust test expectations --- README.md | 10 +++++++--- examples/StepperDemo/StepperDemo.ino | 1 - .../test_sd_04_timing_2560/expect.txt | 18 +++++++++--------- .../test_sd_04_timing_328p/expect.txt | 12 ++++++------ .../test_sd_04_timing_328p_37k/expect.txt | 6 +++--- .../simavr_based/test_sd_11_328p/expect.txt | 6 +++--- .../simavr_based/test_sd_15_328p/expect.txt | 8 ++++---- .../simavr_based/test_sd_16_328p/expect.txt | 8 ++++---- .../simavr_based/test_sd_17_328p/expect.txt | 8 ++++---- .../simavr_based/test_sd_18_328p/expect.txt | 4 ++-- .../test_sd_18_328p/platformio.ini | 2 +- 11 files changed, 43 insertions(+), 40 deletions(-) diff --git a/README.md b/README.md index 8995d86c..fc6584e5 100644 --- a/README.md +++ b/README.md @@ -288,11 +288,15 @@ This stepper driver uses rmt module. ### ESP32S3 -This stepper driver uses mcpwm/pcnt modules. Can drive up to 4 motors. Tested with 2 motors. +This stepper driver uses mcpwm/pcnt modules. Can drive up to 4 motors. Tested with 2 motors (not by me). -### ESP32C3/ESP32-MINI-1 +Apparently the ESP32S3's rmt module is similar to esp32c3 with 4 instead of 2 channels. Theoretically can drive 4 more steppers. -Compatibility with ESP32-C3: Not supported currently. The rmt module has more changes compared to esp32/esp32s2 +### ESP32C3 + +This stepper driver uses rmt module and can drive up to 2 motors. Not thoroughly tested, so only experimental support. + +### ESP32-MINI-1 Compatibility with ESP32-MINI-1: At least mcpwm and pulse counter modules are listed in the datasheet. So there are chances, that this lib works. diff --git a/examples/StepperDemo/StepperDemo.ino b/examples/StepperDemo/StepperDemo.ino index e0d661e9..6e94a14b 100644 --- a/examples/StepperDemo/StepperDemo.ino +++ b/examples/StepperDemo/StepperDemo.ino @@ -908,7 +908,6 @@ void test_direct_drive(const struct stepper_config_s *stepper) { void setup() { SerialInterface.begin(115200); - delay(2000); output_msg(MSG_STEPPER_VERSION); SerialInterface.print(" F_CPU="); SerialInterface.println(F_CPU); diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 1d84d735..5a254a05 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=576437us - StepB: 64000*L->H, 64000*H->L, Max High=30us Total High=393762us - StepC: 64000*L->H, 64000*H->L, Max High=37us Total High=429542us + StepA: 64000*L->H, 64000*H->L, Max High=24us Total High=391242us + StepB: 64000*L->H, 64000*H->L, Max High=33us Total High=394815us + StepC: 64000*L->H, 64000*H->L, Max High=34us Total High=477571us Position[A]=64000 Position[B]=64000 @@ -15,15 +15,15 @@ Position[C]=64000 Time in EnableA max=233598 us, total=233598 us -Time in EnableB max=246081 us, total=246081 us +Time in EnableB max=246086 us, total=246086 us -Time in EnableC max=254229 us, total=254229 us +Time in EnableC max=254235 us, total=254235 us -Time in FillISR max=2697 us, total=2091083 us +Time in FillISR max=3140 us, total=2095013 us -Time in StepA max=28 us, total=576437 us +Time in StepA max=24 us, total=391242 us -Time in StepB max=30 us, total=393762 us +Time in StepB max=33 us, total=394815 us -Time in StepC max=37 us, total=429542 us +Time in StepC max=34 us, total=477571 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 2aa1c005..29b2cd5e 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,21 +2,21 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5618us - StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5419us + StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=5548us + StepB: 1000*L->H, 1000*H->L, Max High=18us Total High=5547us Position[A]=1000 Position[B]=1000 Time in EnableA max=225398 us, total=225398 us -Time in EnableB max=238110 us, total=238110 us +Time in EnableB max=238116 us, total=238116 us -Time in FillISR max=2638 us, total=47569 us +Time in FillISR max=2644 us, total=47713 us -Time in StepA max=14 us, total=5618 us +Time in StepA max=13 us, total=5548 us -Time in StepB max=16 us, total=5419 us +Time in StepB max=18 us, total=5547 us Time in StepISR max=7 us, total=9224 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index ac107712..0e6562ea 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5286us + StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5281us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2015 us, total=27518 us +Time in FillISR max=2015 us, total=27776 us -Time in StepA max=11 us, total=5286 us +Time in StepA max=11 us, total=5281 us Time in StepISR max=7 us, total=4581 us diff --git a/extras/tests/simavr_based/test_sd_11_328p/expect.txt b/extras/tests/simavr_based/test_sd_11_328p/expect.txt index fdf0457e..899e1bf5 100644 --- a/extras/tests/simavr_based/test_sd_11_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_11_328p/expect.txt @@ -2,11 +2,11 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 99808*L->H, 99808*H->L, Max High=13us Total High=514674us + StepA: 99808*L->H, 99808*H->L, Max High=13us Total High=513811us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us -Position[A]=50536 +Position[A]=50736 Time in EnableA max=225395 us, total=225395 us -Time in StepA max=13 us, total=514674 us +Time in StepA max=13 us, total=513811 us diff --git a/extras/tests/simavr_based/test_sd_15_328p/expect.txt b/extras/tests/simavr_based/test_sd_15_328p/expect.txt index 4acdc895..ecfe2309 100644 --- a/extras/tests/simavr_based/test_sd_15_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_15_328p/expect.txt @@ -2,11 +2,11 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 3434*L->H, 3434*H->L, Max High=12us Total High=17813us + StepA: 3433*L->H, 3433*H->L, Max High=12us Total High=17723us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us -Position[A]=-1034 +Position[A]=-1033 -Time in EnableA max=225394 us, total=225394 us +Time in EnableA max=225396 us, total=225396 us -Time in StepA max=12 us, total=17813 us +Time in StepA max=12 us, total=17723 us diff --git a/extras/tests/simavr_based/test_sd_16_328p/expect.txt b/extras/tests/simavr_based/test_sd_16_328p/expect.txt index 63fb6cd1..5f130eb4 100644 --- a/extras/tests/simavr_based/test_sd_16_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_16_328p/expect.txt @@ -2,11 +2,11 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1122*L->H, 1122*H->L, Max High=11us Total High=5775us + StepA: 1123*L->H, 1123*H->L, Max High=12us Total High=5797us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us -Position[A]=1122 +Position[A]=1123 -Time in EnableA max=225395 us, total=225395 us +Time in EnableA max=225397 us, total=225397 us -Time in StepA max=11 us, total=5775 us +Time in StepA max=12 us, total=5797 us diff --git a/extras/tests/simavr_based/test_sd_17_328p/expect.txt b/extras/tests/simavr_based/test_sd_17_328p/expect.txt index 7ff79eff..ba5f522e 100644 --- a/extras/tests/simavr_based/test_sd_17_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_17_328p/expect.txt @@ -2,11 +2,11 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1431*L->H, 1431*H->L, Max High=12us Total High=7390us + StepA: 1430*L->H, 1430*H->L, Max High=11us Total High=7398us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us -Position[A]=1431 +Position[A]=1430 -Time in EnableA max=229524 us, total=229524 us +Time in EnableA max=229523 us, total=229523 us -Time in StepA max=12 us, total=7390 us +Time in StepA max=11 us, total=7398 us diff --git a/extras/tests/simavr_based/test_sd_18_328p/expect.txt b/extras/tests/simavr_based/test_sd_18_328p/expect.txt index 9b9f3401..f3b55c91 100644 --- a/extras/tests/simavr_based/test_sd_18_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_18_328p/expect.txt @@ -2,11 +2,11 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 3866*L->H, 3866*H->L, Max High=12us Total High=19824us + StepA: 3870*L->H, 3870*H->L, Max High=12us Total High=19922us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=0 Time in EnableA max=229523 us, total=229523 us -Time in StepA max=12 us, total=19824 us +Time in StepA max=12 us, total=19922 us diff --git a/extras/tests/simavr_based/test_sd_18_328p/platformio.ini b/extras/tests/simavr_based/test_sd_18_328p/platformio.ini index f6723879..d940baed 100644 --- a/extras/tests/simavr_based/test_sd_18_328p/platformio.ini +++ b/extras/tests/simavr_based/test_sd_18_328p/platformio.ini @@ -21,7 +21,7 @@ # [common] # This is the line input to StepperDemo: -build_flags = -D SIM_TEST_INPUT='"Q M1 A10 V100 j100000 f w1000 A100000 U X w100 R-1431 W "' +build_flags = -D SIM_TEST_INPUT='"Q M1 A10 V100 j100000 f w1000 A100000 U X w100 R-1433 W "' # The -1431 moves the stepper after the force stop, so that the visible position is 0. # Visible means: counted by watching the external step/direction signals. From b8eeea96ed9eee575ea36ed40f316866f38e2405 Mon Sep 17 00:00:00 2001 From: Ben Turley Date: Sat, 14 Oct 2023 11:20:03 -0600 Subject: [PATCH 11/85] Initialize _stepper_cnt to 0 --- src/FastAccelStepper.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index 93dc5c5a..c5db0513 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -20,6 +20,7 @@ FastAccelStepper fas_stepper[MAX_STEPPER]; //************************************************************************************************* void FastAccelStepperEngine::init() { _externalCallForPin = NULL; + _stepper_cnt = 0; fas_init_engine(this, 255); for (uint8_t i = 0; i < MAX_STEPPER; i++) { _stepper[i] = NULL; @@ -29,6 +30,7 @@ void FastAccelStepperEngine::init() { #if defined(SUPPORT_CPU_AFFINITY) void FastAccelStepperEngine::init(uint8_t cpu_core) { _externalCallForPin = NULL; + _stepper_cnt = 0; fas_init_engine(this, cpu_core); } #endif From 2f2727e40ca7e339448e10f549f8575af93c8c4b Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 19:43:36 +0200 Subject: [PATCH 12/85] adjust test expectation and add esp32c3 to automatic build --- examples/StepperDemo/generic.h | 3 ++- .../test_sd_04_timing_2560/expect.txt | 16 ++++++++-------- .../test_sd_04_timing_328p/expect.txt | 10 +++++----- .../test_sd_04_timing_328p_37k/expect.txt | 6 +++--- .../simavr_based/test_sd_11_328p/expect.txt | 6 +++--- src/StepperISR_esp32c3_rmt.cpp | 10 +++++----- 6 files changed, 26 insertions(+), 25 deletions(-) diff --git a/examples/StepperDemo/generic.h b/examples/StepperDemo/generic.h index c9e9873a..aebe9916 100644 --- a/examples/StepperDemo/generic.h +++ b/examples/StepperDemo/generic.h @@ -11,7 +11,8 @@ #error "Unsupported derivate" #endif -#if defined(CONFIG_IDF_TARGET_ESP32C3) +#if defined(CONFIG_TINYUSB_CDC_ENABLED) +USBCDC USBSerial; #define SerialInterface USBSerial #else #define SerialInterface Serial diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 5a254a05..a618ef3b 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=24us Total High=391242us - StepB: 64000*L->H, 64000*H->L, Max High=33us Total High=394815us - StepC: 64000*L->H, 64000*H->L, Max High=34us Total High=477571us + StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=521130us + StepB: 64000*L->H, 64000*H->L, Max High=36us Total High=393609us + StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=485360us Position[A]=64000 Position[B]=64000 @@ -15,15 +15,15 @@ Position[C]=64000 Time in EnableA max=233598 us, total=233598 us -Time in EnableB max=246086 us, total=246086 us +Time in EnableB max=246080 us, total=246080 us Time in EnableC max=254235 us, total=254235 us -Time in FillISR max=3140 us, total=2095013 us +Time in FillISR max=2686 us, total=2092886 us -Time in StepA max=24 us, total=391242 us +Time in StepA max=28 us, total=521130 us -Time in StepB max=33 us, total=394815 us +Time in StepB max=36 us, total=393609 us -Time in StepC max=34 us, total=477571 us +Time in StepC max=40 us, total=485360 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 29b2cd5e..60f6430d 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,8 +2,8 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=5548us - StepB: 1000*L->H, 1000*H->L, Max High=18us Total High=5547us + StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=5545us + StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5514us Position[A]=1000 Position[B]=1000 @@ -12,11 +12,11 @@ Time in EnableA max=225398 us, total=225398 us Time in EnableB max=238116 us, total=238116 us -Time in FillISR max=2644 us, total=47713 us +Time in FillISR max=2644 us, total=47660 us -Time in StepA max=13 us, total=5548 us +Time in StepA max=13 us, total=5545 us -Time in StepB max=18 us, total=5547 us +Time in StepB max=16 us, total=5514 us Time in StepISR max=7 us, total=9224 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index 0e6562ea..d6995ab1 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5281us + StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5268us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2015 us, total=27776 us +Time in FillISR max=2015 us, total=27549 us -Time in StepA max=11 us, total=5281 us +Time in StepA max=11 us, total=5268 us Time in StepISR max=7 us, total=4581 us diff --git a/extras/tests/simavr_based/test_sd_11_328p/expect.txt b/extras/tests/simavr_based/test_sd_11_328p/expect.txt index 899e1bf5..4046871a 100644 --- a/extras/tests/simavr_based/test_sd_11_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_11_328p/expect.txt @@ -2,11 +2,11 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 99808*L->H, 99808*H->L, Max High=13us Total High=513811us + StepA: 99808*L->H, 99808*H->L, Max High=13us Total High=514436us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us -Position[A]=50736 +Position[A]=50536 Time in EnableA max=225395 us, total=225395 us -Time in StepA max=13 us, total=513811 us +Time in StepA max=13 us, total=514436 us diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp index ebb0df9b..655db63e 100644 --- a/src/StepperISR_esp32c3_rmt.cpp +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -28,11 +28,11 @@ // #ifdef SUPPORT_ESP32C3_RMT #define PART_SIZE 23 -#define MEM_SIZE 48 +#define RMT_MEM_SIZE 48 #else #error #define PART_SIZE 31 -#define MEM_SIZE 64 +#define RMT_MEM_SIZE 64 #endif // In order to avoid threshold/end interrupt on end, add one @@ -563,7 +563,7 @@ void StepperQueue::startQueue_rmt() { USBSerial.print(read_idx); USBSerial.print('/'); USBSerial.println(next_write_idx); - for (uint8_t i = 0; i < MEM_SIZE; i++) { + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { USBSerial.print(i); USBSerial.print(' '); USBSerial.println(mem[i], HEX); @@ -591,7 +591,7 @@ void StepperQueue::startQueue_rmt() { USBSerial.println(' '); USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en); USBSerial.println(' '); - for (uint8_t i = 0; i < MEM_SIZE; i++) { + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { USBSerial.print(i); USBSerial.print(' '); USBSerial.println(mem[i], HEX); @@ -609,7 +609,7 @@ void StepperQueue::startQueue_rmt() { USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en); USBSerial.println(' '); - for (uint8_t i = 0; i < MEM_SIZE; i++) { + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { USBSerial.print(i); USBSerial.print(' '); USBSerial.println(mem[i], HEX); From cd6b9367d98947f81d44dd3c295ca8e60de5075e Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 19:58:51 +0200 Subject: [PATCH 13/85] finalize patch #204 --- README.md | 1 + .../simavr_based/test_sd_04_timing_2560/expect.txt | 14 +++++++------- .../simavr_based/test_sd_04_timing_328p/expect.txt | 8 ++++---- .../test_sd_04_timing_328p_37k/expect.txt | 4 ++-- 4 files changed, 14 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index fc6584e5..b85251f4 100644 --- a/README.md +++ b/README.md @@ -457,4 +457,5 @@ As mentioned by kthod861 in [Issue #110](https://github.com/gin66/FastAccelStepp - Thanks clazarowitz for the amazing atmel sam due port (https://github.com/gin66/FastAccelStepper/pull/82) - Thanks HeldeReis for the awesome ESP32-S3 port (https://github.com/gin66/FastAccelStepper/pull/162) - Thanks DaAwesomeP for the extension to ATmega 168/168P/328 (https://github.com/gin66/FastAccelStepper/pull/179) +- Thanks turley for the patch for missing `_stepper_cnt` initialization (https://github.com/gin66/FastAccelStepper/pull/204) diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index a618ef3b..b570d48b 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=521130us - StepB: 64000*L->H, 64000*H->L, Max High=36us Total High=393609us - StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=485360us + StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=521699us + StepB: 64000*L->H, 64000*H->L, Max High=35us Total High=393630us + StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=484932us Position[A]=64000 Position[B]=64000 @@ -19,11 +19,11 @@ Time in EnableB max=246080 us, total=246080 us Time in EnableC max=254235 us, total=254235 us -Time in FillISR max=2686 us, total=2092886 us +Time in FillISR max=2686 us, total=2092758 us -Time in StepA max=28 us, total=521130 us +Time in StepA max=28 us, total=521699 us -Time in StepB max=36 us, total=393609 us +Time in StepB max=35 us, total=393630 us -Time in StepC max=40 us, total=485360 us +Time in StepC max=40 us, total=484932 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 60f6430d..f976eb62 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,8 +2,8 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=5545us - StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5514us + StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=5539us + StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=5513us Position[A]=1000 Position[B]=1000 @@ -14,9 +14,9 @@ Time in EnableB max=238116 us, total=238116 us Time in FillISR max=2644 us, total=47660 us -Time in StepA max=13 us, total=5545 us +Time in StepA max=13 us, total=5539 us -Time in StepB max=16 us, total=5514 us +Time in StepB max=15 us, total=5513 us Time in StepISR max=7 us, total=9224 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index d6995ab1..9e754e06 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,7 +2,7 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5268us + StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5276us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 @@ -10,7 +10,7 @@ Time in EnableA max=225399 us, total=225399 us Time in FillISR max=2015 us, total=27549 us -Time in StepA max=11 us, total=5268 us +Time in StepA max=11 us, total=5276 us Time in StepISR max=7 us, total=4581 us From 15f2bbb683612fd84c32b1a5759e38e4d559fc1c Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 20:09:42 +0200 Subject: [PATCH 14/85] fix esp32s2/s3 compile error due to USBCDC --- examples/StepperDemo/generic.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/StepperDemo/generic.h b/examples/StepperDemo/generic.h index aebe9916..ffd052c5 100644 --- a/examples/StepperDemo/generic.h +++ b/examples/StepperDemo/generic.h @@ -11,7 +11,7 @@ #error "Unsupported derivate" #endif -#if defined(CONFIG_TINYUSB_CDC_ENABLED) +#if defined(CONFIG_IDF_TARGET_ESP32C3) && defined(CONFIG_TINYUSB_CDC_ENABLED) && (CONFIG_TINYUSB_CDC_ENABLED == 1) USBCDC USBSerial; #define SerialInterface USBSerial #else From 1d320ee68e3ae3b38ea9725c0d533046cee6f0df Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 20:26:20 +0200 Subject: [PATCH 15/85] USBSerial is quite bad on platformio --- examples/StepperDemo/generic.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/StepperDemo/generic.h b/examples/StepperDemo/generic.h index ffd052c5..241804f8 100644 --- a/examples/StepperDemo/generic.h +++ b/examples/StepperDemo/generic.h @@ -1,6 +1,8 @@ #ifndef GENERIC_H #define GENERIC_H +#include + #if defined(ARDUINO_ARCH_AVR) #define get_char(x) pgm_read_byte(x) #define MSG_TYPE PGM_P @@ -11,8 +13,7 @@ #error "Unsupported derivate" #endif -#if defined(CONFIG_IDF_TARGET_ESP32C3) && defined(CONFIG_TINYUSB_CDC_ENABLED) && (CONFIG_TINYUSB_CDC_ENABLED == 1) -USBCDC USBSerial; +#if defined(CONFIG_IDF_TARGET_ESP32C3) && (ARDUINO_USB_MODE==1) #define SerialInterface USBSerial #else #define SerialInterface Serial From 8d5e24e10254be80a1388f4b27d89b593cae82d5 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 20:37:56 +0200 Subject: [PATCH 16/85] release 0.30.6 --- CHANGELOG.md | 5 +++-- README.md | 3 ++- examples/StepperDemo/StepperDemo.ino | 6 ++++++ library.properties | 2 +- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 434eaafb..84524164 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,8 +6,9 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.6: -- Partly tested support for ESP32C3 +0.30.6: +- Support for ESP32C3 +- Fix for missing `_stepper_cnt` initialization (patch #204) 0.30.5: - Fix target position for a move() interrupting the keep running mode diff --git a/README.md b/README.md index b85251f4..b8a03323 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# BE AWARE: ARDUINO LIBRARY MANAGER IS BROKEN AND 0.29.x do not show. +# BE AWARE: ARDUINO LIBRARY MANAGER IS BROKEN AND 0.29.x/0.30.x with x>1 do not show. No issue with platformio. Check the [related issue](https://github.com/arduino/library-registry/issues/2829) for the arduino library manager @@ -435,6 +435,7 @@ This feature of StepperDemo allows to compare non-smooth running stepper in an a * In one setup, operating A4988 without microsteps has led to erratic behaviour at some specific low speed (erratic means step forward/backward, while DIR is kept low). No issue with 16 microstep. These two youtube videos show similar behavior: [hard disc stepper](https://youtu.be/DsYgw3GFHZo) and [axes movement](https://youtu.be/Nw18B81Ylhk) * The pulse counters in esp32 have several comparators to trigger interrupts. What the documentation does not mention: All those reference values are only forwarded to the actual comparator on pulse counter reset. Thus the pulse counters cannot be used as lower 16bit of the position, unfortunately. * The [issue #60](https://github.com/gin66/FastAccelStepper/issues/60) was raised due to wrong position on negative moves with esp32. Apparently the issue was with proper ground and/or power lines to the stepper driver. If similar issue is encountered, please check on this issue +* ESP32C3: USBSerial works only under Arduino IDE. platformio support for USBSerial is missing ## 3rd party videos in action diff --git a/examples/StepperDemo/StepperDemo.ino b/examples/StepperDemo/StepperDemo.ino index 6e94a14b..e23e811a 100644 --- a/examples/StepperDemo/StepperDemo.ino +++ b/examples/StepperDemo/StepperDemo.ino @@ -908,6 +908,12 @@ void test_direct_drive(const struct stepper_config_s *stepper) { void setup() { SerialInterface.begin(115200); +#ifdef CONFIG_IDF_TARGET_ESP32C3 + while (!USBSerial) { + ; // wait for USB serial port to connect. + } +#endif + output_msg(MSG_STEPPER_VERSION); SerialInterface.print(" F_CPU="); SerialInterface.println(F_CPU); diff --git a/library.properties b/library.properties index 617a1ae9..7e2c424d 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.5 +version=0.30.6 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes From 84d4bed4b6d4aa150545a477ed285c074ee4a97a Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 20:40:50 +0200 Subject: [PATCH 17/85] update README --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b8a03323..30efde16 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# BE AWARE: ARDUINO LIBRARY MANAGER IS BROKEN AND 0.29.x/0.30.x with x>1 do not show. +# BE AWARE: ARDUINO LIBRARY MANAGER IS BROKEN AND 0.29.x/0.30.x with x>0 do not show. No issue with platformio. Check the [related issue](https://github.com/arduino/library-registry/issues/2829) for the arduino library manager From c9bd34351456b558849273e61d911cadde527ee6 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sat, 14 Oct 2023 21:09:45 +0200 Subject: [PATCH 18/85] add missing esp32c3 workflow --- .github/workflows/build_examples_esp32c3.yml | 21 ++++++++++++++++++++ examples/StepperDemo/StepperDemo.ino | 2 +- 2 files changed, 22 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/build_examples_esp32c3.yml diff --git a/.github/workflows/build_examples_esp32c3.yml b/.github/workflows/build_examples_esp32c3.yml new file mode 100644 index 00000000..49e1011a --- /dev/null +++ b/.github/workflows/build_examples_esp32c3.yml @@ -0,0 +1,21 @@ +name: Build examples for esp32c3arduino @ latest + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32c3 + diff --git a/examples/StepperDemo/StepperDemo.ino b/examples/StepperDemo/StepperDemo.ino index e23e811a..c2f9375d 100644 --- a/examples/StepperDemo/StepperDemo.ino +++ b/examples/StepperDemo/StepperDemo.ino @@ -909,7 +909,7 @@ void test_direct_drive(const struct stepper_config_s *stepper) { void setup() { SerialInterface.begin(115200); #ifdef CONFIG_IDF_TARGET_ESP32C3 - while (!USBSerial) { + while (!Serial) { ; // wait for USB serial port to connect. } #endif From 95a24aab78206a729b48df04eaa8322fdc762ade Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Mon, 20 Nov 2023 22:24:14 +0100 Subject: [PATCH 19/85] add failing simavr test for issue #208 --- examples/Issue208/Issue208.ino | 123 ++++++++++++++++ .../simavr_based/test_issue208/.gitignore | 1 + .../tests/simavr_based/test_issue208/Makefile | 132 ++++++++++++++++++ .../simavr_based/test_issue208/expect.txt | 20 +++ .../simavr_based/test_issue208/platformio.ini | 31 ++++ 5 files changed, 307 insertions(+) create mode 100644 examples/Issue208/Issue208.ino create mode 100644 extras/tests/simavr_based/test_issue208/.gitignore create mode 100644 extras/tests/simavr_based/test_issue208/Makefile create mode 100644 extras/tests/simavr_based/test_issue208/expect.txt create mode 100644 extras/tests/simavr_based/test_issue208/platformio.ini diff --git a/examples/Issue208/Issue208.ino b/examples/Issue208/Issue208.ino new file mode 100644 index 00000000..95593427 --- /dev/null +++ b/examples/Issue208/Issue208.ino @@ -0,0 +1,123 @@ +#include "FastAccelStepper.h" +#include +#ifdef SIMULATOR +#include +#endif + +#if defined(ARDUINO_ARCH_AVR) +#include "AVRStepperPins.h" +#define dirPinStepperAVR 5 +#define stepPinStepperAVR stepPinStepperA +#define enablePinStepperAVR 6 +#elif defined(ARDUINO_ARCH_ESP32) +#define dirPinStepperESP 18 +#define stepPinStepperESP 17 +#define enablePinStepperESP 26 +#endif + +FastAccelStepperEngine engine = FastAccelStepperEngine(); +FastAccelStepper *stepper; + +bool test() { + int32_t errCnt = 0; + Serial.println("Start test: speed up"); + int32_t last_v_mHz = 0; + stepper->setAcceleration(10000); + stepper->setSpeedInHz(36800); + stepper->runForward(); + + while(stepper->rampState() != (RAMP_STATE_COAST | RAMP_DIRECTION_COUNT_UP)) { + int32_t v_mHz = stepper->getCurrentSpeedInMilliHz(); + if (v_mHz == last_v_mHz) { + continue; + } + Serial.println(v_mHz); + if (v_mHz < last_v_mHz) { + Serial.print("FAIL: last="); + Serial.print(last_v_mHz); + Serial.print(" new="); + Serial.println(v_mHz); + errCnt++; + // return false; + } + last_v_mHz = v_mHz; + } + + Serial.println("Reverse"); + + stepper->runBackward(); + + while(stepper->rampState() != (RAMP_STATE_COAST | RAMP_DIRECTION_COUNT_DOWN)) { + int32_t v_mHz = stepper->getCurrentSpeedInMilliHz(); + if (v_mHz == last_v_mHz) { + continue; + } + Serial.println(v_mHz); + if (v_mHz > last_v_mHz) { + Serial.print("FAIL: last="); + Serial.print(last_v_mHz); + Serial.print(" new="); + Serial.println(v_mHz); + errCnt++; + // return false; + } + last_v_mHz = v_mHz; + } + if (errCnt > 0) { + Serial.print("Errors="); + Serial.println(errCnt); + } + return errCnt != 0; +} + +void setup() { + Serial.begin(115200); + Serial.println("Init"); + engine.init(); + + // pins are set to outputs here automatically +#if defined(ARDUINO_ARCH_AVR) + Serial.println("AVR"); + Serial.println(stepPinStepperAVR); + stepper = engine.stepperConnectToPin(stepPinStepperAVR); + stepper->setDirectionPin(dirPinStepperAVR); +// stepper->setEnablePin(enablePinStepperAVR, true); +#elif defined(ARDUINO_ARCH_ESP32) + stepper = engine.stepperConnectToPin(stepPinStepperESP); + stepper->setDirectionPin(dirPinStepperESP); + stepper->setEnablePin(enablePinStepperESP, true); +#endif + + stepper->setAutoEnable(true); + Serial.println(stepper != NULL ? "OK" : "ERROR"); + Serial.println("Done"); + + bool pass = test(); + stepper->stopMove(); + +#ifdef SIMULATOR + // if result is Ok. Toggle port twice, otherwise once +#define PIN stepPinStepperB +#define DIRPIN 7 + pinMode(DIRPIN, OUTPUT); + digitalWrite(DIRPIN, HIGH); + pinMode(PIN, OUTPUT); + digitalWrite(PIN, HIGH); + digitalWrite(PIN, LOW); + if (pass) { + Serial.println("PASS"); + digitalWrite(PIN, HIGH); + digitalWrite(PIN, LOW); + } + else { + Serial.println("FAIL"); + } + delay(1000); + noInterrupts(); + sleep_cpu(); +#endif +} + + +void loop() { +} diff --git a/extras/tests/simavr_based/test_issue208/.gitignore b/extras/tests/simavr_based/test_issue208/.gitignore new file mode 100644 index 00000000..8eba6c8d --- /dev/null +++ b/extras/tests/simavr_based/test_issue208/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue208/Makefile b/extras/tests/simavr_based/test_issue208/Makefile new file mode 100644 index 00000000..d57f9fb6 --- /dev/null +++ b/extras/tests/simavr_based/test_issue208/Makefile @@ -0,0 +1,132 @@ +# +# In order to execute the test for one directory use: +# +# make -C test_sd_01b_328p -f ../Makefile.test + +SRC=$(wildcard ../../../src/*) $(wildcard src/*) + +# platformio should contain only one env section. +# This section states the dut name +# atmega168 +# atmega168p +# atmega328 +# atmega328p +# atmega2560_timer1 +# atmega2560_timer3 +# atmega2560_timer4 +# atmega2560_timer5 +# + +DUT=$(shell gawk '/env:/{print(substr($$1,6,length($$1)-6))}' platformio.ini) + +TRACES=-at StepISR=trace@0x25/0x08 # PB3 +TRACES+=-at FillISR=trace@0x25/0x10 # PB4 + +# +ifeq ($(DUT),atmega2560_timer1) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 ATMega2560 +TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 ATMega2560 +TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer3) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x02e/0x08 #OC3A PE3 5 ATMega2560 +TRACES+=-at StepB=trace@0x02e/0x10 #OC3B PE4 2 ATMega2560 +TRACES+=-at StepC=trace@0x02e/0x20 #OC3C PE5 3 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer4) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x102/0x08 #OC4A PH3 6 ATMega2560 +TRACES+=-at StepB=trace@0x102/0x10 #OC4B PH4 7 ATMega2560 +TRACES+=-at StepC=trace@0x102/0x20 #OC4C PH5 8 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer5) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x10b/0x08 #OC5A PL3 46 ATMega2560 +TRACES+=-at StepB=trace@0x10b/0x10 #OC5B PL4 45 ATMega2560 +TRACES+=-at StepC=trace@0x10b/0x20 #OC5C PL5 44 ATMega2560 + +else ifeq ($(DUT),atmega168) +DEVICE=atmega168 +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168 +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168 + +else ifeq ($(DUT),atmega168p) +DEVICE=atmega168p +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168p +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168p + +else ifeq ($(DUT),atmega328) +DEVICE=atmega328 +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328 +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328 + +else ifeq ($(DUT),atmega328p) +DEVICE=atmega328p +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328p +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328p + +else ifeq ($(DUT),atmega32u4) +DEVICE=atmega32u4 +TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 +TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 +#TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 + +endif + +ifeq ($(DEVICE),atmega2560) +TRACES+=-at DirA=trace@0x2b/0x01 # Pin 21 PD0 +TRACES+=-at DirB=trace@0x2b/0x02 # Pin 20 PD1 +TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 +TRACES+=-at EnableA=trace@0x2b/0x04 # Pin 19 PD2 +TRACES+=-at EnableB=trace@0x2b/0x08 # Pin 18 PD3 +TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 + +else ifeq ($(DEVICE),$(filter $(DEVICE),atmega168 atmega168p atmega328 atmega328p)) +TRACES+=-at DirA=trace@0x2b/0x20 # Pin 5 PD5 +TRACES+=-at DirB=trace@0x2b/0x80 # Pin 7 PD7 +TRACES+=-at EnableA=trace@0x2b/0x40 # Pin 6 PD6 +TRACES+=-at EnableB=trace@0x25/0x01 # Pin 8 PB0 + +else ifeq ($(DUT),atmega32u4) +TRACES+=-at DirA=trace@0x25/0x10 # Pin 26 PB4 +TRACES+=-at DirB=trace@0x25/0x08 # Pin 14 PB3 +#TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 +TRACES+=-at EnableA=trace@0x25/0x04 # Pin 16 PB2 +TRACES+=-at EnableB=trace@0x25/0x02 # Pin 15 PB1 +#TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 + +endif + +FIRMWARE=".pio/build/$(DUT)/firmware.elf" + +DIR=$(shell env pwd) + +ifndef SILENCE + SILENCE=0 +endif + +test: .tested + +.tested: result.txt expect.txt ../judge.awk + echo DUT=$(DUT) + rm -f .tested + gawk -f ../judge.awk -v DIR=$(DIR) result.txt expect.txt + test -f .tested + +result.txt: x.vcd + gawk -v SILENCE=$(SILENCE) -f ../eval.awk x.vcd >/dev/null + cat expect.txt + +x.vcd: $(SRC) ../run_avr platformio.ini src/Issue208.ino + ~/.platformio/penv/bin/pio run -e $(DUT) || ~/.local/bin/pio run -e $(DUT) || env pio run -e $(DUT) + ../run_avr $(FIRMWARE) -m $(DEVICE) -o x.vcd $(TRACES) + +src/Issue208.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue208/Issue208.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue208/expect.txt b/extras/tests/simavr_based/test_issue208/expect.txt new file mode 100644 index 00000000..8de30f0f --- /dev/null +++ b/extras/tests/simavr_based/test_issue208/expect.txt @@ -0,0 +1,20 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 3*L->H, 2*H->L + EnableB: 0*L->H, 0*H->L + StepA: 1048*L->H, 1048*H->L, Max High=10us Total High=5476us + StepB: 2*L->H, 2*H->L, Max High=4us Total High=9us +Position[A]=0 + +Position[B]=2 + +Time in EnableA max=4204 us, total=8408 us + +Time in FillISR max=1906 us, total=25682 us + +Time in StepA max=10 us, total=5476 us + +Time in StepB max=4 us, total=9 us + +Time in StepISR max=7 us, total=4875 us + diff --git a/extras/tests/simavr_based/test_issue208/platformio.ini b/extras/tests/simavr_based/test_issue208/platformio.ini new file mode 100644 index 00000000..9eae37d7 --- /dev/null +++ b/extras/tests/simavr_based/test_issue208/platformio.ini @@ -0,0 +1,31 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[platformio] + +# There should be only one env section for the DUT under test. +# One of +# atmega168p +# atmega328p +# atmega2560_timer1 +# atmega2560_timer3 +# atmega2560_timer4 +# atmega2560_timer5 +# +[common] +# This is the line input to StepperDemo: +build_flags = -D SIMULATOR -D SIMAVR_TIME_MEASUREMENT + +[env:atmega328p] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. From 4192bd59ba1ccce89f5ddae5adf8ff2502438b9d Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Mon, 20 Nov 2023 23:29:31 +0100 Subject: [PATCH 20/85] Fix for issue #208, but speed not monotonic during acceleration/deceleration --- CHANGELOG.md | 4 +++ examples/Issue208/Issue208.ino | 4 +++ src/FastAccelStepper.cpp | 46 ++++++++++++++++------------------ src/FastAccelStepper.h | 1 + src/RampGenerator.h | 7 ++++++ src/StepperISR.cpp | 18 +++++++------ src/StepperISR.h | 2 +- src/common.h | 5 ++++ 8 files changed, 55 insertions(+), 32 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 84524164..5557ee76 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,10 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works +pre-0.30.7: +- Fix for issue #208: close to speed reversal is the sign of current speed incorrect. + Still the speed increase during acceleration/deceleraion is not monotonic + 0.30.6: - Support for ESP32C3 - Fix for missing `_stepper_cnt` initialization (patch #204) diff --git a/examples/Issue208/Issue208.ino b/examples/Issue208/Issue208.ino index 95593427..92dc5ad2 100644 --- a/examples/Issue208/Issue208.ino +++ b/examples/Issue208/Issue208.ino @@ -42,6 +42,10 @@ bool test() { } last_v_mHz = v_mHz; } + // There can be still speed increases in the queue, even so the ramp generator + // is already coasting + delay(20); + last_v_mHz = stepper->getCurrentSpeedInMilliHz(); Serial.println("Reverse"); diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index c5db0513..f0b9a619 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -775,34 +775,32 @@ uint32_t FastAccelStepper::getPeriodInUsAfterCommandsCompleted() { } return 0; } -int32_t FastAccelStepper::getCurrentSpeedInUs() { - uint32_t ticks = fas_queue[_queue_num].getActualTicks(); - if (ticks == 0) { - ticks = getPeriodInTicksAfterCommandsCompleted(); +void FastAccelStepper::getCurrentSpeedInTicks(struct actual_ticks_s *speed) { + bool valid = fas_queue[_queue_num].getActualTicksWithDirection(speed); + if (!valid) { + if (_rg.isRampGeneratorActive()) { + _rg.getCurrentSpeedInTicks(speed); + } } - int32_t speed_in_us = ticks / (TICKS_PER_S / 1000000); - switch (_rg.rampState() & RAMP_DIRECTION_MASK) { - case RAMP_DIRECTION_COUNT_UP: - return speed_in_us; - case RAMP_DIRECTION_COUNT_DOWN: - return -speed_in_us; +} +int32_t FastAccelStepper::getCurrentSpeedInUs() { + struct actual_ticks_s speed; + getCurrentSpeedInTicks(&speed); + int32_t speed_in_us = speed.ticks / (TICKS_PER_S / 1000000); + if (speed.count_up) { + return speed_in_us; } - return 0; + return -speed_in_us; } int32_t FastAccelStepper::getCurrentSpeedInMilliHz() { - uint32_t ticks = fas_queue[_queue_num].getActualTicks(); - if (ticks == 0) { - ticks = getPeriodInTicksAfterCommandsCompleted(); - } - int32_t speed_in_milli_hz = 0; - if (ticks > 0) { - speed_in_milli_hz = ((uint32_t)250 * TICKS_PER_S) / ticks * 4; - } - switch (_rg.rampState() & RAMP_DIRECTION_MASK) { - case RAMP_DIRECTION_COUNT_UP: - return speed_in_milli_hz; - case RAMP_DIRECTION_COUNT_DOWN: - return -speed_in_milli_hz; + struct actual_ticks_s speed; + getCurrentSpeedInTicks(&speed); + if (speed.ticks > 0) { + int32_t speed_in_mhz = ((uint32_t)250 * TICKS_PER_S) / speed.ticks * 4; + if (speed.count_up) { + return speed_in_mhz; + } + return -speed_in_mhz; } return 0; } diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index 3997ed16..9b0cada2 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -646,6 +646,7 @@ class FastAccelStepper { bool needAutoDisable(); bool agreeWithAutoDisable(); bool usesAutoEnablePin(uint8_t pin); + void getCurrentSpeedInTicks(struct actual_ticks_s *speed); FastAccelStepperEngine* _engine; RampGenerator _rg; diff --git a/src/RampGenerator.h b/src/RampGenerator.h index 02a6b309..a31b5204 100644 --- a/src/RampGenerator.h +++ b/src/RampGenerator.h @@ -89,6 +89,13 @@ class RampGenerator { void getNextCommand(const struct queue_end_s *queue_end, NextCommand *cmd_out); void afterCommandEnqueued(NextCommand *cmd_in); + void getCurrentSpeedInTicks(struct actual_ticks_s *speed) { + fasDisableInterrupts(); + speed->ticks = _rw.curr_ticks; + uint8_t rs = _rw.rampState(); + fasEnableInterrupts(); + speed->count_up = ((rs & RAMP_DIRECTION_COUNT_UP) != 0); + } uint32_t getCurrentPeriodInTicks() { fasDisableInterrupts(); uint32_t ticks = _rw.curr_ticks; diff --git a/src/StepperISR.cpp b/src/StepperISR.cpp index dfd7f356..9ee7961b 100644 --- a/src/StepperISR.cpp +++ b/src/StepperISR.cpp @@ -281,28 +281,32 @@ bool StepperQueue::hasTicksInQueue(uint32_t min_ticks) { return false; } -uint16_t StepperQueue::getActualTicks() { - // Retrieve current step rate from the current view. - // This is valid only, if the command describes more than one step +bool StepperQueue::getActualTicksWithDirection(struct actual_ticks_s *speed) { + // Retrieve current step rate from the current command. + // This is valid only, if the command describes more than one step, + // or if the next command contains one step, too. fasDisableInterrupts(); uint8_t rp = read_idx; uint8_t wp = next_write_idx; fasEnableInterrupts(); if (wp == rp) { - return 0; + speed->ticks = 0; + return true; } struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK]; if (e->hasSteps) { + speed->count_up = e->countUp; + speed->ticks = e->ticks; if (e->moreThanOneStep) { - return e->ticks; + return true; } if (wp != ++rp) { if (entry[rp & QUEUE_LEN_MASK].hasSteps) { - return e->ticks; + return true; } } } - return 0; + return false; } void StepperQueue::_initVars() { diff --git a/src/StepperISR.h b/src/StepperISR.h index a00e61e3..5aeca752 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -139,7 +139,7 @@ class StepperQueue { int32_t getCurrentPosition(); uint32_t ticksInQueue(); bool hasTicksInQueue(uint32_t min_ticks); - uint16_t getActualTicks(); + bool getActualTicksWithDirection(struct actual_ticks_s *speed); volatile uint16_t getMaxSpeedInTicks() { return max_speed_in_ticks; } diff --git a/src/common.h b/src/common.h index 9d095196..f76f7a72 100644 --- a/src/common.h +++ b/src/common.h @@ -16,6 +16,11 @@ struct stepper_command_s { bool count_up; }; +struct actual_ticks_s { + uint32_t ticks; // ticks == 0 means standstill + bool count_up; +}; + struct queue_end_s { volatile int32_t pos; // in steps volatile bool count_up; From 7b5ffe5f948e127d78c47a44566726ac288c9559 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Mon, 20 Nov 2023 23:55:43 +0100 Subject: [PATCH 21/85] add parameter realtime to getCurrentSpeedIn...() --- CHANGELOG.md | 4 +- examples/Issue208/Issue208.ino | 77 ++++++----- examples/StepperDemo/generic.h | 2 +- extras/doc/FastAccelStepper_API.md | 21 ++- .../simavr_based/test_issue208/expect.txt | 18 ++- .../test_sd_04_timing_2560/expect.txt | 14 +- .../test_sd_04_timing_328p/expect.txt | 10 +- .../test_sd_04_timing_328p_37k/expect.txt | 6 +- src/FastAccelStepper.cpp | 32 +++-- src/FastAccelStepper.h | 23 +++- src/RampGenerator.h | 4 +- src/StepperISR.cpp | 12 +- src/StepperISR.h | 2 +- src/StepperISR_esp32c3_rmt.cpp | 122 +++++++++--------- src/common.h | 2 +- 15 files changed, 189 insertions(+), 160 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5557ee76..99e6d7d8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,8 +7,8 @@ TODO: - merge the two esp32 rmt drivers as soon as esp32c3 works pre-0.30.7: -- Fix for issue #208: close to speed reversal is the sign of current speed incorrect. - Still the speed increase during acceleration/deceleraion is not monotonic +- Fix for issue #208: the sign of current speed may be incorrect close to direction change +- The functions `getCurrentSpeedInMilliHz()` and `getCurrentSpeedInUs()` have been extended to supply a bool parameter about being realtime. 0.30.6: - Support for ESP32C3 diff --git a/examples/Issue208/Issue208.ino b/examples/Issue208/Issue208.ino index 92dc5ad2..0f515634 100644 --- a/examples/Issue208/Issue208.ino +++ b/examples/Issue208/Issue208.ino @@ -19,6 +19,7 @@ FastAccelStepperEngine engine = FastAccelStepperEngine(); FastAccelStepper *stepper; bool test() { + bool verbose = false; int32_t errCnt = 0; Serial.println("Start test: speed up"); int32_t last_v_mHz = 0; @@ -26,52 +27,53 @@ bool test() { stepper->setSpeedInHz(36800); stepper->runForward(); - while(stepper->rampState() != (RAMP_STATE_COAST | RAMP_DIRECTION_COUNT_UP)) { - int32_t v_mHz = stepper->getCurrentSpeedInMilliHz(); - if (v_mHz == last_v_mHz) { - continue; - } - Serial.println(v_mHz); - if (v_mHz < last_v_mHz) { - Serial.print("FAIL: last="); - Serial.print(last_v_mHz); - Serial.print(" new="); - Serial.println(v_mHz); - errCnt++; - // return false; - } - last_v_mHz = v_mHz; + while (stepper->rampState() != (RAMP_STATE_COAST | RAMP_DIRECTION_COUNT_UP)) { + int32_t v_mHz = stepper->getCurrentSpeedInMilliHz(false); + if (v_mHz == last_v_mHz) { + continue; + } + if (verbose) Serial.println(v_mHz); + if (v_mHz < last_v_mHz) { + Serial.print("FAIL: last="); + Serial.print(last_v_mHz); + Serial.print(" new="); + Serial.println(v_mHz); + errCnt++; + // return false; + } + last_v_mHz = v_mHz; } // There can be still speed increases in the queue, even so the ramp generator // is already coasting delay(20); - last_v_mHz = stepper->getCurrentSpeedInMilliHz(); + last_v_mHz = stepper->getCurrentSpeedInMilliHz(false); Serial.println("Reverse"); stepper->runBackward(); - while(stepper->rampState() != (RAMP_STATE_COAST | RAMP_DIRECTION_COUNT_DOWN)) { - int32_t v_mHz = stepper->getCurrentSpeedInMilliHz(); - if (v_mHz == last_v_mHz) { - continue; - } - Serial.println(v_mHz); - if (v_mHz > last_v_mHz) { - Serial.print("FAIL: last="); - Serial.print(last_v_mHz); - Serial.print(" new="); - Serial.println(v_mHz); - errCnt++; - // return false; - } - last_v_mHz = v_mHz; + while (stepper->rampState() != + (RAMP_STATE_COAST | RAMP_DIRECTION_COUNT_DOWN)) { + int32_t v_mHz = stepper->getCurrentSpeedInMilliHz(false); + if (v_mHz == last_v_mHz) { + continue; + } + if (verbose) Serial.println(v_mHz); + if (v_mHz > last_v_mHz) { + Serial.print("FAIL: last="); + Serial.print(last_v_mHz); + Serial.print(" new="); + Serial.println(v_mHz); + errCnt++; + // return false; + } + last_v_mHz = v_mHz; } if (errCnt > 0) { - Serial.print("Errors="); - Serial.println(errCnt); + Serial.print("Errors="); + Serial.println(errCnt); } - return errCnt != 0; + return errCnt == 0; } void setup() { @@ -112,8 +114,7 @@ void setup() { Serial.println("PASS"); digitalWrite(PIN, HIGH); digitalWrite(PIN, LOW); - } - else { + } else { Serial.println("FAIL"); } delay(1000); @@ -122,6 +123,4 @@ void setup() { #endif } - -void loop() { -} +void loop() {} diff --git a/examples/StepperDemo/generic.h b/examples/StepperDemo/generic.h index 241804f8..47404d16 100644 --- a/examples/StepperDemo/generic.h +++ b/examples/StepperDemo/generic.h @@ -13,7 +13,7 @@ #error "Unsupported derivate" #endif -#if defined(CONFIG_IDF_TARGET_ESP32C3) && (ARDUINO_USB_MODE==1) +#if defined(CONFIG_IDF_TARGET_ESP32C3) && (ARDUINO_USB_MODE == 1) #define SerialInterface USBSerial #else #define SerialInterface Serial diff --git a/extras/doc/FastAccelStepper_API.md b/extras/doc/FastAccelStepper_API.md index 498b0808..fa03dd34 100644 --- a/extras/doc/FastAccelStepper_API.md +++ b/extras/doc/FastAccelStepper_API.md @@ -157,8 +157,8 @@ And the two directions of a move #define RAMP_DIRECTION_COUNT_DOWN 64 ``` A ramp state value of 2 is set after any move call on a stopped motor -and until the stepper task. The stepper task will then control the direction -flags +and until the stepper task is serviced. The stepper task will then +control the direction flags ## Timing values - Architecture dependent @@ -326,9 +326,22 @@ retrieves the actual speed. | > 0 | while position counting up  | | < 0 | while position counting down | +If the parameter realtime is true, then the most actual speed +from the stepper queue is derived. This works only, if the queue +does not contain pauses, which is normally the case for slow speeds. +Otherwise the speed from the ramp generator is reported, which is +done always in case of `realtime == false`. That speed is typically +the value of the speed a couple of milliseconds later. + +The drawback of `realtime == true` is, that the reported speed +may either come from the queue or from the ramp generator. +This means the returned speed may have jumps during +acceleration/deceleration. + +For backward compatibility, the default is true. ```cpp - int32_t getCurrentSpeedInUs(); - int32_t getCurrentSpeedInMilliHz(); + int32_t getCurrentSpeedInUs(bool realtime = true); + int32_t getCurrentSpeedInMilliHz(bool realtime = true); ``` ## Acceleration setAcceleration() expects as parameter the change of speed diff --git a/extras/tests/simavr_based/test_issue208/expect.txt b/extras/tests/simavr_based/test_issue208/expect.txt index 8de30f0f..e08e8010 100644 --- a/extras/tests/simavr_based/test_issue208/expect.txt +++ b/extras/tests/simavr_based/test_issue208/expect.txt @@ -1,20 +1,18 @@ DirA: 0*L->H, 1*H->L DirB: 1*L->H, 0*H->L - EnableA: 3*L->H, 2*H->L + EnableA: 0*L->H, 0*H->L EnableB: 0*L->H, 0*H->L - StepA: 1048*L->H, 1048*H->L, Max High=10us Total High=5476us - StepB: 2*L->H, 2*H->L, Max High=4us Total High=9us -Position[A]=0 + StepA: 235909*L->H, 235909*H->L, Max High=11us Total High=1237097us + StepB: 2*L->H, 2*H->L, Max High=13us Total High=17us +Position[A]=37461 Position[B]=2 -Time in EnableA max=4204 us, total=8408 us +Time in FillISR max=769 us, total=1237221 us -Time in FillISR max=1906 us, total=25682 us +Time in StepA max=11 us, total=1237097 us -Time in StepA max=10 us, total=5476 us +Time in StepB max=13 us, total=17 us -Time in StepB max=4 us, total=9 us - -Time in StepISR max=7 us, total=4875 us +Time in StepISR max=7 us, total=1021996 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index b570d48b..1240f3bc 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=521699us - StepB: 64000*L->H, 64000*H->L, Max High=35us Total High=393630us - StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=484932us + StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=580688us + StepB: 64000*L->H, 64000*H->L, Max High=31us Total High=392513us + StepC: 64000*L->H, 64000*H->L, Max High=41us Total High=426208us Position[A]=64000 Position[B]=64000 @@ -19,11 +19,11 @@ Time in EnableB max=246080 us, total=246080 us Time in EnableC max=254235 us, total=254235 us -Time in FillISR max=2686 us, total=2092758 us +Time in FillISR max=2686 us, total=2091889 us -Time in StepA max=28 us, total=521699 us +Time in StepA max=28 us, total=580688 us -Time in StepB max=35 us, total=393630 us +Time in StepB max=31 us, total=392513 us -Time in StepC max=40 us, total=484932 us +Time in StepC max=41 us, total=426208 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index f976eb62..323d519e 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,8 +2,8 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=5539us - StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=5513us + StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=5545us + StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5520us Position[A]=1000 Position[B]=1000 @@ -12,11 +12,11 @@ Time in EnableA max=225398 us, total=225398 us Time in EnableB max=238116 us, total=238116 us -Time in FillISR max=2644 us, total=47660 us +Time in FillISR max=2644 us, total=47652 us -Time in StepA max=13 us, total=5539 us +Time in StepA max=13 us, total=5545 us -Time in StepB max=15 us, total=5513 us +Time in StepB max=16 us, total=5520 us Time in StepISR max=7 us, total=9224 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index 9e754e06..158958c3 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=5276us + StepA: 1000*L->H, 1000*H->L, Max High=12us Total High=5274us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2015 us, total=27549 us +Time in FillISR max=2015 us, total=27557 us -Time in StepA max=11 us, total=5276 us +Time in StepA max=12 us, total=5274 us Time in StepISR max=7 us, total=4581 us diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index f0b9a619..cfe422f9 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -775,32 +775,38 @@ uint32_t FastAccelStepper::getPeriodInUsAfterCommandsCompleted() { } return 0; } -void FastAccelStepper::getCurrentSpeedInTicks(struct actual_ticks_s *speed) { - bool valid = fas_queue[_queue_num].getActualTicksWithDirection(speed); +void FastAccelStepper::getCurrentSpeedInTicks(struct actual_ticks_s* speed, + bool realtime) { + bool valid; + if (realtime) { + valid = fas_queue[_queue_num].getActualTicksWithDirection(speed); + } else { + valid = false; + } if (!valid) { if (_rg.isRampGeneratorActive()) { - _rg.getCurrentSpeedInTicks(speed); - } + _rg.getCurrentSpeedInTicks(speed); + } } } -int32_t FastAccelStepper::getCurrentSpeedInUs() { +int32_t FastAccelStepper::getCurrentSpeedInUs(bool realtime) { struct actual_ticks_s speed; - getCurrentSpeedInTicks(&speed); + getCurrentSpeedInTicks(&speed, realtime); int32_t speed_in_us = speed.ticks / (TICKS_PER_S / 1000000); if (speed.count_up) { - return speed_in_us; + return speed_in_us; } return -speed_in_us; } -int32_t FastAccelStepper::getCurrentSpeedInMilliHz() { +int32_t FastAccelStepper::getCurrentSpeedInMilliHz(bool realtime) { struct actual_ticks_s speed; - getCurrentSpeedInTicks(&speed); + getCurrentSpeedInTicks(&speed, realtime); if (speed.ticks > 0) { int32_t speed_in_mhz = ((uint32_t)250 * TICKS_PER_S) / speed.ticks * 4; - if (speed.count_up) { - return speed_in_mhz; - } - return -speed_in_mhz; + if (speed.count_up) { + return speed_in_mhz; + } + return -speed_in_mhz; } return 0; } diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index 9b0cada2..779bad98 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -187,8 +187,8 @@ class FastAccelStepperEngine { #define RAMP_DIRECTION_COUNT_DOWN 64 // A ramp state value of 2 is set after any move call on a stopped motor -// and until the stepper task. The stepper task will then control the direction -// flags +// and until the stepper task is serviced. The stepper task will then +// control the direction flags #include "RampGenerator.h" @@ -365,8 +365,21 @@ class FastAccelStepper { // | > 0 | while position counting up  | // | < 0 | while position counting down | // - int32_t getCurrentSpeedInUs(); - int32_t getCurrentSpeedInMilliHz(); + // If the parameter realtime is true, then the most actual speed + // from the stepper queue is derived. This works only, if the queue + // does not contain pauses, which is normally the case for slow speeds. + // Otherwise the speed from the ramp generator is reported, which is + // done always in case of `realtime == false`. That speed is typically + // the value of the speed a couple of milliseconds later. + // + // The drawback of `realtime == true` is, that the reported speed + // may either come from the queue or from the ramp generator. + // This means the returned speed may have jumps during + // acceleration/deceleration. + // + // For backward compatibility, the default is true. + int32_t getCurrentSpeedInUs(bool realtime = true); + int32_t getCurrentSpeedInMilliHz(bool realtime = true); // ## Acceleration // setAcceleration() expects as parameter the change of speed @@ -646,7 +659,7 @@ class FastAccelStepper { bool needAutoDisable(); bool agreeWithAutoDisable(); bool usesAutoEnablePin(uint8_t pin); - void getCurrentSpeedInTicks(struct actual_ticks_s *speed); + void getCurrentSpeedInTicks(struct actual_ticks_s* speed, bool realtime); FastAccelStepperEngine* _engine; RampGenerator _rg; diff --git a/src/RampGenerator.h b/src/RampGenerator.h index a31b5204..b3a25365 100644 --- a/src/RampGenerator.h +++ b/src/RampGenerator.h @@ -92,9 +92,9 @@ class RampGenerator { void getCurrentSpeedInTicks(struct actual_ticks_s *speed) { fasDisableInterrupts(); speed->ticks = _rw.curr_ticks; - uint8_t rs = _rw.rampState(); + uint8_t rs = _rw.rampState(); fasEnableInterrupts(); - speed->count_up = ((rs & RAMP_DIRECTION_COUNT_UP) != 0); + speed->count_up = ((rs & RAMP_DIRECTION_COUNT_UP) != 0); } uint32_t getCurrentPeriodInTicks() { fasDisableInterrupts(); diff --git a/src/StepperISR.cpp b/src/StepperISR.cpp index 9ee7961b..de814e77 100644 --- a/src/StepperISR.cpp +++ b/src/StepperISR.cpp @@ -281,7 +281,7 @@ bool StepperQueue::hasTicksInQueue(uint32_t min_ticks) { return false; } -bool StepperQueue::getActualTicksWithDirection(struct actual_ticks_s *speed) { +bool StepperQueue::getActualTicksWithDirection(struct actual_ticks_s* speed) { // Retrieve current step rate from the current command. // This is valid only, if the command describes more than one step, // or if the next command contains one step, too. @@ -290,19 +290,19 @@ bool StepperQueue::getActualTicksWithDirection(struct actual_ticks_s *speed) { uint8_t wp = next_write_idx; fasEnableInterrupts(); if (wp == rp) { - speed->ticks = 0; + speed->ticks = 0; return true; } struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK]; if (e->hasSteps) { - speed->count_up = e->countUp; - speed->ticks = e->ticks; + speed->count_up = e->countUp; + speed->ticks = e->ticks; if (e->moreThanOneStep) { - return true; + return true; } if (wp != ++rp) { if (entry[rp & QUEUE_LEN_MASK].hasSteps) { - return true; + return true; } } } diff --git a/src/StepperISR.h b/src/StepperISR.h index 5aeca752..b09e1a71 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -139,7 +139,7 @@ class StepperQueue { int32_t getCurrentPosition(); uint32_t ticksInQueue(); bool hasTicksInQueue(uint32_t min_ticks); - bool getActualTicksWithDirection(struct actual_ticks_s *speed); + bool getActualTicksWithDirection(struct actual_ticks_s* speed); volatile uint16_t getMaxSpeedInTicks() { return max_speed_in_ticks; } diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp index 655db63e..34e6efe4 100644 --- a/src/StepperISR_esp32c3_rmt.cpp +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -24,7 +24,7 @@ // - if the end marker is hit in continuous mode, there is no end interrupt // - there is no tick lost with the end marker // - minimum periods as per relation 1 and 2 to be adhered to -// +// // #ifdef SUPPORT_ESP32C3_RMT #define PART_SIZE 23 @@ -36,16 +36,16 @@ #endif // In order to avoid threshold/end interrupt on end, add one -#define enable_rmt_interrupts(channel) { \ - RMT.tx_lim[channel].RMT_LIMIT = PART_SIZE + 2;\ - RMT.tx_conf[channel].conf_update = 1; \ - RMT.tx_conf[channel].conf_update = 0; \ - RMT.int_clr.val |= 0x101 << channel; \ - RMT.int_ena.val |= 0x101 << channel; \ -} -#define disable_rmt_interrupts(channel) { \ - RMT.int_ena.val &= ~(0x101 << channel); \ -} +#define enable_rmt_interrupts(channel) \ + { \ + RMT.tx_lim[channel].RMT_LIMIT = PART_SIZE + 2; \ + RMT.tx_conf[channel].conf_update = 1; \ + RMT.tx_conf[channel].conf_update = 0; \ + RMT.int_clr.val |= 0x101 << channel; \ + RMT.int_ena.val |= 0x101 << channel; \ + } +#define disable_rmt_interrupts(channel) \ + { RMT.int_ena.val &= ~(0x101 << channel); } void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // We are stopping the rmt by letting it run into the end at high speed. @@ -87,7 +87,7 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, // make a pause with approx. 1ms // 258 ticks * 2 * 31 = 15996 @ 16MHz // 347 ticks * 2 * 23 = 15962 @ 16MHz - *data++ = 0x010001 * (16000/2/PART_SIZE); + *data++ = 0x010001 * (16000 / 2 / PART_SIZE); } } else { q->stop_rmt(false); @@ -127,17 +127,17 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, if (steps == 0) { q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; // Perhaps the rmt performs look ahead - ticks -= (PART_SIZE-2) * 4 + 8; + ticks -= (PART_SIZE - 2) * 4 + 8; uint16_t ticks_l = ticks >> 1; uint16_t ticks_r = ticks - ticks_l; last_entry = ticks_l; last_entry <<= 16; last_entry |= ticks_r; - *data++ = last_entry; + *data++ = last_entry; for (uint8_t i = 1; i < PART_SIZE - 1; i++) { *data++ = 0x00020002; } - last_entry = 0x00040004; + last_entry = 0x00040004; } else { q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; if (ticks == 0xffff) { @@ -209,13 +209,13 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, } // No tick lost mentioned for esp32c3 // if (!fill_part_one) { - // Note: When enabling the continuous transmission mode by setting - // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the - // channel continuously, that is, from the first byte to the last one, - // then from the first to the last again, and so on. In this mode, there - // will be an idle level lasting one clk_div cycle between N and N+1 - // transmissions. - // last_entry -= 1; + // Note: When enabling the continuous transmission mode by setting + // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the + // channel continuously, that is, from the first byte to the last one, + // then from the first to the last again, and so on. In this mode, there + // will be an idle level lasting one clk_div cycle between N and N+1 + // transmissions. + // last_entry -= 1; // } *data = last_entry; @@ -258,7 +258,7 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, PROBE_2_TOGGLE; \ /* demonstrate modification of RAM */ \ uint32_t *mem = FAS_RMT_MEM(ch); \ - mem[PART_SIZE] = 0x33ff33ff; \ + mem[PART_SIZE] = 0x33ff33ff; \ RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ } else { \ /* first half of buffer sent */ \ @@ -269,34 +269,34 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, RMT.tx_conf[ch].conf_update = 0; \ } #else -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - disable_rmt_interrupts(q->channel); \ - q->_isRunning = false; \ - PROBE_1_TOGGLE; \ - PROBE_2_TOGGLE; \ - PROBE_3_TOGGLE; \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ - StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - uint32_t *mem = FAS_RMT_MEM(ch); \ - if (old_limit == PART_SIZE + 1) { \ - /* second half of buffer sent */ \ - PROBE_2_TOGGLE; \ - apply_command(q, false, mem); \ - /* demonstrate modification of RAM */ \ - /*mem[PART_SIZE] = 0x33fff3ff; */ \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ - } else { \ - /* first half of buffer sent */ \ - PROBE_3_TOGGLE; \ - apply_command(q, true, mem); \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ - } \ - RMT.tx_conf[ch].conf_update = 1; \ - RMT.tx_conf[ch].conf_update = 0; \ +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + disable_rmt_interrupts(q->channel); \ + q->_isRunning = false; \ + PROBE_1_TOGGLE; \ + PROBE_2_TOGGLE; \ + PROBE_3_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + apply_command(q, false, mem); \ + /* demonstrate modification of RAM */ \ + /*mem[PART_SIZE] = 0x33fff3ff; */ \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + apply_command(q, true, mem); \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 0; \ } #endif @@ -357,14 +357,14 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { // 3 * T_APB + 5 * T_RMT_CLK < period * T_CLK_DIV // => 8 * T_APB < period * T_APB*5 // => period > 8/5 - // => period >= 2 - // + // => period >= 2 + // // Relation 2 in esp32c3 technical reference before end marker: // 6 * T_APB + 12 * T_RMT_CLK < period * T_CLK_DIV // => 18 * T_APB < period * T_APB*5 // => period > 18/5 - // => period >= 4 - // + // => period >= 4 + // disable_rmt_interrupts(channel); rmt_set_source_clk(channel, RMT_BASECLK_APB); rmt_set_clk_div(channel, 5); @@ -409,7 +409,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { RMT.tx_conf[channel].mem_tx_wrap_en = 0; RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].conf_update = 0; - enable_rmt_interrupts(channel); + enable_rmt_interrupts(channel); // tx_start does not need conf_update PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 RMT.tx_conf[channel].tx_start = 1; @@ -544,8 +544,8 @@ void StepperQueue::startQueue_rmt() { // Fill the buffer with a significant pattern for debugging // Keep it for now for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { -// mem[i] = 0x0fff8fff; -// mem[i + 1] = 0x7fff8fff; + // mem[i] = 0x0fff8fff; + // mem[i + 1] = 0x7fff8fff; } #endif // Write end marker @@ -630,9 +630,9 @@ void StepperQueue::startQueue_rmt() { RMT.tx_conf[channel].tx_conti_mode = 1; RMT.tx_conf[channel].conf_update = 1; RMT.tx_conf[channel].conf_update = 0; - RMT.tx_conf[channel].mem_tx_wrap_en = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; PROBE_1_TOGGLE; RMT.tx_conf[channel].tx_start = 1; diff --git a/src/common.h b/src/common.h index f76f7a72..31079051 100644 --- a/src/common.h +++ b/src/common.h @@ -17,7 +17,7 @@ struct stepper_command_s { }; struct actual_ticks_s { - uint32_t ticks; // ticks == 0 means standstill + uint32_t ticks; // ticks == 0 means standstill bool count_up; }; From ae0e4252a0ec0343df1a86ac7fabb42561fd6580 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Tue, 21 Nov 2023 00:00:42 +0100 Subject: [PATCH 22/85] fix build errors due to unused variable in Issue208.ino --- examples/Issue208/Issue208.ino | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/examples/Issue208/Issue208.ino b/examples/Issue208/Issue208.ino index 0f515634..911d2989 100644 --- a/examples/Issue208/Issue208.ino +++ b/examples/Issue208/Issue208.ino @@ -101,6 +101,15 @@ void setup() { bool pass = test(); stepper->stopMove(); +#ifndef SIMULATOR + if (pass) { + Serial.print("PASS"); + } + else { + Serial.print("FAIL"); + } +#endif + #ifdef SIMULATOR // if result is Ok. Toggle port twice, otherwise once #define PIN stepPinStepperB From 57a34da0b4792206eabe3f97fbefe24dbf379fac Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Tue, 21 Nov 2023 00:09:09 +0100 Subject: [PATCH 23/85] bump version for issue #208 --- CHANGELOG.md | 2 +- library.properties | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 99e6d7d8..221af4aa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.7: +0.30.7: - Fix for issue #208: the sign of current speed may be incorrect close to direction change - The functions `getCurrentSpeedInMilliHz()` and `getCurrentSpeedInUs()` have been extended to supply a bool parameter about being realtime. diff --git a/library.properties b/library.properties index 7e2c424d..6d0af476 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.6 +version=0.30.7 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes From d7fa184c23eba26aad9cb735f0cf92c44bcaeccd Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Fri, 1 Dec 2023 02:20:14 +0100 Subject: [PATCH 24/85] add star history --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 30efde16..66823f6f 100644 --- a/README.md +++ b/README.md @@ -66,6 +66,11 @@ FastAccelStepper offers the following features: * Provide API to each steppers' command queue. Those commands are tied to timer ticks aka the CPU frequency! * Command queue can be filled with commands and then started. This allows near synchronous start of several steppers for multi axis applications. +## Star History + +[![Star History Chart](https://api.star-history.com/svg?repos=gin66/FastAccelStepper&type=Date)](https://star-history.com/#gin66/FastAccelStepper&Date) + + General behaviour: * The desired end position to move to is set by calls to moveTo() and move() * The desired end position is in case of moveTo() given as absolute position From fc988a07f53654a217b9e77d3b8de77b0e857678 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sun, 3 Dec 2023 11:31:54 +0100 Subject: [PATCH 25/85] Implement setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) as proposed by issue #210 --- CHANGELOG.md | 3 +++ library.properties | 2 +- src/FastAccelStepper.cpp | 3 +++ src/FastAccelStepper.h | 6 ++++++ src/StepperISR.h | 3 +++ src/StepperISR_esp32.cpp | 4 ++++ src/common.h | 4 ++++ 7 files changed, 24 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 221af4aa..d056341a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,9 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works +pre-0.30.8: +- Implement `setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks)` as proposed by issue #210 + 0.30.7: - Fix for issue #208: the sign of current speed may be incorrect close to direction change - The functions `getCurrentSpeedInMilliHz()` and `getCurrentSpeedInUs()` have been extended to supply a bool parameter about being realtime. diff --git a/library.properties b/library.properties index 6d0af476..5c1064dd 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.7 +version=0.30.8 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index cfe422f9..a832e8b5 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -828,6 +828,9 @@ uint32_t FastAccelStepper::getMaxSpeedInMilliHz() { uint32_t speed_in_milli_hz = ((uint32_t)250 * TICKS_PER_S) / ticks * 4; return speed_in_milli_hz; } +void FastAccelStepper::setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) { + fas_queue[_queue_num].setAbsoluteSpeedLimit(max_speed_in_ticks); +} int8_t FastAccelStepper::setSpeedInTicks(uint32_t min_step_ticks) { if (min_step_ticks < getMaxSpeedInTicks()) { return -1; diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index 779bad98..949bdade 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -331,6 +331,12 @@ class FastAccelStepper { uint32_t getMaxSpeedInHz(); uint32_t getMaxSpeedInMilliHz(); + // For esp32, the device's maximum allowed speed can be overridden + // This is absolutely untested and use at your own risk. +#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING==1 + setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); +#endif + // Setting the speed can be done with the four `setSpeed...()` calls. // The new value will be used only after call of these functions: // diff --git a/src/StepperISR.h b/src/StepperISR.h index b09e1a71..c26a34d1 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -134,6 +134,9 @@ class StepperQueue { entry[read_idx & QUEUE_LEN_MASK].repeat_entry = 0; } #endif +#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING==1 + setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); +#endif int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start); int32_t getCurrentPosition(); diff --git a/src/StepperISR_esp32.cpp b/src/StepperISR_esp32.cpp index 9b7b2e02..0b8ffef2 100644 --- a/src/StepperISR_esp32.cpp +++ b/src/StepperISR_esp32.cpp @@ -125,6 +125,10 @@ void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { max_speed_in_ticks = 80; // This equals 200kHz @ 16MHz } +void StepperQueue::setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) { + self->max_speed_in_ticks = max_speed_in_ticks; +} + void fas_init_engine(FastAccelStepperEngine *engine, uint8_t cpu_core) { #define STACK_SIZE 2000 #define PRIORITY configMAX_PRIORITIES diff --git a/src/common.h b/src/common.h index 31079051..880ab1d4 100644 --- a/src/common.h +++ b/src/common.h @@ -79,6 +79,7 @@ struct queue_end_s { #define MIN_DIR_DELAY_US (MIN_CMD_TICKS / (TICKS_PER_S / 1000000)) #define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000)) #define DELAY_MS_BASE 1 +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 0 #define noop_or_wait @@ -96,6 +97,7 @@ struct queue_end_s { #define SUPPORT_ESP32 #define SUPPORT_EXTERNAL_DIRECTION_PIN +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1 // Some more esp32 specific includes #include @@ -237,6 +239,7 @@ struct queue_end_s { #elif defined(ESP_PLATFORM) #define SUPPORT_ESP32 +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1 // esp32 specific includes #include @@ -337,6 +340,7 @@ struct queue_end_s { //========================================================================== #elif defined(ARDUINO_ARCH_AVR) #define SUPPORT_AVR +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 0 // this is an arduino platform, so include the Arduino.h header file #include From 90bf7f1b60cdd792104215cb2a5ae8e9304c26fd Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sun, 3 Dec 2023 11:54:13 +0100 Subject: [PATCH 26/85] fix compile issues. Update README --- CHANGELOG.md | 2 +- README.md | 8 +++++++- examples/Issue208/Issue208.ino | 3 +-- extras/doc/FastAccelStepper_API.md | 9 ++++++++- src/FastAccelStepper.cpp | 2 ++ src/FastAccelStepper.h | 8 ++++---- src/StepperISR.h | 4 ++-- src/StepperISR_esp32.cpp | 4 ++-- 8 files changed, 27 insertions(+), 13 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d056341a..c51d215a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.8: +0.30.8: - Implement `setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks)` as proposed by issue #210 0.30.7: diff --git a/README.md b/README.md index 66823f6f..46901211 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ No issue with platformio. Check the [related issue](https://github.com/arduino/l [![Build examples for Atmega32U4](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_atmega32u4.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_nanoatmega32u4.yml) This is a high speed alternative for the [AccelStepper library](http://www.airspayce.com/mikem/arduino/AccelStepper/). -Supported are avr (ATmega 168/328/P, ATmega2560, ATmega32u4), esp32, esp32s2, esp32s3 and atmelsam due. +Supported are avr (ATmega 168/328/P, ATmega2560, ATmega32u4), esp32, esp32s2, esp32s3, esp32c3 and atmelsam due. The stepper motors should be connected via a driver IC (like A4988) with a 1, 2 or 3-wire connection: * Step Signal @@ -150,6 +150,12 @@ Comments to pin sharing: * supports up to four stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) * Steppers' command queue depth: 32 +### ESP32C3 + +* allows up to 200000 generated steps per second ? +* supports up to two stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Steppers' command queue depth: 32 + ### Atmel SAM Due * allows up to 50000 generated steps per second diff --git a/examples/Issue208/Issue208.ino b/examples/Issue208/Issue208.ino index 911d2989..d4ec04c0 100644 --- a/examples/Issue208/Issue208.ino +++ b/examples/Issue208/Issue208.ino @@ -104,8 +104,7 @@ void setup() { #ifndef SIMULATOR if (pass) { Serial.print("PASS"); - } - else { + } else { Serial.print("FAIL"); } #endif diff --git a/extras/doc/FastAccelStepper_API.md b/extras/doc/FastAccelStepper_API.md index fa03dd34..cfa49af3 100644 --- a/extras/doc/FastAccelStepper_API.md +++ b/extras/doc/FastAccelStepper_API.md @@ -157,7 +157,7 @@ And the two directions of a move #define RAMP_DIRECTION_COUNT_DOWN 64 ``` A ramp state value of 2 is set after any move call on a stopped motor -and until the stepper task is serviced. The stepper task will then +and until the stepper task is serviced. The stepper task will then control the direction flags ## Timing values - Architecture dependent @@ -290,6 +290,13 @@ For the device's maximum allowed speed, the following calls can be used. uint32_t getMaxSpeedInHz(); uint32_t getMaxSpeedInMilliHz(); ``` +For esp32, the device's maximum allowed speed can be overridden +This is absolutely untested. Use at your own risk. +```cpp +#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 + void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); +#endif +``` Setting the speed can be done with the four `setSpeed...()` calls. The new value will be used only after call of these functions: diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index a832e8b5..3c5dad57 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -828,9 +828,11 @@ uint32_t FastAccelStepper::getMaxSpeedInMilliHz() { uint32_t speed_in_milli_hz = ((uint32_t)250 * TICKS_PER_S) / ticks * 4; return speed_in_milli_hz; } +#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 void FastAccelStepper::setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) { fas_queue[_queue_num].setAbsoluteSpeedLimit(max_speed_in_ticks); } +#endif int8_t FastAccelStepper::setSpeedInTicks(uint32_t min_step_ticks) { if (min_step_ticks < getMaxSpeedInTicks()) { return -1; diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index 949bdade..02edb827 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -187,7 +187,7 @@ class FastAccelStepperEngine { #define RAMP_DIRECTION_COUNT_DOWN 64 // A ramp state value of 2 is set after any move call on a stopped motor -// and until the stepper task is serviced. The stepper task will then +// and until the stepper task is serviced. The stepper task will then // control the direction flags #include "RampGenerator.h" @@ -332,9 +332,9 @@ class FastAccelStepper { uint32_t getMaxSpeedInMilliHz(); // For esp32, the device's maximum allowed speed can be overridden - // This is absolutely untested and use at your own risk. -#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING==1 - setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); + // This is absolutely untested. Use at your own risk. +#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 + void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); #endif // Setting the speed can be done with the four `setSpeed...()` calls. diff --git a/src/StepperISR.h b/src/StepperISR.h index c26a34d1..a4058049 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -134,8 +134,8 @@ class StepperQueue { entry[read_idx & QUEUE_LEN_MASK].repeat_entry = 0; } #endif -#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING==1 - setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); +#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 + void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); #endif int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start); diff --git a/src/StepperISR_esp32.cpp b/src/StepperISR_esp32.cpp index 0b8ffef2..02ca1717 100644 --- a/src/StepperISR_esp32.cpp +++ b/src/StepperISR_esp32.cpp @@ -125,8 +125,8 @@ void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { max_speed_in_ticks = 80; // This equals 200kHz @ 16MHz } -void StepperQueue::setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) { - self->max_speed_in_ticks = max_speed_in_ticks; +void StepperQueue::setAbsoluteSpeedLimit(uint16_t ticks) { + max_speed_in_ticks = ticks; } void fas_init_engine(FastAccelStepperEngine *engine, uint8_t cpu_core) { From 84e5ff952e0c18201e45540e1a32da05f2bcbcb6 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Wed, 6 Dec 2023 09:30:42 +0100 Subject: [PATCH 27/85] Fix esp32s3 to support the fourth stepper (issue #212) --- CHANGELOG.md | 3 +++ src/StepperISR_esp32_mcpwm_pcnt.cpp | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c51d215a..fb465a4f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,9 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works +pre-0.30.9: +- Fix esp32s3 to support the fourth stepper (issue #212) + 0.30.8: - Implement `setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks)` as proposed by issue #210 diff --git a/src/StepperISR_esp32_mcpwm_pcnt.cpp b/src/StepperISR_esp32_mcpwm_pcnt.cpp index 4177ede7..e0910e50 100644 --- a/src/StepperISR_esp32_mcpwm_pcnt.cpp +++ b/src/StepperISR_esp32_mcpwm_pcnt.cpp @@ -55,7 +55,6 @@ static struct mapping_s channel2mapping[NUM_QUEUES] = { cmpr_tea_int_ena : MCPWM_OP2_TEA_INT_ENA, cmpr_tea_int_raw : MCPWM_OP2_TEA_INT_RAW }, -#ifndef SUPPORT_ESP32S3_MCPWM_PCNT { mcpwm_unit : MCPWM_UNIT_1, timer : 0, @@ -66,6 +65,7 @@ static struct mapping_s channel2mapping[NUM_QUEUES] = { cmpr_tea_int_ena : MCPWM_OP0_TEA_INT_ENA, cmpr_tea_int_raw : MCPWM_OP0_TEA_INT_RAW }, +#ifndef SUPPORT_ESP32S3_MCPWM_PCNT { mcpwm_unit : MCPWM_UNIT_1, timer : 1, @@ -339,8 +339,8 @@ static void IRAM_ATTR mcpwm0_isr_service(void *arg) { MCPWM_SERVICE(MCPWM0, 2, 2); } static void IRAM_ATTR mcpwm1_isr_service(void *arg) { -#ifndef SUPPORT_ESP32S3_MCPWM_PCNT MCPWM_SERVICE(MCPWM1, 0, 3); +#ifndef SUPPORT_ESP32S3_MCPWM_PCNT MCPWM_SERVICE(MCPWM1, 1, 4); MCPWM_SERVICE(MCPWM1, 2, 5); #endif From 14266cf8ed29a4b25c50da2c9fbd21874859d178 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 8 Dec 2023 20:59:04 +0100 Subject: [PATCH 28/85] bump version --- CHANGELOG.md | 2 +- library.properties | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index fb465a4f..004670cd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.9: +0.30.9: - Fix esp32s3 to support the fourth stepper (issue #212) 0.30.8: diff --git a/library.properties b/library.properties index 5c1064dd..79d79c3a 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.8 +version=0.30.9 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes From 9ea8c817fc399766fffb4ddbc370a42584564461 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Wed, 27 Dec 2023 10:47:18 +0100 Subject: [PATCH 29/85] replace the two stepperConnectToPi definition by use of default parameter --- src/FastAccelStepper.cpp | 5 ----- src/FastAccelStepper.h | 5 +++-- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index 3c5dad57..66e0bf92 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -94,11 +94,6 @@ FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( return s; } #else -FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( - uint8_t step_pin) { - return stepperConnectToPin(step_pin, DRIVER_DONT_CARE); -} - FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( uint8_t step_pin, uint8_t driver_type) { // Check if already connected diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index 02edb827..673aa13c 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -81,8 +81,9 @@ class FastAccelStepperEngine { // hardware may have limitations - e.g. no stepper resources anymore, or the // step pin cannot be used, then NULL is returned. So it is advised to check // the return value of this call. +#if !defined(SUPPORT_SELECT_DRIVER_TYPE) FastAccelStepper* stepperConnectToPin(uint8_t step_pin); - +#endif // For e.g. esp32, there are two types of driver. // One using mcpwm and pcnt module. And another using rmt module. // This call allows to select the respective driver @@ -90,7 +91,7 @@ class FastAccelStepperEngine { #define DRIVER_MCPWM_PCNT 0 #define DRIVER_RMT 1 #define DRIVER_DONT_CARE 2 - FastAccelStepper* stepperConnectToPin(uint8_t step_pin, uint8_t driver_type); + FastAccelStepper* stepperConnectToPin(uint8_t step_pin, uint8_t driver_type = DRIVER_DONT_CARE); #endif // Comments to valid pins: From 5789b988567171e4b5f5d852552b00fb51f208e6 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Wed, 27 Dec 2023 10:56:40 +0100 Subject: [PATCH 30/85] Unify code in stepperConnectToPin to fix bug mentioned in #221 --- CHANGELOG.md | 3 +++ src/FastAccelStepper.cpp | 35 ++++++++--------------------------- 2 files changed, 11 insertions(+), 27 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 004670cd..3680a333 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,9 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works +pre-0.30.10: +- Unify code in stepperConnectToPin to fix bug mentioned in #221 + 0.30.9: - Fix esp32s3 to support the fourth stepper (issue #212) diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index 66e0bf92..604b4b97 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -62,7 +62,12 @@ bool FastAccelStepperEngine::isDirPinBusy(uint8_t dir_pin, //************************************************************************************************* #if !defined(SUPPORT_SELECT_DRIVER_TYPE) FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( - uint8_t step_pin) { + uint8_t step_pin) +#else +FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( + uint8_t step_pin, uint8_t driver_type) +#endif +{ // Check if already connected for (uint8_t i = 0; i < MAX_STEPPER; i++) { FastAccelStepper* s = _stepper[i]; @@ -75,6 +80,7 @@ FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( if (!_isValidStepPin(step_pin)) { return NULL; } +#if !defined(SUPPORT_SELECT_DRIVER_TYPE) int8_t fas_stepper_num = StepperQueue::queueNumForStepPin(step_pin); if (fas_stepper_num < 0) { // flexible, so just choose next if (_stepper_cnt >= MAX_STEPPER) { @@ -82,32 +88,7 @@ FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( } fas_stepper_num = _stepper_cnt; } - _stepper_cnt++; - - FastAccelStepper* s = &fas_stepper[fas_stepper_num]; - _stepper[fas_stepper_num] = s; - s->init(this, fas_stepper_num, step_pin); - for (uint8_t i = 0; i < _stepper_cnt; i++) { - FastAccelStepper* sx = _stepper[i]; - fas_queue[sx->_queue_num].adjustSpeedToStepperCount(_stepper_cnt); - } - return s; -} #else -FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( - uint8_t step_pin, uint8_t driver_type) { - // Check if already connected - for (uint8_t i = 0; i < MAX_STEPPER; i++) { - FastAccelStepper* s = _stepper[i]; - if (s) { - if (s->getStepPin() == step_pin) { - return NULL; - } - } - } - if (!_isValidStepPin(step_pin)) { - return NULL; - } uint8_t queue_from = 0; uint8_t queue_to = QUEUES_MCPWM_PCNT + QUEUES_RMT; if (driver_type == DRIVER_MCPWM_PCNT) { @@ -126,6 +107,7 @@ FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( if (fas_stepper_num < 0) { return NULL; } +#endif _stepper_cnt++; FastAccelStepper* s = &fas_stepper[fas_stepper_num]; @@ -139,7 +121,6 @@ FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( } return s; } -#endif //************************************************************************************************* void FastAccelStepperEngine::setDebugLed(uint8_t ledPin) { fas_ledPin = ledPin; From f9536fca46a54c9aaa65f2c770c1f91fe6913121 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Wed, 27 Dec 2023 10:58:10 +0100 Subject: [PATCH 31/85] format code --- extras/doc/FastAccelStepper_API.md | 4 +++- src/FastAccelStepper.cpp | 3 +-- src/FastAccelStepper.h | 3 ++- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/extras/doc/FastAccelStepper_API.md b/extras/doc/FastAccelStepper_API.md index cfa49af3..e5e38c8d 100644 --- a/extras/doc/FastAccelStepper_API.md +++ b/extras/doc/FastAccelStepper_API.md @@ -64,7 +64,9 @@ hardware may have limitations - e.g. no stepper resources anymore, or the step pin cannot be used, then NULL is returned. So it is advised to check the return value of this call. ```cpp +#if !defined(SUPPORT_SELECT_DRIVER_TYPE) FastAccelStepper* stepperConnectToPin(uint8_t step_pin); +#endif ``` For e.g. esp32, there are two types of driver. One using mcpwm and pcnt module. And another using rmt module. @@ -74,7 +76,7 @@ This call allows to select the respective driver #define DRIVER_MCPWM_PCNT 0 #define DRIVER_RMT 1 #define DRIVER_DONT_CARE 2 - FastAccelStepper* stepperConnectToPin(uint8_t step_pin, uint8_t driver_type); + FastAccelStepper* stepperConnectToPin(uint8_t step_pin, uint8_t driver_type = DRIVER_DONT_CARE); #endif ``` Comments to valid pins: diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index 604b4b97..6b5aabe2 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -61,8 +61,7 @@ bool FastAccelStepperEngine::isDirPinBusy(uint8_t dir_pin, } //************************************************************************************************* #if !defined(SUPPORT_SELECT_DRIVER_TYPE) -FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( - uint8_t step_pin) +FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin(uint8_t step_pin) #else FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( uint8_t step_pin, uint8_t driver_type) diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index 673aa13c..48ca5093 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -91,7 +91,8 @@ class FastAccelStepperEngine { #define DRIVER_MCPWM_PCNT 0 #define DRIVER_RMT 1 #define DRIVER_DONT_CARE 2 - FastAccelStepper* stepperConnectToPin(uint8_t step_pin, uint8_t driver_type = DRIVER_DONT_CARE); + FastAccelStepper* stepperConnectToPin(uint8_t step_pin, + uint8_t driver_type = DRIVER_DONT_CARE); #endif // Comments to valid pins: From db36a5e8b418869db9cedc3672317e1a324d4358 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Wed, 27 Dec 2023 11:07:22 +0100 Subject: [PATCH 32/85] rename common.h to fas_common.h as proposed in #220 --- .gitignore | 2 +- CHANGELOG.md | 1 + extras/tests/pc_based/Makefile | 2 +- extras/tests/pc_based/off_test_15.cpp | 124 ++++++++++++++++++++++++++ src/FastAccelStepper.h | 2 +- src/RampCalculator.h | 2 +- src/RampConstAcceleration.cpp | 2 +- src/RampConstAcceleration.h | 2 +- src/RampGenerator.h | 2 +- src/StepperISR.h | 2 +- src/{common.h => fas_common.h} | 0 11 files changed, 133 insertions(+), 8 deletions(-) create mode 100644 extras/tests/pc_based/off_test_15.cpp rename src/{common.h => fas_common.h} (100%) diff --git a/.gitignore b/.gitignore index 65a06c31..301098ca 100644 --- a/.gitignore +++ b/.gitignore @@ -20,7 +20,7 @@ extras/tests/pc_based/RampGenerator.cpp extras/tests/pc_based/RampGenerator.h extras/tests/pc_based/StepperISR*.cpp extras/tests/pc_based/StepperISR.h -extras/tests/pc_based/common.h +extras/tests/pc_based/fas_common.h extras/tests/pc_based/*.gnuplot extras/tests/pc_based/*_test extras/tests/pc_based/test.log diff --git a/CHANGELOG.md b/CHANGELOG.md index 3680a333..7e24fe11 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ TODO: pre-0.30.10: - Unify code in stepperConnectToPin to fix bug mentioned in #221 +- rename `common.h` to `fas_common.h` as proposed in #220 0.30.9: - Fix esp32s3 to support the fourth stepper (issue #212) diff --git a/extras/tests/pc_based/Makefile b/extras/tests/pc_based/Makefile index 839452a3..6787c2a7 100644 --- a/extras/tests/pc_based/Makefile +++ b/extras/tests/pc_based/Makefile @@ -13,7 +13,7 @@ test: $(TESTS) pmf_test rmc_test $(addsuffix >>test.log &&,$(addprefix ./,$(TESTS))) echo "All tests passed" LIB_H=FastAccelStepper.h PoorManFloat.h StepperISR.h \ - RampGenerator.h RampConstAcceleration.h RampCalculator.h common.h + RampGenerator.h RampConstAcceleration.h RampCalculator.h fas_common.h LIB_O=FastAccelStepper.o PoorManFloat.o StepperISR_test.o \ RampGenerator.o RampConstAcceleration.o RampCalculator.o StepperISR.o diff --git a/extras/tests/pc_based/off_test_15.cpp b/extras/tests/pc_based/off_test_15.cpp new file mode 100644 index 00000000..f2815a05 --- /dev/null +++ b/extras/tests/pc_based/off_test_15.cpp @@ -0,0 +1,124 @@ +#include +#include +#include + +#include "FastAccelStepper.h" +#include "StepperISR.h" + +char TCCR1A; +char TCCR1B; +char TCCR1C; +char TIMSK1; +char TIFR1; +unsigned short OCR1A; +unsigned short OCR1B; + +StepperQueue fas_queue[NUM_QUEUES]; + +void inject_fill_interrupt(int mark) {} +void noInterrupts() {} +void interrupts() {} + +#include "RampChecker.h" + +class FastAccelStepperTest { + public: + void init_queue() { + fas_queue[0].read_idx = 0; + fas_queue[1].read_idx = 0; + fas_queue[0].next_write_idx = 0; + fas_queue[1].next_write_idx = 0; + } + + void ramp() { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + assert(s.isQueueEmpty()); + s.setSpeedInHz(36800); + s.setAcceleration(1000000); + s.fill_queue(); + assert(s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + + char fname[100]; + sprintf(fname, "test_15.gnuplot"); + rc.start_plot(fname); + s.runForward(); + int32_t wait_ticks = TICKS_PER_S / 10; + uint8_t state = 0; + for (int i = 0; i < 2000; i++) { + if ((state == 0) && (wait_ticks < rc.total_ticks)) { + printf("New move\n"); + s.move(-100000); + wait_ticks += TICKS_PER_S / 10; + state += 1; + } + if ((state == 1) && (wait_ticks < rc.total_ticks)) { + printf("move with changed acceleration\n"); + s.setAcceleration(5000); + s.move(100000); + wait_ticks += TICKS_PER_S / 10; + state += 1; + } + if ((state == 2) && (wait_ticks < rc.total_ticks)) { + printf("stop"); + s.stopMove(); + wait_ticks += 2 * TICKS_PER_S / 10; + state += 1; + } + if (true) { + printf( + "Loop %d: Queue read/write = %d/%d Target pos = %d, Queue End " + "pos = %d QueueEmpty=%s\n", + i, fas_queue[0].read_idx, fas_queue[0].next_write_idx, + s.targetPos(), s.getPositionAfterCommandsCompleted(), + s.isQueueEmpty() ? "yes" : "no"); + } + if (!s.isRampGeneratorActive()) { + break; + } + s.fill_queue(); + if ((state == 3) && (wait_ticks < rc.total_ticks)) { + break; + } + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + rc.increase_ok = true; + rc.decrease_ok = true; + rc.check_section( + &fas_queue[0].entry[fas_queue[0].read_idx & QUEUE_LEN_MASK]); + fas_queue[0].read_idx++; + } + uint32_t to_dt = rc.total_ticks; + float planned_time = (to_dt - from_dt) * 1.0 / 16000000; + printf("planned time in buffer: %.6fs\n", planned_time); + // This must be ensured, so that the stepper does not run out of + // commands + assert((i == 0) || (old_planned_time_in_buffer > 0.005)); + old_planned_time_in_buffer = planned_time; + // stop after + if (rc.total_ticks > TICKS_PER_S * 40) { + break; + } + } + rc.finish_plot(); + // test(!s.isRampGeneratorActive(), "too many commands created"); + test(s.getCurrentPosition() > 70000, "stepper runs too slow"); + test(s.getCurrentPosition() < 80000, "stepper runs too fast"); + printf("Total time %f\n", rc.total_ticks / 16000000.0); + +#if (TEST_CREATE_QUEUE_CHECKSUM == 1) + printf("CHECKSUM for %d/%d/%d: %d\n", steps, travel_dt, accel, s.checksum); +#endif + } +}; +int main() { + FastAccelStepperTest test; + test.ramp(); + printf("TEST_15 PASSED\n"); + return 0; +} diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index 48ca5093..b3e93333 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -2,7 +2,7 @@ #define FASTACCELSTEPPER_H #include #include "PoorManFloat.h" -#include "common.h" +#include "fas_common.h" // # FastAccelStepper // diff --git a/src/RampCalculator.h b/src/RampCalculator.h index efa4862f..8527605e 100644 --- a/src/RampCalculator.h +++ b/src/RampCalculator.h @@ -4,7 +4,7 @@ #include #include "PoorManFloat.h" -#include "common.h" +#include "fas_common.h" #if (TICKS_PER_S == 16000000L) #define PMF_TICKS_PER_S PMF_CONST_16E6 diff --git a/src/RampConstAcceleration.cpp b/src/RampConstAcceleration.cpp index fdeb0ad9..2a230b7d 100644 --- a/src/RampConstAcceleration.cpp +++ b/src/RampConstAcceleration.cpp @@ -4,7 +4,7 @@ #include "StepperISR.h" #include "RampConstAcceleration.h" -#include "common.h" +#include "fas_common.h" #ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES static pmf_logarithmic pmfl_timer_freq; diff --git a/src/RampConstAcceleration.h b/src/RampConstAcceleration.h index 65130c0d..50b9bc16 100644 --- a/src/RampConstAcceleration.h +++ b/src/RampConstAcceleration.h @@ -1,7 +1,7 @@ #ifndef RAMP_CONST_ACCELERATION_H #define RAMP_CONST_ACCELERATION_H -#include "common.h" +#include "fas_common.h" struct ramp_ro_s { struct ramp_config_s config; diff --git a/src/RampGenerator.h b/src/RampGenerator.h index b3a25365..8c7c8766 100644 --- a/src/RampGenerator.h +++ b/src/RampGenerator.h @@ -4,7 +4,7 @@ #include "FastAccelStepper.h" #include "RampCalculator.h" #include "RampConstAcceleration.h" -#include "common.h" +#include "fas_common.h" class FastAccelStepper; diff --git a/src/StepperISR.h b/src/StepperISR.h index a4058049..db431fd1 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -1,7 +1,7 @@ #include #include "FastAccelStepper.h" -#include "common.h" +#include "fas_common.h" // Here are the global variables to interface with the interrupts diff --git a/src/common.h b/src/fas_common.h similarity index 100% rename from src/common.h rename to src/fas_common.h From 0f4c92629c849c293394ddd20291e78242ba2dc4 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Wed, 27 Dec 2023 11:22:05 +0100 Subject: [PATCH 33/85] adjust expectations for simavr tests --- CHANGELOG.md | 2 ++ .../tests/simavr_based/test_issue208/expect.txt | 8 ++++---- .../test_sd_04_timing_2560/expect.txt | 16 ++++++++-------- .../test_sd_04_timing_328p/expect.txt | 12 ++++++------ .../test_sd_04_timing_328p_37k/expect.txt | 4 ++-- 5 files changed, 22 insertions(+), 20 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7e24fe11..56b9c1c1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,8 @@ TODO: pre-0.30.10: - Unify code in stepperConnectToPin to fix bug mentioned in #221 + This ensures setting of per stepper speed limit is working for avr variants without side effect + of uncontrolled write to I/O-region - rename `common.h` to `fas_common.h` as proposed in #220 0.30.9: diff --git a/extras/tests/simavr_based/test_issue208/expect.txt b/extras/tests/simavr_based/test_issue208/expect.txt index e08e8010..d36fdf33 100644 --- a/extras/tests/simavr_based/test_issue208/expect.txt +++ b/extras/tests/simavr_based/test_issue208/expect.txt @@ -2,17 +2,17 @@ DirB: 1*L->H, 0*H->L EnableA: 0*L->H, 0*H->L EnableB: 0*L->H, 0*H->L - StepA: 235909*L->H, 235909*H->L, Max High=11us Total High=1237097us + StepA: 235908*L->H, 235908*H->L, Max High=11us Total High=1237142us StepB: 2*L->H, 2*H->L, Max High=13us Total High=17us -Position[A]=37461 +Position[A]=37462 Position[B]=2 Time in FillISR max=769 us, total=1237221 us -Time in StepA max=11 us, total=1237097 us +Time in StepA max=11 us, total=1237142 us Time in StepB max=13 us, total=17 us -Time in StepISR max=7 us, total=1021996 us +Time in StepISR max=7 us, total=1021992 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 1240f3bc..81103a66 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=28us Total High=580688us - StepB: 64000*L->H, 64000*H->L, Max High=31us Total High=392513us - StepC: 64000*L->H, 64000*H->L, Max High=41us Total High=426208us + StepA: 64000*L->H, 64000*H->L, Max High=27us Total High=390370us + StepB: 64000*L->H, 64000*H->L, Max High=33us Total High=393554us + StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=618397us Position[A]=64000 Position[B]=64000 @@ -17,13 +17,13 @@ Time in EnableA max=233598 us, total=233598 us Time in EnableB max=246080 us, total=246080 us -Time in EnableC max=254235 us, total=254235 us +Time in EnableC max=254236 us, total=254236 us -Time in FillISR max=2686 us, total=2091889 us +Time in FillISR max=3102 us, total=2092792 us -Time in StepA max=28 us, total=580688 us +Time in StepA max=27 us, total=390370 us -Time in StepB max=31 us, total=392513 us +Time in StepB max=33 us, total=393554 us -Time in StepC max=41 us, total=426208 us +Time in StepC max=40 us, total=618397 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 323d519e..e8fd56dd 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,21 +2,21 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=5545us - StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5520us + StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5545us + StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=5535us Position[A]=1000 Position[B]=1000 Time in EnableA max=225398 us, total=225398 us -Time in EnableB max=238116 us, total=238116 us +Time in EnableB max=238117 us, total=238117 us -Time in FillISR max=2644 us, total=47652 us +Time in FillISR max=2644 us, total=47661 us -Time in StepA max=13 us, total=5545 us +Time in StepA max=14 us, total=5545 us -Time in StepB max=16 us, total=5520 us +Time in StepB max=15 us, total=5535 us Time in StepISR max=7 us, total=9224 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index 158958c3..ddcc9626 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,7 +2,7 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=12us Total High=5274us + StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=5270us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 @@ -10,7 +10,7 @@ Time in EnableA max=225399 us, total=225399 us Time in FillISR max=2015 us, total=27557 us -Time in StepA max=12 us, total=5274 us +Time in StepA max=10 us, total=5270 us Time in StepISR max=7 us, total=4581 us From dc13c7e57672ed6664587062e22c799fde777766 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Wed, 27 Dec 2023 11:22:25 +0100 Subject: [PATCH 34/85] bump version --- CHANGELOG.md | 2 +- library.properties | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 56b9c1c1..2de6b152 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.10: +0.30.10: - Unify code in stepperConnectToPin to fix bug mentioned in #221 This ensures setting of per stepper speed limit is working for avr variants without side effect of uncontrolled write to I/O-region diff --git a/library.properties b/library.properties index 79d79c3a..814926c0 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.9 +version=0.30.10 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes From 8c9f1de52c606991b6200a559889c3515073daa8 Mon Sep 17 00:00:00 2001 From: GarmischWg Date: Thu, 11 Jan 2024 23:54:59 +0800 Subject: [PATCH 35/85] Attempting add esp32s3 --- src/StepperISR_esp32s3_rmt.cpp | 653 +++++++++++++++++++++++++++++++++ src/fas_common.h | 45 ++- 2 files changed, 683 insertions(+), 15 deletions(-) create mode 100644 src/StepperISR_esp32s3_rmt.cpp diff --git a/src/StepperISR_esp32s3_rmt.cpp b/src/StepperISR_esp32s3_rmt.cpp new file mode 100644 index 00000000..c8130900 --- /dev/null +++ b/src/StepperISR_esp32s3_rmt.cpp @@ -0,0 +1,653 @@ +#include "StepperISR.h" +#ifdef SUPPORT_ESP32S3_RMT + +// #define TEST_MODE +// #define TRACE + +#include "test_probe.h" + +// The following concept is in use: +// +// The rmt buffer is split into two parts. +// Each part will hold one command (or part of). +// After the two parts an end marker is placed. +// +// Of these 32 bits, the low 16-bit entry is sent first and the high entry +// second. +// Every 16 bit entry defines with MSB the output level and the lower 15 bits +// the ticks. +// +// Important difference of esp32c3 (compared to esp32): +// - configuration updates need an conf_update strobe +// (apparently the manual is not correct by mentioning to set conf_update +// first) +// - if the end marker is hit in continuous mode, there is no end interrupt +// - there is no tick lost with the end marker +// - minimum periods as per relation 1 and 2 to be adhered to +// +// +#ifdef SUPPORT_ESP32S3_RMT +#define PART_SIZE 23 +#define RMT_MEM_SIZE 48 +#else +#error +#define PART_SIZE 31 +#define RMT_MEM_SIZE 64 +#endif + +// In order to avoid threshold/end interrupt on end, add one +#define enable_rmt_interrupts(channel) \ + { \ + RMT.tx_lim[channel].RMT_LIMIT = PART_SIZE + 2; \ + RMT.tx_conf[channel].conf_update = 1; \ + RMT.tx_conf[channel].conf_update = 0; \ + RMT.int_clr.val |= 0x101 << channel; \ + RMT.int_ena.val |= 0x101 << channel; \ + } +#define disable_rmt_interrupts(channel) \ + { RMT.int_ena.val &= ~(0x101 << channel); } + +void IRAM_ATTR StepperQueue::stop_rmt(bool both) { + // We are stopping the rmt by letting it run into the end at high speed. + // + // disable the interrupts + // rmt_set_tx_intr_en(channel, false); + // rmt_set_tx_thr_intr_en(channel, false, 0); + + // stop esp32 rmt, by let it hit the end + RMT.tx_conf[channel].tx_conti_mode = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + + // replace second part of buffer with pauses + uint32_t *data = FAS_RMT_MEM(channel); + uint8_t start = both ? 0 : PART_SIZE; + data = &data[start]; + for (uint8_t i = start; i < 2 * PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); + } + *data = 0; + + // as the rmt is not running anymore, mark it as stopped + _rmtStopped = true; +} + +static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, + uint32_t *data) { + if (!fill_part_one) { + data += PART_SIZE; + } + uint8_t rp = q->read_idx; + if (rp == q->next_write_idx) { + // no command in queue + if (fill_part_one) { + q->bufferContainsSteps[0] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // make a pause with approx. 1ms + // 258 ticks * 2 * 31 = 15996 @ 16MHz + // 347 ticks * 2 * 23 = 15962 @ 16MHz + *data++ = 0x010001 * (16000 / 2 / PART_SIZE); + } + } else { + q->stop_rmt(false); + } + return; + } + // Process command + struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; + + if (e_curr->toggle_dir) { + // the command requests dir pin toggle + // This is ok only, if the ongoing command does not contain steps + if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { + // So we need a pause. change the finished read entry into a pause + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * + ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE)); + } + return; + } + // The ongoing command does not contain steps, so change dir here should be + // ok + gpio_num_t dirPin = (gpio_num_t)q->dirPin; + gpio_set_level(dirPin, gpio_get_level(dirPin) ^ 1); + // and delete the request + e_curr->toggle_dir = 0; + } + + uint8_t steps = e_curr->steps; + uint16_t ticks = e_curr->ticks; + // if (steps != 0) { + // PROBE_1_TOGGLE; + //} + uint32_t last_entry; + if (steps == 0) { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + // Perhaps the rmt performs look ahead + ticks -= (PART_SIZE - 2) * 4 + 8; + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + last_entry = ticks_l; + last_entry <<= 16; + last_entry |= ticks_r; + *data++ = last_entry; + for (uint8_t i = 1; i < PART_SIZE - 1; i++) { + *data++ = 0x00020002; + } + last_entry = 0x00040004; + } else { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; + if (ticks == 0xffff) { + // special treatment for this case, because an rmt entry can only cover up + // to 0xfffe ticks every step must be minimum split into two rmt entries, + // so at max PART/2 steps can be done. + if (steps < PART_SIZE / 2) { + for (uint8_t i = 1; i < steps; i++) { + // steps-1 iterations + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; + } + *data++ = 0x40007fff | 0x8000; + uint16_t delta = PART_SIZE - 2 * steps; + delta <<= 5; + *data++ = 0x20002000 - delta; + // 2*(steps - 1) + 1 already stored => 2*steps - 1 + // and after this for loop one entry added => 2*steps + for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { + *data++ = 0x00100010; + } + last_entry = 0x00100010; + steps = 0; + } else { + steps -= PART_SIZE / 2; + for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; + } + *data++ = 0x40007fff | 0x8000; + last_entry = 0x20002000; + } + } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { + uint8_t steps_to_do = steps; + if (steps > PART_SIZE) { + steps_to_do /= 2; + } + + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 1; i < steps_to_do; i++) { + *data++ = rmt_entry; + } + uint32_t delta = (PART_SIZE - steps_to_do) * 4 + 8; + delta <<= 16; // shift in upper 16bit + *data++ = rmt_entry - delta; + for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { + *data++ = 0x00020002; + } + last_entry = 0x00040004; + steps -= steps_to_do; + } else { + // either >= 2*PART_SIZE or = PART_SIZE + // every entry one step + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + *data++ = rmt_entry; + } + last_entry = rmt_entry; + steps -= PART_SIZE; + } + } + // No tick lost mentioned for esp32c3 + // if (!fill_part_one) { + // Note: When enabling the continuous transmission mode by setting + // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the + // channel continuously, that is, from the first byte to the last one, + // then from the first to the last again, and so on. In this mode, there + // will be an idle level lasting one clk_div cycle between N and N+1 + // transmissions. + // last_entry -= 1; + // } + *data = last_entry; + + // Data is complete + if (steps == 0) { + // The command has been completed + if (e_curr->repeat_entry == 0) { + q->read_idx = rp + 1; + } + } else { + e_curr->steps = steps; + } +} + +#if !defined(RMT_CHANNEL_MEM) && !defined(SUPPORT_ESP32C3_RMT) +#define RMT_LIMIT tx_lim +#define RMT_FIFO apb_fifo_mask +#else +#define RMT_LIMIT limit +#define RMT_FIFO fifo_mask +#endif + +// The threshold interrupts are happening in the "middle" of the previous entry. +// Best strategy: +// 1. enable threshold interrupt with PART_SIZE+2 +// 2. On every threshold interrupt toggle PART_SIZE and PART_SIZE+1 +// This way the first interrupt happens on the first entry of second half, +// and the second interrupt on the first entry of the first half. +// Afterwards alternating. This way the end interrupt is always "half buffer +// away" from the threshold interrupt +#ifdef TEST_MODE +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_1_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + /* demonstrate modification of RAM */ \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + mem[PART_SIZE] = 0x33ff33ff; \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 0; \ + } +#else +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + disable_rmt_interrupts(q->channel); \ + q->_isRunning = false; \ + PROBE_1_TOGGLE; \ + PROBE_2_TOGGLE; \ + PROBE_3_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + apply_command(q, false, mem); \ + /* demonstrate modification of RAM */ \ + /*mem[PART_SIZE] = 0x33fff3ff; */ \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + apply_command(q, true, mem); \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 0; \ + } +#endif + +static void IRAM_ATTR tx_intr_handler(void *arg) { + uint32_t mask = RMT.int_st.val; + RMT.int_clr.val = mask; + PROCESS_CHANNEL(0); + PROCESS_CHANNEL(1); +#if QUEUES_RMT >= 4 + PROCESS_CHANNEL(2); + PROCESS_CHANNEL(3); +#endif +#if QUEUES_RMT == 8 + PROCESS_CHANNEL(4); + PROCESS_CHANNEL(5); + PROCESS_CHANNEL(6); + PROCESS_CHANNEL(7); +#endif +} + +void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { +#ifdef TEST_PROBE_1 + if (channel_num == 0) { + pinMode(TEST_PROBE_1, OUTPUT); + PROBE_1(HIGH); + delay(10); + PROBE_1(LOW); + delay(5); + PROBE_1(HIGH); + delay(5); + PROBE_1(LOW); + delay(5); + } +#endif +#ifdef TEST_PROBE_2 + pinMode(TEST_PROBE_2, OUTPUT); + PROBE_2(LOW); +#endif +#ifdef TEST_PROBE_3 + pinMode(TEST_PROBE_3, OUTPUT); + PROBE_3(LOW); +#endif + + _initVars(); + _step_pin = step_pin; + // digitalWrite(step_pin, LOW); + // pinMode(step_pin, OUTPUT); + + channel = (rmt_channel_t)channel_num; + + if (channel_num == 0) { + periph_module_enable(PERIPH_RMT_MODULE); + } + // APB_CLOCK=80 MHz + // CLK_DIV = APB_CLOCK/5 = 16 MHz + // + // Relation 1 in esp32c3 technical reference: + // 3 * T_APB + 5 * T_RMT_CLK < period * T_CLK_DIV + // => 8 * T_APB < period * T_APB*5 + // => period > 8/5 + // => period >= 2 + // + // Relation 2 in esp32c3 technical reference before end marker: + // 6 * T_APB + 12 * T_RMT_CLK < period * T_CLK_DIV + // => 18 * T_APB < period * T_APB*5 + // => period > 18/5 + // => period >= 4 + // + disable_rmt_interrupts(channel); + rmt_set_source_clk(channel, RMT_BASECLK_APB); + rmt_set_clk_div(channel, 5); + rmt_set_mem_block_num(channel, 1); + rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); + rmt_tx_stop(channel); + rmt_memory_rw_rst(channel); + // rmt_rx_stop(channel); TX only channel ! + // rmt_tx_memory_reset is not defined in arduino V340 and based on test + // result not needed rmt_tx_memory_reset(channel); + if (channel_num == 0) { + rmt_isr_register(tx_intr_handler, NULL, + ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); + RMT.sys_conf.fifo_mask = 1; // disable fifo mode + } + + _isRunning = false; + _rmtStopped = true; + + connect_rmt(); + +#ifdef TEST_MODE + if (channel == 0) { + RMT.tx_conf[channel].mem_rd_rst = 1; + RMT.tx_conf[channel].mem_rd_rst = 0; + uint32_t *mem = FAS_RMT_MEM(channel); + // Fill the buffer with a significant pattern for debugging + for (uint8_t i = 0; i < PART_SIZE; i++) { + *mem++ = 0x7fffffff; + } + for (uint8_t i = PART_SIZE; i < 2 * PART_SIZE; i++) { + *mem++ = 0x3fffdfff; + } + // without end marker it does not loop + *mem++ = 0; + *mem++ = 0; + + // conti mode is accepted with the conf_update 1 strobe + RMT.tx_conf[channel].tx_conti_mode = 1; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + enable_rmt_interrupts(channel); + // tx_start does not need conf_update + PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 + RMT.tx_conf[channel].tx_start = 1; + + delay(1000); + if (false) { + mem--; + mem--; + // destroy end marker => no end interrupt, no repeat + *mem++ = 0x00010001; + *mem = 0x00010001; + } + if (true) { + // just clear conti mode => causes end interrupt, no repeat + RMT.tx_conf[channel].tx_conti_mode = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + } + delay(1000); + // actually no need to enable/disable interrupts. + // and this seems to avoid some pitfalls + + // This runs the RMT buffer once + RMT.tx_conf[channel].tx_conti_mode = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].tx_start = 1; + while (true) { + delay(1000); + PROBE_1_TOGGLE; + } + } +#endif +} + +void StepperQueue::connect_rmt() { + RMT.tx_conf[channel].idle_out_lv = 0; + RMT.tx_conf[channel].idle_out_en = 1; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // RMT.tx_conf[channel].mem_tx_wrap_en = 0; +#ifndef __ESP32_IDF_V44__ + rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); +#else + rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false); +#endif + +#ifdef TEST_MODE + // here gpio is 0 + delay(1); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 1; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 1 + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 0 + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 0 + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 1; + RMT.tx_conf[channel].idle_out_en = 1; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 1 + delay(2); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + // here gpio is 0 + delay(2); + PROBE_3_TOGGLE; +#endif +} + +void StepperQueue::disconnect_rmt() { + disable_rmt_interrupts(channel); +#ifndef __ESP32_IDF_V44__ +// rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1); +#else +// rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); +#endif + RMT.tx_conf[channel].idle_out_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; +} + +void StepperQueue::startQueue_rmt() { +#ifdef TRACE + USBSerial.println("START"); +#endif +#ifdef TEST_PROBE_2 + PROBE_2(LOW); +#endif +#ifdef TEST_PROBE_3 + PROBE_3(LOW); +#endif +#if defined(TEST_PROBE_1) && defined(TEST_PROBE_2) && defined(TEST_PROBE_3) + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_2_TOGGLE; + delay(2); + PROBE_2_TOGGLE; + delay(2); + PROBE_3_TOGGLE; + delay(3); + PROBE_3_TOGGLE; + delay(3); +#endif + rmt_tx_stop(channel); + // rmt_rx_stop(channel); + RMT.tx_conf[channel].mem_rd_rst = 1; + RMT.tx_conf[channel].mem_rd_rst = 0; + uint32_t *mem = FAS_RMT_MEM(channel); +// #define TRACE +#ifdef TRACE + // Fill the buffer with a significant pattern for debugging + // Keep it for now + for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { + // mem[i] = 0x0fff8fff; + // mem[i + 1] = 0x7fff8fff; + } +#endif + // Write end marker + mem[2 * PART_SIZE] = 0; + mem[2 * PART_SIZE + 1] = 0; + _isRunning = true; + _rmtStopped = false; + disable_rmt_interrupts(channel); + RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + +#ifdef TRACE + USBSerial.print("Queue:"); + USBSerial.print(read_idx); + USBSerial.print('/'); + USBSerial.println(next_write_idx); + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } +#endif + + // set dirpin toggle here + uint8_t rp = read_idx; + if (rp == next_write_idx) { + // nothing to do ? + // Should not happen, so bail + return; + } + if (entry[rp & QUEUE_LEN_MASK].toggle_dir) { + gpio_set_level((gpio_num_t)dirPin, gpio_get_level((gpio_num_t)dirPin) ^ 1); + entry[rp & QUEUE_LEN_MASK].toggle_dir = false; + } + + bufferContainsSteps[0] = true; + bufferContainsSteps[1] = true; + apply_command(this, true, mem); + +#ifdef TRACE + USBSerial.print(RMT.tx_conf[channel].val, BIN); + USBSerial.println(' '); + USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en); + USBSerial.println(' '); + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } + if (!isRunning()) { + USBSerial.println("STOPPED 1"); + } +#endif + + apply_command(this, false, mem); + +#ifdef TRACE + USBSerial.print(RMT.tx_conf[channel].val, BIN); + USBSerial.print(' '); + USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en); + USBSerial.println(' '); + + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } +#endif + enable_rmt_interrupts(channel); + +#ifdef TRACE + USBSerial.print("Interrupt enable:"); + USBSerial.println(RMT.int_ena.val, BIN); + USBSerial.print("Interrupt status:"); + USBSerial.println(RMT.int_st.val, BIN); + USBSerial.print("Threshold: 0x"); + USBSerial.println(RMT.tx_lim[channel].val, HEX); +#endif + + // This starts the rmt module + RMT.tx_conf[channel].tx_conti_mode = 1; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.tx_conf[channel].conf_update = 1; + RMT.tx_conf[channel].conf_update = 0; + + PROBE_1_TOGGLE; + RMT.tx_conf[channel].tx_start = 1; +} +void StepperQueue::forceStop_rmt() { + stop_rmt(true); + + // and empty the buffer + read_idx = next_write_idx; +} +bool StepperQueue::isReadyForCommands_rmt() { + if (_isRunning) { + return !_rmtStopped; + } + return true; +} +uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; } +#endif diff --git a/src/fas_common.h b/src/fas_common.h index 880ab1d4..c7705313 100644 --- a/src/fas_common.h +++ b/src/fas_common.h @@ -11,20 +11,20 @@ // ticks is multiplied by (1/TICKS_PER_S) in s // If steps is 0, then a pause is generated struct stepper_command_s { - uint16_t ticks; - uint8_t steps; - bool count_up; + uint16_t ticks; + uint8_t steps; + bool count_up; }; struct actual_ticks_s { - uint32_t ticks; // ticks == 0 means standstill - bool count_up; + uint32_t ticks; // ticks == 0 means standstill + bool count_up; }; struct queue_end_s { - volatile int32_t pos; // in steps - volatile bool count_up; - volatile bool dir; + volatile int32_t pos; // in steps + volatile bool count_up; + volatile bool dir; }; // use own min/max/abs function, because the lib versions are messed up @@ -55,6 +55,7 @@ struct queue_end_s { #include #include #include + #include "../extras/tests/pc_based/stubs.h" // For pc-based testing, the macro TEST is defined. The pc-based testing does @@ -136,8 +137,8 @@ struct queue_end_s { #elif CONFIG_IDF_TARGET_ESP32S2 #define SUPPORT_ESP32S3_PULSE_COUNTER #define SUPPORT_ESP32_RMT -#include #include +#include #include #include #include @@ -160,9 +161,17 @@ struct queue_end_s { #include #include +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32S3_RMT +#include #include +#include +#include +#include +#define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) + #define QUEUES_MCPWM_PCNT 4 -#define QUEUES_RMT 0 +#define QUEUES_RMT 2 // have support for pulse counter #define SUPPORT_ESP32_PULSE_COUNTER @@ -344,12 +353,13 @@ struct queue_end_s { // this is an arduino platform, so include the Arduino.h header file #include + #include "AVRStepperPins.h" // for AVR processors a reentrant version of disabling/enabling interrupts is // used #define fasDisableInterrupts() \ - uint8_t prevSREG = SREG; \ - cli() + uint8_t prevSREG = SREG; \ + cli() #define fasEnableInterrupts() SREG = prevSREG // Here are shorthand definitions for number of queues, the queues/channel @@ -382,7 +392,8 @@ struct queue_end_s { #define NUM_QUEUES 2 #define fas_queue_A fas_queue[0] #define fas_queue_B fas_queue[1] -enum channels { channelA, channelB }; +enum channels { channelA, + channelB }; //========================================================================== // // AVR derivate ATmega 2560 @@ -395,7 +406,9 @@ enum channels { channelA, channelB }; #define fas_queue_A fas_queue[0] #define fas_queue_B fas_queue[1] #define fas_queue_C fas_queue[2] -enum channels { channelA, channelB, channelC }; +enum channels { channelA, + channelB, + channelC }; //========================================================================== // // AVR derivate ATmega 32U4 @@ -407,7 +420,9 @@ enum channels { channelA, channelB, channelC }; #define fas_queue_A fas_queue[0] #define fas_queue_B fas_queue[1] #define fas_queue_C fas_queue[2] -enum channels { channelA, channelB, channelC }; +enum channels { channelA, + channelB, + channelC }; //========================================================================== // // For all unsupported AVR derivates From fca7069c15ee6691677fa2cc499febfeb05e7605 Mon Sep 17 00:00:00 2001 From: GarmischWg Date: Fri, 12 Jan 2024 14:42:52 +0800 Subject: [PATCH 36/85] disabled ESP32 rmt for esp32s3 --- src/StepperISR_esp32_rmt.cpp | 792 +++++++++++++++++------------------ 1 file changed, 396 insertions(+), 396 deletions(-) diff --git a/src/StepperISR_esp32_rmt.cpp b/src/StepperISR_esp32_rmt.cpp index ce4371ec..66bd27c3 100644 --- a/src/StepperISR_esp32_rmt.cpp +++ b/src/StepperISR_esp32_rmt.cpp @@ -1,7 +1,7 @@ #include "StepperISR.h" -#if defined(SUPPORT_ESP32_RMT) && !defined(SUPPORT_ESP32C3_RMT) +#if defined(SUPPORT_ESP32_RMT) && !defined(SUPPORT_ESP32C3_RMT) && !defined(SUPPORT_ESP32S3_RMT) -//#define TEST_MODE +// #define TEST_MODE #include "test_probe.h" @@ -22,179 +22,179 @@ #define PART_SIZE 31 void IRAM_ATTR StepperQueue::stop_rmt(bool both) { - // We are stopping the rmt by letting it run into the end at high speed. - // - // disable the interrupts - // rmt_set_tx_intr_en(channel, false); - // rmt_set_tx_thr_intr_en(channel, false, 0); - - // stop esp32 rmt, by let it hit the end - RMT.conf_ch[channel].conf1.tx_conti_mode = 0; - - // replace second part of buffer with pauses - uint32_t *data = FAS_RMT_MEM(channel); - uint8_t start = both ? 0 : PART_SIZE; - data = &data[start]; - for (uint8_t i = start; i < 2 * PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); - } - *data = 0; - - // as the rmt is not running anymore, mark it as stopped - _rmtStopped = true; + // We are stopping the rmt by letting it run into the end at high speed. + // + // disable the interrupts + // rmt_set_tx_intr_en(channel, false); + // rmt_set_tx_thr_intr_en(channel, false, 0); + + // stop esp32 rmt, by let it hit the end + RMT.conf_ch[channel].conf1.tx_conti_mode = 0; + + // replace second part of buffer with pauses + uint32_t *data = FAS_RMT_MEM(channel); + uint8_t start = both ? 0 : PART_SIZE; + data = &data[start]; + for (uint8_t i = start; i < 2 * PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); + } + *data = 0; + + // as the rmt is not running anymore, mark it as stopped + _rmtStopped = true; } static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, uint32_t *data) { - if (!fill_part_one) { - data += PART_SIZE; - } - uint8_t rp = q->read_idx; - if (rp == q->next_write_idx) { - // no command in queue - if (fill_part_one) { - q->bufferContainsSteps[0] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // make a pause with approx. 1ms - // 258 ticks * 2 * 31 = 15996 @ 16MHz - *data++ = 0x01020102; - } - } else { - q->stop_rmt(false); + if (!fill_part_one) { + data += PART_SIZE; } - return; - } - // Process command - struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; - - if (e_curr->toggle_dir) { - // the command requests dir pin toggle - // This is ok only, if the ongoing command does not contain steps - if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { - // So we need a pause. change the finished read entry into a pause - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); - } - return; + uint8_t rp = q->read_idx; + if (rp == q->next_write_idx) { + // no command in queue + if (fill_part_one) { + q->bufferContainsSteps[0] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // make a pause with approx. 1ms + // 258 ticks * 2 * 31 = 15996 @ 16MHz + *data++ = 0x01020102; + } + } else { + q->stop_rmt(false); + } + return; } - // The ongoing command does not contain steps, so change dir here should be - // ok - gpio_num_t dirPin = (gpio_num_t)q->dirPin; - gpio_set_level(dirPin, gpio_get_level(dirPin) ^ 1); - // and delete the request - e_curr->toggle_dir = 0; - } - - uint8_t steps = e_curr->steps; - uint16_t ticks = e_curr->ticks; - // if (steps != 0) { - // PROBE_2_TOGGLE; - //} - uint32_t last_entry; - if (steps == 0) { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - // two pauses à 3 ticks. the 2 for debugging - *data++ = 0x00010002; - ticks -= 3; + // Process command + struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; + + if (e_curr->toggle_dir) { + // the command requests dir pin toggle + // This is ok only, if the ongoing command does not contain steps + if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { + // So we need a pause. change the finished read entry into a pause + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); + } + return; + } + // The ongoing command does not contain steps, so change dir here should be + // ok + gpio_num_t dirPin = (gpio_num_t)q->dirPin; + gpio_set_level(dirPin, gpio_get_level(dirPin) ^ 1); + // and delete the request + e_curr->toggle_dir = 0; } - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - last_entry = ticks_l; - last_entry <<= 16; - last_entry |= ticks_r; - } else { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; - if (ticks == 0xffff) { - // special treatment for this case, because an rmt entry can only cover up - // to 0xfffe ticks every step must be minimum split into two rmt entries, - // so at max PART/2 steps can be done. - if (steps < PART_SIZE / 2) { - for (uint8_t i = 1; i < steps; i++) { - // steps-1 iterations - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; + + uint8_t steps = e_curr->steps; + uint16_t ticks = e_curr->ticks; + // if (steps != 0) { + // PROBE_2_TOGGLE; + //} + uint32_t last_entry; + if (steps == 0) { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + // two pauses à 3 ticks. the 2 for debugging + *data++ = 0x00010002; + ticks -= 3; } - *data++ = 0x40007fff | 0x8000; - uint16_t delta = PART_SIZE - 2 * steps; - delta <<= 5; - *data++ = 0x20002000 - delta; - // 2*(steps - 1) + 1 already stored => 2*steps - 1 - // and after this for loop one entry added => 2*steps - for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { - *data++ = 0x00100010; + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + last_entry = ticks_l; + last_entry <<= 16; + last_entry |= ticks_r; + } else { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; + if (ticks == 0xffff) { + // special treatment for this case, because an rmt entry can only cover up + // to 0xfffe ticks every step must be minimum split into two rmt entries, + // so at max PART/2 steps can be done. + if (steps < PART_SIZE / 2) { + for (uint8_t i = 1; i < steps; i++) { + // steps-1 iterations + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; + } + *data++ = 0x40007fff | 0x8000; + uint16_t delta = PART_SIZE - 2 * steps; + delta <<= 5; + *data++ = 0x20002000 - delta; + // 2*(steps - 1) + 1 already stored => 2*steps - 1 + // and after this for loop one entry added => 2*steps + for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { + *data++ = 0x00100010; + } + last_entry = 0x00100010; + steps = 0; + } else { + steps -= PART_SIZE / 2; + for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; + } + *data++ = 0x40007fff | 0x8000; + last_entry = 0x20002000; + } + } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { + uint8_t steps_to_do = steps; + if (steps > PART_SIZE) { + steps_to_do /= 2; + } + + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 1; i < steps_to_do; i++) { + *data++ = rmt_entry; + } + uint32_t delta = PART_SIZE - steps_to_do; + delta <<= 18; // shift in upper 16bit and multiply with 4 + *data++ = rmt_entry - delta; + for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { + *data++ = 0x00020002; + } + last_entry = 0x00020002; + steps -= steps_to_do; + } else { + // either >= 2*PART_SIZE or = PART_SIZE + // every entry one step + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + *data++ = rmt_entry; + } + last_entry = rmt_entry; + steps -= PART_SIZE; } - last_entry = 0x00100010; - steps = 0; - } else { - steps -= PART_SIZE / 2; - for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; + } + if (!fill_part_one) { + // Note: When enabling the continuous transmission mode by setting + // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the + // channel continuously, that is, from the first byte to the last one, + // then from the first to the last again, and so on. In this mode, there + // will be an idle level lasting one clk_div cycle between N and N+1 + // transmissions. + last_entry -= 1; + } + *data = last_entry; + + // Data is complete + if (steps == 0) { + // The command has been completed + if (e_curr->repeat_entry == 0) { + q->read_idx = rp + 1; } - *data++ = 0x40007fff | 0x8000; - last_entry = 0x20002000; - } - } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { - uint8_t steps_to_do = steps; - if (steps > PART_SIZE) { - steps_to_do /= 2; - } - - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 1; i < steps_to_do; i++) { - *data++ = rmt_entry; - } - uint32_t delta = PART_SIZE - steps_to_do; - delta <<= 18; // shift in upper 16bit and multiply with 4 - *data++ = rmt_entry - delta; - for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; - } - last_entry = 0x00020002; - steps -= steps_to_do; } else { - // either >= 2*PART_SIZE or = PART_SIZE - // every entry one step - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - *data++ = rmt_entry; - } - last_entry = rmt_entry; - steps -= PART_SIZE; + e_curr->steps = steps; } - } - if (!fill_part_one) { - // Note: When enabling the continuous transmission mode by setting - // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the - // channel continuously, that is, from the first byte to the last one, - // then from the first to the last again, and so on. In this mode, there - // will be an idle level lasting one clk_div cycle between N and N+1 - // transmissions. - last_entry -= 1; - } - *data = last_entry; - - // Data is complete - if (steps == 0) { - // The command has been completed - if (e_curr->repeat_entry == 0) { - q->read_idx = rp + 1; - } - } else { - e_curr->steps = steps; - } } #ifndef RMT_CHANNEL_MEM @@ -206,306 +206,306 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, #endif #ifdef TEST_MODE -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_2_TOGGLE; \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - PROBE_3_TOGGLE; \ - /* now repeat the interrupt at buffer size + end marker */ \ - RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ - } +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_2_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + PROBE_3_TOGGLE; \ + /* now repeat the interrupt at buffer size + end marker */ \ + RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ + } #else -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_2_TOGGLE; \ - StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - if (q->_rmtStopped) { \ - rmt_set_tx_intr_en(q->channel, false); \ - rmt_set_tx_thr_intr_en(q->channel, false, PART_SIZE + 1); \ - q->_isRunning = false; \ - PROBE_1_TOGGLE; \ - } else { \ - apply_command(q, false, FAS_RMT_MEM(ch)); \ - } \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - PROBE_3_TOGGLE; \ - StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - if (!q->_rmtStopped) { \ - apply_command(q, true, FAS_RMT_MEM(ch)); \ - /* now repeat the interrupt at buffer size + end marker */ \ - RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ - } \ - } +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_2_TOGGLE; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + if (q->_rmtStopped) { \ + rmt_set_tx_intr_en(q->channel, false); \ + rmt_set_tx_thr_intr_en(q->channel, false, PART_SIZE + 1); \ + q->_isRunning = false; \ + PROBE_1_TOGGLE; \ + } else { \ + apply_command(q, false, FAS_RMT_MEM(ch)); \ + } \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + PROBE_3_TOGGLE; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + if (!q->_rmtStopped) { \ + apply_command(q, true, FAS_RMT_MEM(ch)); \ + /* now repeat the interrupt at buffer size + end marker */ \ + RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ + } \ + } #endif static void IRAM_ATTR tx_intr_handler(void *arg) { - uint32_t mask = RMT.int_st.val; - RMT.int_clr.val = mask; - PROCESS_CHANNEL(0); - PROCESS_CHANNEL(1); + uint32_t mask = RMT.int_st.val; + RMT.int_clr.val = mask; + PROCESS_CHANNEL(0); + PROCESS_CHANNEL(1); #if QUEUES_RMT >= 4 - PROCESS_CHANNEL(2); - PROCESS_CHANNEL(3); + PROCESS_CHANNEL(2); + PROCESS_CHANNEL(3); #endif #if QUEUES_RMT == 8 - PROCESS_CHANNEL(4); - PROCESS_CHANNEL(5); - PROCESS_CHANNEL(6); - PROCESS_CHANNEL(7); + PROCESS_CHANNEL(4); + PROCESS_CHANNEL(5); + PROCESS_CHANNEL(6); + PROCESS_CHANNEL(7); #endif } void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { #ifdef TEST_PROBE_1 - if (channel_num == 0) { - pinMode(TEST_PROBE_1, OUTPUT); - PROBE_1(HIGH); - delay(10); - PROBE_1(LOW); - delay(5); - PROBE_1(HIGH); - delay(5); - PROBE_1(LOW); - delay(5); - } + if (channel_num == 0) { + pinMode(TEST_PROBE_1, OUTPUT); + PROBE_1(HIGH); + delay(10); + PROBE_1(LOW); + delay(5); + PROBE_1(HIGH); + delay(5); + PROBE_1(LOW); + delay(5); + } #endif #ifdef TEST_PROBE_2 - pinMode(TEST_PROBE_2, OUTPUT); - PROBE_2(LOW); + pinMode(TEST_PROBE_2, OUTPUT); + PROBE_2(LOW); #endif #ifdef TEST_PROBE_3 - pinMode(TEST_PROBE_3, OUTPUT); - PROBE_3(LOW); + pinMode(TEST_PROBE_3, OUTPUT); + PROBE_3(LOW); #endif - _initVars(); - _step_pin = step_pin; - digitalWrite(step_pin, LOW); - pinMode(step_pin, OUTPUT); - - channel = (rmt_channel_t)channel_num; - - if (channel_num == 0) { - periph_module_enable(PERIPH_RMT_MODULE); - } - // 80 MHz/5 = 16 MHz - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); - rmt_set_source_clk(channel, RMT_BASECLK_APB); - rmt_set_clk_div(channel, 5); - rmt_set_mem_block_num(channel, 1); - rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); - rmt_tx_stop(channel); - rmt_rx_stop(channel); - // rmt_tx_memory_reset is not defined in arduino V340 and based on test result - // not needed rmt_tx_memory_reset(channel); - if (channel_num == 0) { - rmt_isr_register(tx_intr_handler, NULL, - ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); - RMT.apb_conf.RMT_FIFO = 1; // disable fifo mode - RMT.apb_conf.mem_tx_wrap_en = 0; - } - - _isRunning = false; - _rmtStopped = true; - - connect(); + _initVars(); + _step_pin = step_pin; + digitalWrite(step_pin, LOW); + pinMode(step_pin, OUTPUT); -#ifdef TEST_MODE - if (channel == 0) { - uint32_t *mem = FAS_RMT_MEM(channel); - // Fill the buffer with a significant pattern for debugging - for (uint8_t i = 0; i < 32; i += 2) { - *mem++ = 0x7fffffff; - *mem++ = 0x7fff7fff; - } - for (uint8_t i = 32; i < 62; i += 2) { - *mem++ = 0x3fffdfff; - *mem++ = 0x3fff3fff; - } - // without end marker it does not loop - *mem++ = 0; - *mem++ = 0; - RMT.conf_ch[channel].conf1.tx_conti_mode = 1; - rmt_set_tx_intr_en(channel, true); - rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); - // rmt_tx_start(channel, true); - PROBE_2_TOGGLE; - - delay(1000); - if (false) { - mem--; - mem--; - // destroy end marker => no end interrupt, no repeat - *mem++ = 0x00010001; - *mem = 0x00010001; + channel = (rmt_channel_t)channel_num; + + if (channel_num == 0) { + periph_module_enable(PERIPH_RMT_MODULE); } - if (true) { - // just clear conti mode => causes end interrup, no repeat - RMT.conf_ch[channel].conf1.tx_conti_mode = 0; + // 80 MHz/5 = 16 MHz + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); + rmt_set_source_clk(channel, RMT_BASECLK_APB); + rmt_set_clk_div(channel, 5); + rmt_set_mem_block_num(channel, 1); + rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); + rmt_tx_stop(channel); + rmt_rx_stop(channel); + // rmt_tx_memory_reset is not defined in arduino V340 and based on test result + // not needed rmt_tx_memory_reset(channel); + if (channel_num == 0) { + rmt_isr_register(tx_intr_handler, NULL, + ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); + RMT.apb_conf.RMT_FIFO = 1; // disable fifo mode + RMT.apb_conf.mem_tx_wrap_en = 0; } - delay(1000); - // actually no need to enable/disable interrupts. - // and this seems to avoid some pitfalls - // This runs the RMT buffer once - RMT.conf_ch[channel].conf1.tx_conti_mode = 1; - delay(1); - RMT.conf_ch[channel].conf1.tx_conti_mode = 0; - while (true) { - delay(1); + _isRunning = false; + _rmtStopped = true; + + connect(); + +#ifdef TEST_MODE + if (channel == 0) { + uint32_t *mem = FAS_RMT_MEM(channel); + // Fill the buffer with a significant pattern for debugging + for (uint8_t i = 0; i < 32; i += 2) { + *mem++ = 0x7fffffff; + *mem++ = 0x7fff7fff; + } + for (uint8_t i = 32; i < 62; i += 2) { + *mem++ = 0x3fffdfff; + *mem++ = 0x3fff3fff; + } + // without end marker it does not loop + *mem++ = 0; + *mem++ = 0; + RMT.conf_ch[channel].conf1.tx_conti_mode = 1; + rmt_set_tx_intr_en(channel, true); + rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + // rmt_tx_start(channel, true); + PROBE_2_TOGGLE; + + delay(1000); + if (false) { + mem--; + mem--; + // destroy end marker => no end interrupt, no repeat + *mem++ = 0x00010001; + *mem = 0x00010001; + } + if (true) { + // just clear conti mode => causes end interrup, no repeat + RMT.conf_ch[channel].conf1.tx_conti_mode = 0; + } + delay(1000); + // actually no need to enable/disable interrupts. + // and this seems to avoid some pitfalls + + // This runs the RMT buffer once + RMT.conf_ch[channel].conf1.tx_conti_mode = 1; + delay(1); + RMT.conf_ch[channel].conf1.tx_conti_mode = 0; + while (true) { + delay(1); + } } - } #endif } void StepperQueue::connect_rmt() { - // rmt_set_tx_intr_en(channel, true); - // rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); - RMT.conf_ch[channel].conf1.idle_out_lv = 0; - RMT.conf_ch[channel].conf1.idle_out_en = 1; + // rmt_set_tx_intr_en(channel, true); + // rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + RMT.conf_ch[channel].conf1.idle_out_lv = 0; + RMT.conf_ch[channel].conf1.idle_out_en = 1; #ifndef __ESP32_IDF_V44__ - rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); + rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); #else - rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false); + rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false); #endif } void StepperQueue::disconnect_rmt() { - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); #ifndef __ESP32_IDF_V44__ // rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1); #else - rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); + rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); #endif - RMT.conf_ch[channel].conf1.idle_out_en = 0; + RMT.conf_ch[channel].conf1.idle_out_en = 0; } void StepperQueue::startQueue_rmt() { -//#define TRACE +// #define TRACE #ifdef TRACE - Serial.println("START"); + Serial.println("START"); #endif #ifdef TEST_PROBE_2 - PROBE_2(LOW); + PROBE_2(LOW); #endif #ifdef TEST_PROBE_3 - PROBE_3(LOW); + PROBE_3(LOW); #endif #ifdef TEST_PROBE_1 - delay(1); - PROBE_1_TOGGLE; - delay(1); - PROBE_1_TOGGLE; - delay(1); - PROBE_1_TOGGLE; - delay(1); + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); #endif - rmt_tx_stop(channel); - // rmt_rx_stop(channel); - rmt_memory_rw_rst(channel); - // the following assignment should not be needed; - // RMT.data_ch[channel] = 0; - uint32_t *mem = FAS_RMT_MEM(channel); - // Fill the buffer with a significant pattern for debugging - // Keep it for now - for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { - mem[i] = 0x0fff8fff; - mem[i + 1] = 0x7fff8fff; - } - mem[2 * PART_SIZE] = 0; - mem[2 * PART_SIZE + 1] = 0; - _isRunning = true; - _rmtStopped = false; - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, 0); - // RMT.apb_conf.mem_tx_wrap_en = 0; - -//#define TRACE + rmt_tx_stop(channel); + // rmt_rx_stop(channel); + rmt_memory_rw_rst(channel); + // the following assignment should not be needed; + // RMT.data_ch[channel] = 0; + uint32_t *mem = FAS_RMT_MEM(channel); + // Fill the buffer with a significant pattern for debugging + // Keep it for now + for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { + mem[i] = 0x0fff8fff; + mem[i + 1] = 0x7fff8fff; + } + mem[2 * PART_SIZE] = 0; + mem[2 * PART_SIZE + 1] = 0; + _isRunning = true; + _rmtStopped = false; + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, 0); + // RMT.apb_conf.mem_tx_wrap_en = 0; + +// #define TRACE #ifdef TRACE - Serial.print("Queue:"); - Serial.print(read_idx); - Serial.print('/'); - Serial.println(next_write_idx); - for (uint8_t i = 0; i < 64; i++) { - Serial.print(i); - Serial.print(' '); - Serial.println(mem[i], HEX); - } + Serial.print("Queue:"); + Serial.print(read_idx); + Serial.print('/'); + Serial.println(next_write_idx); + for (uint8_t i = 0; i < 64; i++) { + Serial.print(i); + Serial.print(' '); + Serial.println(mem[i], HEX); + } #endif - // set dirpin toggle here - uint8_t rp = read_idx; - if (rp == next_write_idx) { - // nothing to do ? - // Should not happen, so bail - return; - } - if (entry[rp & QUEUE_LEN_MASK].toggle_dir) { - gpio_set_level((gpio_num_t)dirPin, gpio_get_level((gpio_num_t)dirPin) ^ 1); - entry[rp & QUEUE_LEN_MASK].toggle_dir = false; - } - - bufferContainsSteps[0] = true; - bufferContainsSteps[1] = true; - apply_command(this, true, mem); + // set dirpin toggle here + uint8_t rp = read_idx; + if (rp == next_write_idx) { + // nothing to do ? + // Should not happen, so bail + return; + } + if (entry[rp & QUEUE_LEN_MASK].toggle_dir) { + gpio_set_level((gpio_num_t)dirPin, gpio_get_level((gpio_num_t)dirPin) ^ 1); + entry[rp & QUEUE_LEN_MASK].toggle_dir = false; + } + + bufferContainsSteps[0] = true; + bufferContainsSteps[1] = true; + apply_command(this, true, mem); #ifdef TRACE - Serial.print(RMT.conf_ch[channel].conf0.val, BIN); - Serial.print(' '); - Serial.print(RMT.conf_ch[channel].conf1.val, BIN); - Serial.println(' '); - Serial.print(RMT.apb_conf.mem_tx_wrap_en); - Serial.println(' '); - for (uint8_t i = 0; i < 64; i++) { - Serial.print(i); + Serial.print(RMT.conf_ch[channel].conf0.val, BIN); Serial.print(' '); - Serial.println(mem[i], HEX); - } - if (!isRunning()) { - Serial.println("STOPPED 1"); - } + Serial.print(RMT.conf_ch[channel].conf1.val, BIN); + Serial.println(' '); + Serial.print(RMT.apb_conf.mem_tx_wrap_en); + Serial.println(' '); + for (uint8_t i = 0; i < 64; i++) { + Serial.print(i); + Serial.print(' '); + Serial.println(mem[i], HEX); + } + if (!isRunning()) { + Serial.println("STOPPED 1"); + } #endif - apply_command(this, false, mem); + apply_command(this, false, mem); #ifdef TRACE - Serial.print(RMT.conf_ch[channel].conf0.val, BIN); - Serial.print(' '); - Serial.print(RMT.conf_ch[channel].conf1.val, BIN); - Serial.println(' '); - Serial.print(RMT.apb_conf.mem_tx_wrap_en); - Serial.println(' '); - - for (uint8_t i = 0; i < 64; i++) { - Serial.print(i); + Serial.print(RMT.conf_ch[channel].conf0.val, BIN); Serial.print(' '); - Serial.println(mem[i], HEX); - } + Serial.print(RMT.conf_ch[channel].conf1.val, BIN); + Serial.println(' '); + Serial.print(RMT.apb_conf.mem_tx_wrap_en); + Serial.println(' '); + + for (uint8_t i = 0; i < 64; i++) { + Serial.print(i); + Serial.print(' '); + Serial.println(mem[i], HEX); + } #endif - rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); - rmt_set_tx_intr_en(channel, true); - _rmtStopped = false; + rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + rmt_set_tx_intr_en(channel, true); + _rmtStopped = false; - // This starts the rmt module - RMT.conf_ch[channel].conf1.tx_conti_mode = 1; + // This starts the rmt module + RMT.conf_ch[channel].conf1.tx_conti_mode = 1; - RMT.conf_ch[channel].conf1.tx_start = 1; + RMT.conf_ch[channel].conf1.tx_start = 1; } void StepperQueue::forceStop_rmt() { - stop_rmt(true); + stop_rmt(true); - // and empty the buffer - read_idx = next_write_idx; + // and empty the buffer + read_idx = next_write_idx; } bool StepperQueue::isReadyForCommands_rmt() { - if (_isRunning) { - return !_rmtStopped; - } - return true; + if (_isRunning) { + return !_rmtStopped; + } + return true; } uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; } #endif From b5c2ad3d0b8faeff7585c2a0e086bcf1981afb29 Mon Sep 17 00:00:00 2001 From: GarmischWg Date: Fri, 12 Jan 2024 15:51:58 +0800 Subject: [PATCH 37/85] renaming register names --- src/StepperISR_esp32s3_rmt.cpp | 116 ++++++++++++++++----------------- 1 file changed, 58 insertions(+), 58 deletions(-) diff --git a/src/StepperISR_esp32s3_rmt.cpp b/src/StepperISR_esp32s3_rmt.cpp index c8130900..1c408893 100644 --- a/src/StepperISR_esp32s3_rmt.cpp +++ b/src/StepperISR_esp32s3_rmt.cpp @@ -36,13 +36,13 @@ #endif // In order to avoid threshold/end interrupt on end, add one -#define enable_rmt_interrupts(channel) \ - { \ - RMT.tx_lim[channel].RMT_LIMIT = PART_SIZE + 2; \ - RMT.tx_conf[channel].conf_update = 1; \ - RMT.tx_conf[channel].conf_update = 0; \ - RMT.int_clr.val |= 0x101 << channel; \ - RMT.int_ena.val |= 0x101 << channel; \ +#define enable_rmt_interrupts(channel) \ + { \ + RMT.chn_tx_lim[channel].RMT_LIMIT = PART_SIZE + 2; \ + RMT.chnconf0[channel].conf_update_n = 1; \ + RMT.chnconf0[channel].conf_update_n = 0; \ + RMT.int_clr.val |= 0x101 << channel; \ + RMT.int_ena.val |= 0x101 << channel; \ } #define disable_rmt_interrupts(channel) \ { RMT.int_ena.val &= ~(0x101 << channel); } @@ -55,9 +55,9 @@ void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // rmt_set_tx_thr_intr_en(channel, false, 0); // stop esp32 rmt, by let it hit the end - RMT.tx_conf[channel].tx_conti_mode = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].tx_conti_mode_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; // replace second part of buffer with pauses uint32_t *data = FAS_RMT_MEM(channel); @@ -230,12 +230,12 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, } } -#if !defined(RMT_CHANNEL_MEM) && !defined(SUPPORT_ESP32C3_RMT) +#if !defined(RMT_CHANNEL_MEM) && !defined(SUPPORT_ESP32S3_RMT) #define RMT_LIMIT tx_lim #define RMT_FIFO apb_fifo_mask #else -#define RMT_LIMIT limit -#define RMT_FIFO fifo_mask +#define RMT_LIMIT tx_lim_chn +#define RMT_FIFO apb_fifo_mask #endif // The threshold interrupts are happening in the "middle" of the previous entry. @@ -247,26 +247,26 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, // Afterwards alternating. This way the end interrupt is always "half buffer // away" from the threshold interrupt #ifdef TEST_MODE -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_1_TOGGLE; \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ - if (old_limit == PART_SIZE + 1) { \ - /* second half of buffer sent */ \ - PROBE_2_TOGGLE; \ - /* demonstrate modification of RAM */ \ - uint32_t *mem = FAS_RMT_MEM(ch); \ - mem[PART_SIZE] = 0x33ff33ff; \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ - } else { \ - /* first half of buffer sent */ \ - PROBE_3_TOGGLE; \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ - } \ - RMT.tx_conf[ch].conf_update = 1; \ - RMT.tx_conf[ch].conf_update = 0; \ +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_1_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + /* demonstrate modification of RAM */ \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + mem[PART_SIZE] = 0x33ff33ff; \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 0; \ } #else #define PROCESS_CHANNEL(ch) \ @@ -279,7 +279,7 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, PROBE_3_TOGGLE; \ } \ if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ + uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \ StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ uint32_t *mem = FAS_RMT_MEM(ch); \ if (old_limit == PART_SIZE + 1) { \ @@ -288,15 +288,15 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, apply_command(q, false, mem); \ /* demonstrate modification of RAM */ \ /*mem[PART_SIZE] = 0x33fff3ff; */ \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ } else { \ /* first half of buffer sent */ \ PROBE_3_TOGGLE; \ apply_command(q, true, mem); \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ } \ - RMT.tx_conf[ch].conf_update = 1; \ - RMT.tx_conf[ch].conf_update = 0; \ + RMT.chnconf0[ch].conf_update_n = 1; \ + RMT.chnconf0[ch].conf_update_n = 0; \ } #endif @@ -378,7 +378,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { if (channel_num == 0) { rmt_isr_register(tx_intr_handler, NULL, ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); - RMT.sys_conf.fifo_mask = 1; // disable fifo mode + RMT.sys_conf.apb_fifo_mask = 1; // disable fifo mode } _isRunning = false; @@ -446,10 +446,10 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { } void StepperQueue::connect_rmt() { - RMT.tx_conf[channel].idle_out_lv = 0; - RMT.tx_conf[channel].idle_out_en = 1; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].idle_out_lv_n = 0; + RMT.chnconf0[channel].idle_out_en_n = 1; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; // RMT.tx_conf[channel].mem_tx_wrap_en = 0; #ifndef __ESP32_IDF_V44__ rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); @@ -502,9 +502,9 @@ void StepperQueue::disconnect_rmt() { #else // rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); #endif - RMT.tx_conf[channel].idle_out_en = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].idle_out_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; } void StepperQueue::startQueue_rmt() { @@ -536,8 +536,8 @@ void StepperQueue::startQueue_rmt() { #endif rmt_tx_stop(channel); // rmt_rx_stop(channel); - RMT.tx_conf[channel].mem_rd_rst = 1; - RMT.tx_conf[channel].mem_rd_rst = 0; + RMT.chnconf0[channel].mem_rd_rst_n = 1; + RMT.chnconf0[channel].mem_rd_rst_n = 0; uint32_t *mem = FAS_RMT_MEM(channel); // #define TRACE #ifdef TRACE @@ -554,9 +554,9 @@ void StepperQueue::startQueue_rmt() { _isRunning = true; _rmtStopped = false; disable_rmt_interrupts(channel); - RMT.tx_conf[channel].mem_tx_wrap_en = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; #ifdef TRACE USBSerial.print("Queue:"); @@ -627,15 +627,15 @@ void StepperQueue::startQueue_rmt() { #endif // This starts the rmt module - RMT.tx_conf[channel].tx_conti_mode = 1; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; - RMT.tx_conf[channel].mem_tx_wrap_en = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].tx_conti_mode_n = 1; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; PROBE_1_TOGGLE; - RMT.tx_conf[channel].tx_start = 1; + RMT.chnconf0[channel].tx_start_n = 1; } void StepperQueue::forceStop_rmt() { stop_rmt(true); From dfe1da30784410844fa800d675b51d3050d06495 Mon Sep 17 00:00:00 2001 From: GarmischWg Date: Fri, 12 Jan 2024 17:14:18 +0800 Subject: [PATCH 38/85] increased rmt limit --- src/StepperISR_esp32s3_rmt.cpp | 40 +++++++++++++++++----------------- src/fas_common.h | 2 +- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/StepperISR_esp32s3_rmt.cpp b/src/StepperISR_esp32s3_rmt.cpp index 1c408893..d9244f98 100644 --- a/src/StepperISR_esp32s3_rmt.cpp +++ b/src/StepperISR_esp32s3_rmt.cpp @@ -247,26 +247,26 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, // Afterwards alternating. This way the end interrupt is always "half buffer // away" from the threshold interrupt #ifdef TEST_MODE -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_1_TOGGLE; \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \ - if (old_limit == PART_SIZE + 1) { \ - /* second half of buffer sent */ \ - PROBE_2_TOGGLE; \ - /* demonstrate modification of RAM */ \ - uint32_t *mem = FAS_RMT_MEM(ch); \ - mem[PART_SIZE] = 0x33ff33ff; \ - RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ - } else { \ - /* first half of buffer sent */ \ - PROBE_3_TOGGLE; \ - RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ - } \ - RMT.tx_conf[ch].conf_update = 1; \ - RMT.tx_conf[ch].conf_update = 0; \ +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_1_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + /* demonstrate modification of RAM */ \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + mem[PART_SIZE] = 0x33ff33ff; \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + RMT.tx_conf[ch].conf_update = 1; \ + RMT.tx_conf[ch].conf_update = 0; \ } #else #define PROCESS_CHANNEL(ch) \ diff --git a/src/fas_common.h b/src/fas_common.h index c7705313..87c0e481 100644 --- a/src/fas_common.h +++ b/src/fas_common.h @@ -171,7 +171,7 @@ struct queue_end_s { #define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) #define QUEUES_MCPWM_PCNT 4 -#define QUEUES_RMT 2 +#define QUEUES_RMT 4 // have support for pulse counter #define SUPPORT_ESP32_PULSE_COUNTER From 0a0017fd4dbb83b1002c992002d582bd6992bebf Mon Sep 17 00:00:00 2001 From: GarmischWg Date: Fri, 12 Jan 2024 18:48:34 +0800 Subject: [PATCH 39/85] Applying changes to test mode --- src/StepperISR_esp32s3_rmt.cpp | 112 ++++++++++++++++----------------- 1 file changed, 56 insertions(+), 56 deletions(-) diff --git a/src/StepperISR_esp32s3_rmt.cpp b/src/StepperISR_esp32s3_rmt.cpp index d9244f98..5d45f053 100644 --- a/src/StepperISR_esp32s3_rmt.cpp +++ b/src/StepperISR_esp32s3_rmt.cpp @@ -247,26 +247,26 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, // Afterwards alternating. This way the end interrupt is always "half buffer // away" from the threshold interrupt #ifdef TEST_MODE -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_1_TOGGLE; \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - uint8_t old_limit = RMT.tx_lim[ch].RMT_LIMIT; \ - if (old_limit == PART_SIZE + 1) { \ - /* second half of buffer sent */ \ - PROBE_2_TOGGLE; \ - /* demonstrate modification of RAM */ \ - uint32_t *mem = FAS_RMT_MEM(ch); \ - mem[PART_SIZE] = 0x33ff33ff; \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ - } else { \ - /* first half of buffer sent */ \ - PROBE_3_TOGGLE; \ - RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ - } \ - RMT.tx_conf[ch].conf_update = 1; \ - RMT.tx_conf[ch].conf_update = 0; \ +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_1_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + /* demonstrate modification of RAM */ \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + mem[PART_SIZE] = 0x33ff33ff; \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + RMT.chnconf0[ch].conf_update_n = 1; \ + RMT.chnconf0[ch].conf_update_n = 0; \ } #else #define PROCESS_CHANNEL(ch) \ @@ -388,8 +388,8 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { #ifdef TEST_MODE if (channel == 0) { - RMT.tx_conf[channel].mem_rd_rst = 1; - RMT.tx_conf[channel].mem_rd_rst = 0; + RMT.chnconf0[channel].mem_rd_rst_n = 1; + RMT.chnconf0[channel].mem_rd_rst_n = 0; uint32_t *mem = FAS_RMT_MEM(channel); // Fill the buffer with a significant pattern for debugging for (uint8_t i = 0; i < PART_SIZE; i++) { @@ -403,16 +403,16 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { *mem++ = 0; // conti mode is accepted with the conf_update 1 strobe - RMT.tx_conf[channel].tx_conti_mode = 1; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; - RMT.tx_conf[channel].mem_tx_wrap_en = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].tx_conti_mode_n = 1; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; enable_rmt_interrupts(channel); // tx_start does not need conf_update PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 - RMT.tx_conf[channel].tx_start = 1; + RMT.chnconf0[channel].tx_start_n = 1; delay(1000); if (false) { @@ -424,19 +424,19 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { } if (true) { // just clear conti mode => causes end interrupt, no repeat - RMT.tx_conf[channel].tx_conti_mode = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].tx_conti_mode_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; } delay(1000); // actually no need to enable/disable interrupts. // and this seems to avoid some pitfalls // This runs the RMT buffer once - RMT.tx_conf[channel].tx_conti_mode = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; - RMT.tx_conf[channel].tx_start = 1; + RMT.chnconf0[channel].tx_conti_mode_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + RMT.chnconf0[channel].tx_start_n = 1; while (true) { delay(1000); PROBE_1_TOGGLE; @@ -461,34 +461,34 @@ void StepperQueue::connect_rmt() { // here gpio is 0 delay(1); PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv = 1; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.tx_conf[channel].idle_out_lv_n = 1; + RMT.tx_conf[channel].conf_update_n = 1; + RMT.tx_conf[channel].conf_update_n = 0; // here gpio is 1 delay(2); PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].idle_out_lv_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; // here gpio is 0 delay(2); PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_en = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].idle_out_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; // here gpio is 0 delay(2); PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv = 1; - RMT.tx_conf[channel].idle_out_en = 1; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].idle_out_lv_n = 1; + RMT.chnconf0[channel].idle_out_en_n = 1; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; // here gpio is 1 delay(2); PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv = 0; - RMT.tx_conf[channel].conf_update = 1; - RMT.tx_conf[channel].conf_update = 0; + RMT.chnconf0[channel].idle_out_lv_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; // here gpio is 0 delay(2); PROBE_3_TOGGLE; @@ -587,9 +587,9 @@ void StepperQueue::startQueue_rmt() { apply_command(this, true, mem); #ifdef TRACE - USBSerial.print(RMT.tx_conf[channel].val, BIN); + USBSerial.print(RMT.chnconf0[channel].val, BIN); USBSerial.println(' '); - USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en); + USBSerial.print(RMT.chnconf0[channel].mem_tx_wrap_en_n); USBSerial.println(' '); for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { USBSerial.print(i); @@ -604,9 +604,9 @@ void StepperQueue::startQueue_rmt() { apply_command(this, false, mem); #ifdef TRACE - USBSerial.print(RMT.tx_conf[channel].val, BIN); + USBSerial.print(RMT.chnconf0[channel].val, BIN); USBSerial.print(' '); - USBSerial.print(RMT.tx_conf[channel].mem_tx_wrap_en); + USBSerial.print(RMT.chnconf0[channel].mem_tx_wrap_en_n); USBSerial.println(' '); for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { From 743d5e141e555a2f4b3b7f18bb3d680c8314a54f Mon Sep 17 00:00:00 2001 From: GarmischWg Date: Fri, 12 Jan 2024 18:52:52 +0800 Subject: [PATCH 40/85] updated readme --- README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 46901211..f6c9b85f 100644 --- a/README.md +++ b/README.md @@ -147,7 +147,7 @@ Comments to pin sharing: ### ESP32S3 * allows up to 200000 generated steps per second ? -* supports up to four stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* supports up to eight stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) * Steppers' command queue depth: 32 ### ESP32C3 @@ -280,12 +280,12 @@ A note to `MIN_CMD_TICKS` using mcpwm/pcnt: The current implementation uses one What are the differences between mcpwm/pcnt and rmt ? -| | mcpwm/pcnt | rmt | -|:---------------------------|:----------------------------------------|:------------------------------------------------------------------------------| -|Interrupt rate/stepper | one interrupt per command | min: one interrupt per command, max: one interrupt per 31 steps at high speed | -|Required interrupt response | at high speed: time between two steps | at high speed: time between 31 steps | -|Module usage    | 1 or 2 mcpcms, up to 6 channels of pcnt | rmt | -|esp32 notes | availabe pcnt modules can be connected | no pcnt module used, so can be attached to rmt output as realtime position | +| | mcpwm/pcnt | rmt | +| :-------------------------- | :-------------------------------------- | :---------------------------------------------------------------------------- | +| Interrupt rate/stepper | one interrupt per command | min: one interrupt per command, max: one interrupt per 31 steps at high speed | +| Required interrupt response | at high speed: time between two steps | at high speed: time between 31 steps | +| Module usage | 1 or 2 mcpcms, up to 6 channels of pcnt | rmt | +| esp32 notes | availabe pcnt modules can be connected | no pcnt module used, so can be attached to rmt output as realtime position | If the interrupt load is not an issue, then rmt is the better choice. With rmt the below (multi-axis application) mentioned loss of synchonicity at high speeds can be avoided. The rmt driver is - besides some rmt modules perks - less complex and way more straightforward. @@ -299,9 +299,9 @@ This stepper driver uses rmt module. ### ESP32S3 -This stepper driver uses mcpwm/pcnt modules. Can drive up to 4 motors. Tested with 2 motors (not by me). +This stepper driver uses mcpwm/pcnt + rmt modules. Can drive up to 8 motors. Tested with 6 motors (not by me). -Apparently the ESP32S3's rmt module is similar to esp32c3 with 4 instead of 2 channels. Theoretically can drive 4 more steppers. +The ESP32S3's rmt module is similar to esp32c3 with 4 instead of 2 channels and with different register names. ### ESP32C3 From 9f633431cc1bba5459fe38c6341337bae17b7020 Mon Sep 17 00:00:00 2001 From: GarmischWg Date: Fri, 12 Jan 2024 19:04:11 +0800 Subject: [PATCH 41/85] inline comments update --- src/StepperISR_esp32s3_rmt.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/StepperISR_esp32s3_rmt.cpp b/src/StepperISR_esp32s3_rmt.cpp index 5d45f053..cdfdbfd5 100644 --- a/src/StepperISR_esp32s3_rmt.cpp +++ b/src/StepperISR_esp32s3_rmt.cpp @@ -17,7 +17,7 @@ // Every 16 bit entry defines with MSB the output level and the lower 15 bits // the ticks. // -// Important difference of esp32c3 (compared to esp32): +// Important difference of esp32s3 (compared to esp32): // - configuration updates need an conf_update strobe // (apparently the manual is not correct by mentioning to set conf_update // first) @@ -207,7 +207,7 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, steps -= PART_SIZE; } } - // No tick lost mentioned for esp32c3 + // No tick lost mentioned for esp32s3 // if (!fill_part_one) { // Note: When enabling the continuous transmission mode by setting // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the @@ -353,13 +353,13 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { // APB_CLOCK=80 MHz // CLK_DIV = APB_CLOCK/5 = 16 MHz // - // Relation 1 in esp32c3 technical reference: + // Relation 1 in esp32s3 technical reference: // 3 * T_APB + 5 * T_RMT_CLK < period * T_CLK_DIV // => 8 * T_APB < period * T_APB*5 // => period > 8/5 // => period >= 2 // - // Relation 2 in esp32c3 technical reference before end marker: + // Relation 2 in esp32s3 technical reference before end marker: // 6 * T_APB + 12 * T_RMT_CLK < period * T_CLK_DIV // => 18 * T_APB < period * T_APB*5 // => period > 18/5 From 6da0257caffd0f9573e4dedb1770aca7f12bb39f Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 11:17:55 +0100 Subject: [PATCH 42/85] format code --- src/StepperISR_esp32_rmt.cpp | 785 +++++++++++++------------- src/StepperISR_esp32s3_rmt.cpp | 996 ++++++++++++++++----------------- src/fas_common.h | 31 +- 3 files changed, 904 insertions(+), 908 deletions(-) diff --git a/src/StepperISR_esp32_rmt.cpp b/src/StepperISR_esp32_rmt.cpp index 66bd27c3..64fffa53 100644 --- a/src/StepperISR_esp32_rmt.cpp +++ b/src/StepperISR_esp32_rmt.cpp @@ -1,5 +1,6 @@ #include "StepperISR.h" -#if defined(SUPPORT_ESP32_RMT) && !defined(SUPPORT_ESP32C3_RMT) && !defined(SUPPORT_ESP32S3_RMT) +#if defined(SUPPORT_ESP32_RMT) && !defined(SUPPORT_ESP32C3_RMT) && \ + !defined(SUPPORT_ESP32S3_RMT) // #define TEST_MODE @@ -22,179 +23,179 @@ #define PART_SIZE 31 void IRAM_ATTR StepperQueue::stop_rmt(bool both) { - // We are stopping the rmt by letting it run into the end at high speed. - // - // disable the interrupts - // rmt_set_tx_intr_en(channel, false); - // rmt_set_tx_thr_intr_en(channel, false, 0); - - // stop esp32 rmt, by let it hit the end - RMT.conf_ch[channel].conf1.tx_conti_mode = 0; - - // replace second part of buffer with pauses - uint32_t *data = FAS_RMT_MEM(channel); - uint8_t start = both ? 0 : PART_SIZE; - data = &data[start]; - for (uint8_t i = start; i < 2 * PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); - } - *data = 0; - - // as the rmt is not running anymore, mark it as stopped - _rmtStopped = true; + // We are stopping the rmt by letting it run into the end at high speed. + // + // disable the interrupts + // rmt_set_tx_intr_en(channel, false); + // rmt_set_tx_thr_intr_en(channel, false, 0); + + // stop esp32 rmt, by let it hit the end + RMT.conf_ch[channel].conf1.tx_conti_mode = 0; + + // replace second part of buffer with pauses + uint32_t *data = FAS_RMT_MEM(channel); + uint8_t start = both ? 0 : PART_SIZE; + data = &data[start]; + for (uint8_t i = start; i < 2 * PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); + } + *data = 0; + + // as the rmt is not running anymore, mark it as stopped + _rmtStopped = true; } static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, uint32_t *data) { - if (!fill_part_one) { - data += PART_SIZE; + if (!fill_part_one) { + data += PART_SIZE; + } + uint8_t rp = q->read_idx; + if (rp == q->next_write_idx) { + // no command in queue + if (fill_part_one) { + q->bufferContainsSteps[0] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // make a pause with approx. 1ms + // 258 ticks * 2 * 31 = 15996 @ 16MHz + *data++ = 0x01020102; + } + } else { + q->stop_rmt(false); } - uint8_t rp = q->read_idx; - if (rp == q->next_write_idx) { - // no command in queue - if (fill_part_one) { - q->bufferContainsSteps[0] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // make a pause with approx. 1ms - // 258 ticks * 2 * 31 = 15996 @ 16MHz - *data++ = 0x01020102; - } - } else { - q->stop_rmt(false); - } - return; + return; + } + // Process command + struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; + + if (e_curr->toggle_dir) { + // the command requests dir pin toggle + // This is ok only, if the ongoing command does not contain steps + if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { + // So we need a pause. change the finished read entry into a pause + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); + } + return; } - // Process command - struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; - - if (e_curr->toggle_dir) { - // the command requests dir pin toggle - // This is ok only, if the ongoing command does not contain steps - if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { - // So we need a pause. change the finished read entry into a pause - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); - } - return; - } - // The ongoing command does not contain steps, so change dir here should be - // ok - gpio_num_t dirPin = (gpio_num_t)q->dirPin; - gpio_set_level(dirPin, gpio_get_level(dirPin) ^ 1); - // and delete the request - e_curr->toggle_dir = 0; + // The ongoing command does not contain steps, so change dir here should be + // ok + gpio_num_t dirPin = (gpio_num_t)q->dirPin; + gpio_set_level(dirPin, gpio_get_level(dirPin) ^ 1); + // and delete the request + e_curr->toggle_dir = 0; + } + + uint8_t steps = e_curr->steps; + uint16_t ticks = e_curr->ticks; + // if (steps != 0) { + // PROBE_2_TOGGLE; + //} + uint32_t last_entry; + if (steps == 0) { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + // two pauses à 3 ticks. the 2 for debugging + *data++ = 0x00010002; + ticks -= 3; } - - uint8_t steps = e_curr->steps; - uint16_t ticks = e_curr->ticks; - // if (steps != 0) { - // PROBE_2_TOGGLE; - //} - uint32_t last_entry; - if (steps == 0) { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - // two pauses à 3 ticks. the 2 for debugging - *data++ = 0x00010002; - ticks -= 3; + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + last_entry = ticks_l; + last_entry <<= 16; + last_entry |= ticks_r; + } else { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; + if (ticks == 0xffff) { + // special treatment for this case, because an rmt entry can only cover up + // to 0xfffe ticks every step must be minimum split into two rmt entries, + // so at max PART/2 steps can be done. + if (steps < PART_SIZE / 2) { + for (uint8_t i = 1; i < steps; i++) { + // steps-1 iterations + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; } - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - last_entry = ticks_l; - last_entry <<= 16; - last_entry |= ticks_r; - } else { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; - if (ticks == 0xffff) { - // special treatment for this case, because an rmt entry can only cover up - // to 0xfffe ticks every step must be minimum split into two rmt entries, - // so at max PART/2 steps can be done. - if (steps < PART_SIZE / 2) { - for (uint8_t i = 1; i < steps; i++) { - // steps-1 iterations - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - uint16_t delta = PART_SIZE - 2 * steps; - delta <<= 5; - *data++ = 0x20002000 - delta; - // 2*(steps - 1) + 1 already stored => 2*steps - 1 - // and after this for loop one entry added => 2*steps - for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { - *data++ = 0x00100010; - } - last_entry = 0x00100010; - steps = 0; - } else { - steps -= PART_SIZE / 2; - for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - last_entry = 0x20002000; - } - } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { - uint8_t steps_to_do = steps; - if (steps > PART_SIZE) { - steps_to_do /= 2; - } - - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 1; i < steps_to_do; i++) { - *data++ = rmt_entry; - } - uint32_t delta = PART_SIZE - steps_to_do; - delta <<= 18; // shift in upper 16bit and multiply with 4 - *data++ = rmt_entry - delta; - for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; - } - last_entry = 0x00020002; - steps -= steps_to_do; - } else { - // either >= 2*PART_SIZE or = PART_SIZE - // every entry one step - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - *data++ = rmt_entry; - } - last_entry = rmt_entry; - steps -= PART_SIZE; + *data++ = 0x40007fff | 0x8000; + uint16_t delta = PART_SIZE - 2 * steps; + delta <<= 5; + *data++ = 0x20002000 - delta; + // 2*(steps - 1) + 1 already stored => 2*steps - 1 + // and after this for loop one entry added => 2*steps + for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { + *data++ = 0x00100010; } - } - if (!fill_part_one) { - // Note: When enabling the continuous transmission mode by setting - // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the - // channel continuously, that is, from the first byte to the last one, - // then from the first to the last again, and so on. In this mode, there - // will be an idle level lasting one clk_div cycle between N and N+1 - // transmissions. - last_entry -= 1; - } - *data = last_entry; - - // Data is complete - if (steps == 0) { - // The command has been completed - if (e_curr->repeat_entry == 0) { - q->read_idx = rp + 1; + last_entry = 0x00100010; + steps = 0; + } else { + steps -= PART_SIZE / 2; + for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; } + *data++ = 0x40007fff | 0x8000; + last_entry = 0x20002000; + } + } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { + uint8_t steps_to_do = steps; + if (steps > PART_SIZE) { + steps_to_do /= 2; + } + + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 1; i < steps_to_do; i++) { + *data++ = rmt_entry; + } + uint32_t delta = PART_SIZE - steps_to_do; + delta <<= 18; // shift in upper 16bit and multiply with 4 + *data++ = rmt_entry - delta; + for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { + *data++ = 0x00020002; + } + last_entry = 0x00020002; + steps -= steps_to_do; } else { - e_curr->steps = steps; + // either >= 2*PART_SIZE or = PART_SIZE + // every entry one step + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + *data++ = rmt_entry; + } + last_entry = rmt_entry; + steps -= PART_SIZE; } + } + if (!fill_part_one) { + // Note: When enabling the continuous transmission mode by setting + // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the + // channel continuously, that is, from the first byte to the last one, + // then from the first to the last again, and so on. In this mode, there + // will be an idle level lasting one clk_div cycle between N and N+1 + // transmissions. + last_entry -= 1; + } + *data = last_entry; + + // Data is complete + if (steps == 0) { + // The command has been completed + if (e_curr->repeat_entry == 0) { + q->read_idx = rp + 1; + } + } else { + e_curr->steps = steps; + } } #ifndef RMT_CHANNEL_MEM @@ -206,306 +207,306 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, #endif #ifdef TEST_MODE -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_2_TOGGLE; \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - PROBE_3_TOGGLE; \ - /* now repeat the interrupt at buffer size + end marker */ \ - RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ - } +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_2_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + PROBE_3_TOGGLE; \ + /* now repeat the interrupt at buffer size + end marker */ \ + RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ + } #else -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_2_TOGGLE; \ - StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - if (q->_rmtStopped) { \ - rmt_set_tx_intr_en(q->channel, false); \ - rmt_set_tx_thr_intr_en(q->channel, false, PART_SIZE + 1); \ - q->_isRunning = false; \ - PROBE_1_TOGGLE; \ - } else { \ - apply_command(q, false, FAS_RMT_MEM(ch)); \ - } \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - PROBE_3_TOGGLE; \ - StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - if (!q->_rmtStopped) { \ - apply_command(q, true, FAS_RMT_MEM(ch)); \ - /* now repeat the interrupt at buffer size + end marker */ \ - RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ - } \ - } +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_2_TOGGLE; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + if (q->_rmtStopped) { \ + rmt_set_tx_intr_en(q->channel, false); \ + rmt_set_tx_thr_intr_en(q->channel, false, PART_SIZE + 1); \ + q->_isRunning = false; \ + PROBE_1_TOGGLE; \ + } else { \ + apply_command(q, false, FAS_RMT_MEM(ch)); \ + } \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + PROBE_3_TOGGLE; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + if (!q->_rmtStopped) { \ + apply_command(q, true, FAS_RMT_MEM(ch)); \ + /* now repeat the interrupt at buffer size + end marker */ \ + RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ + } \ + } #endif static void IRAM_ATTR tx_intr_handler(void *arg) { - uint32_t mask = RMT.int_st.val; - RMT.int_clr.val = mask; - PROCESS_CHANNEL(0); - PROCESS_CHANNEL(1); + uint32_t mask = RMT.int_st.val; + RMT.int_clr.val = mask; + PROCESS_CHANNEL(0); + PROCESS_CHANNEL(1); #if QUEUES_RMT >= 4 - PROCESS_CHANNEL(2); - PROCESS_CHANNEL(3); + PROCESS_CHANNEL(2); + PROCESS_CHANNEL(3); #endif #if QUEUES_RMT == 8 - PROCESS_CHANNEL(4); - PROCESS_CHANNEL(5); - PROCESS_CHANNEL(6); - PROCESS_CHANNEL(7); + PROCESS_CHANNEL(4); + PROCESS_CHANNEL(5); + PROCESS_CHANNEL(6); + PROCESS_CHANNEL(7); #endif } void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { #ifdef TEST_PROBE_1 - if (channel_num == 0) { - pinMode(TEST_PROBE_1, OUTPUT); - PROBE_1(HIGH); - delay(10); - PROBE_1(LOW); - delay(5); - PROBE_1(HIGH); - delay(5); - PROBE_1(LOW); - delay(5); - } + if (channel_num == 0) { + pinMode(TEST_PROBE_1, OUTPUT); + PROBE_1(HIGH); + delay(10); + PROBE_1(LOW); + delay(5); + PROBE_1(HIGH); + delay(5); + PROBE_1(LOW); + delay(5); + } #endif #ifdef TEST_PROBE_2 - pinMode(TEST_PROBE_2, OUTPUT); - PROBE_2(LOW); + pinMode(TEST_PROBE_2, OUTPUT); + PROBE_2(LOW); #endif #ifdef TEST_PROBE_3 - pinMode(TEST_PROBE_3, OUTPUT); - PROBE_3(LOW); + pinMode(TEST_PROBE_3, OUTPUT); + PROBE_3(LOW); #endif - _initVars(); - _step_pin = step_pin; - digitalWrite(step_pin, LOW); - pinMode(step_pin, OUTPUT); + _initVars(); + _step_pin = step_pin; + digitalWrite(step_pin, LOW); + pinMode(step_pin, OUTPUT); + + channel = (rmt_channel_t)channel_num; + + if (channel_num == 0) { + periph_module_enable(PERIPH_RMT_MODULE); + } + // 80 MHz/5 = 16 MHz + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); + rmt_set_source_clk(channel, RMT_BASECLK_APB); + rmt_set_clk_div(channel, 5); + rmt_set_mem_block_num(channel, 1); + rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); + rmt_tx_stop(channel); + rmt_rx_stop(channel); + // rmt_tx_memory_reset is not defined in arduino V340 and based on test result + // not needed rmt_tx_memory_reset(channel); + if (channel_num == 0) { + rmt_isr_register(tx_intr_handler, NULL, + ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); + RMT.apb_conf.RMT_FIFO = 1; // disable fifo mode + RMT.apb_conf.mem_tx_wrap_en = 0; + } + + _isRunning = false; + _rmtStopped = true; + + connect(); - channel = (rmt_channel_t)channel_num; - - if (channel_num == 0) { - periph_module_enable(PERIPH_RMT_MODULE); +#ifdef TEST_MODE + if (channel == 0) { + uint32_t *mem = FAS_RMT_MEM(channel); + // Fill the buffer with a significant pattern for debugging + for (uint8_t i = 0; i < 32; i += 2) { + *mem++ = 0x7fffffff; + *mem++ = 0x7fff7fff; } - // 80 MHz/5 = 16 MHz - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); - rmt_set_source_clk(channel, RMT_BASECLK_APB); - rmt_set_clk_div(channel, 5); - rmt_set_mem_block_num(channel, 1); - rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); - rmt_tx_stop(channel); - rmt_rx_stop(channel); - // rmt_tx_memory_reset is not defined in arduino V340 and based on test result - // not needed rmt_tx_memory_reset(channel); - if (channel_num == 0) { - rmt_isr_register(tx_intr_handler, NULL, - ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); - RMT.apb_conf.RMT_FIFO = 1; // disable fifo mode - RMT.apb_conf.mem_tx_wrap_en = 0; + for (uint8_t i = 32; i < 62; i += 2) { + *mem++ = 0x3fffdfff; + *mem++ = 0x3fff3fff; } + // without end marker it does not loop + *mem++ = 0; + *mem++ = 0; + RMT.conf_ch[channel].conf1.tx_conti_mode = 1; + rmt_set_tx_intr_en(channel, true); + rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + // rmt_tx_start(channel, true); + PROBE_2_TOGGLE; + + delay(1000); + if (false) { + mem--; + mem--; + // destroy end marker => no end interrupt, no repeat + *mem++ = 0x00010001; + *mem = 0x00010001; + } + if (true) { + // just clear conti mode => causes end interrup, no repeat + RMT.conf_ch[channel].conf1.tx_conti_mode = 0; + } + delay(1000); + // actually no need to enable/disable interrupts. + // and this seems to avoid some pitfalls - _isRunning = false; - _rmtStopped = true; - - connect(); - -#ifdef TEST_MODE - if (channel == 0) { - uint32_t *mem = FAS_RMT_MEM(channel); - // Fill the buffer with a significant pattern for debugging - for (uint8_t i = 0; i < 32; i += 2) { - *mem++ = 0x7fffffff; - *mem++ = 0x7fff7fff; - } - for (uint8_t i = 32; i < 62; i += 2) { - *mem++ = 0x3fffdfff; - *mem++ = 0x3fff3fff; - } - // without end marker it does not loop - *mem++ = 0; - *mem++ = 0; - RMT.conf_ch[channel].conf1.tx_conti_mode = 1; - rmt_set_tx_intr_en(channel, true); - rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); - // rmt_tx_start(channel, true); - PROBE_2_TOGGLE; - - delay(1000); - if (false) { - mem--; - mem--; - // destroy end marker => no end interrupt, no repeat - *mem++ = 0x00010001; - *mem = 0x00010001; - } - if (true) { - // just clear conti mode => causes end interrup, no repeat - RMT.conf_ch[channel].conf1.tx_conti_mode = 0; - } - delay(1000); - // actually no need to enable/disable interrupts. - // and this seems to avoid some pitfalls - - // This runs the RMT buffer once - RMT.conf_ch[channel].conf1.tx_conti_mode = 1; - delay(1); - RMT.conf_ch[channel].conf1.tx_conti_mode = 0; - while (true) { - delay(1); - } + // This runs the RMT buffer once + RMT.conf_ch[channel].conf1.tx_conti_mode = 1; + delay(1); + RMT.conf_ch[channel].conf1.tx_conti_mode = 0; + while (true) { + delay(1); } + } #endif } void StepperQueue::connect_rmt() { - // rmt_set_tx_intr_en(channel, true); - // rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); - RMT.conf_ch[channel].conf1.idle_out_lv = 0; - RMT.conf_ch[channel].conf1.idle_out_en = 1; + // rmt_set_tx_intr_en(channel, true); + // rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + RMT.conf_ch[channel].conf1.idle_out_lv = 0; + RMT.conf_ch[channel].conf1.idle_out_en = 1; #ifndef __ESP32_IDF_V44__ - rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); + rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); #else - rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false); + rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false); #endif } void StepperQueue::disconnect_rmt() { - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, PART_SIZE + 1); #ifndef __ESP32_IDF_V44__ // rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1); #else - rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); + rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); #endif - RMT.conf_ch[channel].conf1.idle_out_en = 0; + RMT.conf_ch[channel].conf1.idle_out_en = 0; } void StepperQueue::startQueue_rmt() { // #define TRACE #ifdef TRACE - Serial.println("START"); + Serial.println("START"); #endif #ifdef TEST_PROBE_2 - PROBE_2(LOW); + PROBE_2(LOW); #endif #ifdef TEST_PROBE_3 - PROBE_3(LOW); + PROBE_3(LOW); #endif #ifdef TEST_PROBE_1 - delay(1); - PROBE_1_TOGGLE; - delay(1); - PROBE_1_TOGGLE; - delay(1); - PROBE_1_TOGGLE; - delay(1); + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); #endif - rmt_tx_stop(channel); - // rmt_rx_stop(channel); - rmt_memory_rw_rst(channel); - // the following assignment should not be needed; - // RMT.data_ch[channel] = 0; - uint32_t *mem = FAS_RMT_MEM(channel); - // Fill the buffer with a significant pattern for debugging - // Keep it for now - for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { - mem[i] = 0x0fff8fff; - mem[i + 1] = 0x7fff8fff; - } - mem[2 * PART_SIZE] = 0; - mem[2 * PART_SIZE + 1] = 0; - _isRunning = true; - _rmtStopped = false; - rmt_set_tx_intr_en(channel, false); - rmt_set_tx_thr_intr_en(channel, false, 0); - // RMT.apb_conf.mem_tx_wrap_en = 0; + rmt_tx_stop(channel); + // rmt_rx_stop(channel); + rmt_memory_rw_rst(channel); + // the following assignment should not be needed; + // RMT.data_ch[channel] = 0; + uint32_t *mem = FAS_RMT_MEM(channel); + // Fill the buffer with a significant pattern for debugging + // Keep it for now + for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { + mem[i] = 0x0fff8fff; + mem[i + 1] = 0x7fff8fff; + } + mem[2 * PART_SIZE] = 0; + mem[2 * PART_SIZE + 1] = 0; + _isRunning = true; + _rmtStopped = false; + rmt_set_tx_intr_en(channel, false); + rmt_set_tx_thr_intr_en(channel, false, 0); + // RMT.apb_conf.mem_tx_wrap_en = 0; // #define TRACE #ifdef TRACE - Serial.print("Queue:"); - Serial.print(read_idx); - Serial.print('/'); - Serial.println(next_write_idx); - for (uint8_t i = 0; i < 64; i++) { - Serial.print(i); - Serial.print(' '); - Serial.println(mem[i], HEX); - } + Serial.print("Queue:"); + Serial.print(read_idx); + Serial.print('/'); + Serial.println(next_write_idx); + for (uint8_t i = 0; i < 64; i++) { + Serial.print(i); + Serial.print(' '); + Serial.println(mem[i], HEX); + } #endif - // set dirpin toggle here - uint8_t rp = read_idx; - if (rp == next_write_idx) { - // nothing to do ? - // Should not happen, so bail - return; - } - if (entry[rp & QUEUE_LEN_MASK].toggle_dir) { - gpio_set_level((gpio_num_t)dirPin, gpio_get_level((gpio_num_t)dirPin) ^ 1); - entry[rp & QUEUE_LEN_MASK].toggle_dir = false; - } - - bufferContainsSteps[0] = true; - bufferContainsSteps[1] = true; - apply_command(this, true, mem); + // set dirpin toggle here + uint8_t rp = read_idx; + if (rp == next_write_idx) { + // nothing to do ? + // Should not happen, so bail + return; + } + if (entry[rp & QUEUE_LEN_MASK].toggle_dir) { + gpio_set_level((gpio_num_t)dirPin, gpio_get_level((gpio_num_t)dirPin) ^ 1); + entry[rp & QUEUE_LEN_MASK].toggle_dir = false; + } + + bufferContainsSteps[0] = true; + bufferContainsSteps[1] = true; + apply_command(this, true, mem); #ifdef TRACE - Serial.print(RMT.conf_ch[channel].conf0.val, BIN); + Serial.print(RMT.conf_ch[channel].conf0.val, BIN); + Serial.print(' '); + Serial.print(RMT.conf_ch[channel].conf1.val, BIN); + Serial.println(' '); + Serial.print(RMT.apb_conf.mem_tx_wrap_en); + Serial.println(' '); + for (uint8_t i = 0; i < 64; i++) { + Serial.print(i); Serial.print(' '); - Serial.print(RMT.conf_ch[channel].conf1.val, BIN); - Serial.println(' '); - Serial.print(RMT.apb_conf.mem_tx_wrap_en); - Serial.println(' '); - for (uint8_t i = 0; i < 64; i++) { - Serial.print(i); - Serial.print(' '); - Serial.println(mem[i], HEX); - } - if (!isRunning()) { - Serial.println("STOPPED 1"); - } + Serial.println(mem[i], HEX); + } + if (!isRunning()) { + Serial.println("STOPPED 1"); + } #endif - apply_command(this, false, mem); + apply_command(this, false, mem); #ifdef TRACE - Serial.print(RMT.conf_ch[channel].conf0.val, BIN); + Serial.print(RMT.conf_ch[channel].conf0.val, BIN); + Serial.print(' '); + Serial.print(RMT.conf_ch[channel].conf1.val, BIN); + Serial.println(' '); + Serial.print(RMT.apb_conf.mem_tx_wrap_en); + Serial.println(' '); + + for (uint8_t i = 0; i < 64; i++) { + Serial.print(i); Serial.print(' '); - Serial.print(RMT.conf_ch[channel].conf1.val, BIN); - Serial.println(' '); - Serial.print(RMT.apb_conf.mem_tx_wrap_en); - Serial.println(' '); - - for (uint8_t i = 0; i < 64; i++) { - Serial.print(i); - Serial.print(' '); - Serial.println(mem[i], HEX); - } + Serial.println(mem[i], HEX); + } #endif - rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); - rmt_set_tx_intr_en(channel, true); - _rmtStopped = false; + rmt_set_tx_thr_intr_en(channel, true, PART_SIZE + 1); + rmt_set_tx_intr_en(channel, true); + _rmtStopped = false; - // This starts the rmt module - RMT.conf_ch[channel].conf1.tx_conti_mode = 1; + // This starts the rmt module + RMT.conf_ch[channel].conf1.tx_conti_mode = 1; - RMT.conf_ch[channel].conf1.tx_start = 1; + RMT.conf_ch[channel].conf1.tx_start = 1; } void StepperQueue::forceStop_rmt() { - stop_rmt(true); + stop_rmt(true); - // and empty the buffer - read_idx = next_write_idx; + // and empty the buffer + read_idx = next_write_idx; } bool StepperQueue::isReadyForCommands_rmt() { - if (_isRunning) { - return !_rmtStopped; - } - return true; + if (_isRunning) { + return !_rmtStopped; + } + return true; } uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; } #endif diff --git a/src/StepperISR_esp32s3_rmt.cpp b/src/StepperISR_esp32s3_rmt.cpp index cdfdbfd5..3281a325 100644 --- a/src/StepperISR_esp32s3_rmt.cpp +++ b/src/StepperISR_esp32s3_rmt.cpp @@ -36,198 +36,198 @@ #endif // In order to avoid threshold/end interrupt on end, add one -#define enable_rmt_interrupts(channel) \ - { \ - RMT.chn_tx_lim[channel].RMT_LIMIT = PART_SIZE + 2; \ - RMT.chnconf0[channel].conf_update_n = 1; \ - RMT.chnconf0[channel].conf_update_n = 0; \ - RMT.int_clr.val |= 0x101 << channel; \ - RMT.int_ena.val |= 0x101 << channel; \ - } +#define enable_rmt_interrupts(channel) \ + { \ + RMT.chn_tx_lim[channel].RMT_LIMIT = PART_SIZE + 2; \ + RMT.chnconf0[channel].conf_update_n = 1; \ + RMT.chnconf0[channel].conf_update_n = 0; \ + RMT.int_clr.val |= 0x101 << channel; \ + RMT.int_ena.val |= 0x101 << channel; \ + } #define disable_rmt_interrupts(channel) \ - { RMT.int_ena.val &= ~(0x101 << channel); } + { RMT.int_ena.val &= ~(0x101 << channel); } void IRAM_ATTR StepperQueue::stop_rmt(bool both) { - // We are stopping the rmt by letting it run into the end at high speed. - // - // disable the interrupts - // rmt_set_tx_intr_en(channel, false); - // rmt_set_tx_thr_intr_en(channel, false, 0); - - // stop esp32 rmt, by let it hit the end - RMT.chnconf0[channel].tx_conti_mode_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - - // replace second part of buffer with pauses - uint32_t *data = FAS_RMT_MEM(channel); - uint8_t start = both ? 0 : PART_SIZE; - data = &data[start]; - for (uint8_t i = start; i < 2 * PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); - } - *data = 0; - - // as the rmt is not running anymore, mark it as stopped - _rmtStopped = true; + // We are stopping the rmt by letting it run into the end at high speed. + // + // disable the interrupts + // rmt_set_tx_intr_en(channel, false); + // rmt_set_tx_thr_intr_en(channel, false, 0); + + // stop esp32 rmt, by let it hit the end + RMT.chnconf0[channel].tx_conti_mode_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + + // replace second part of buffer with pauses + uint32_t *data = FAS_RMT_MEM(channel); + uint8_t start = both ? 0 : PART_SIZE; + data = &data[start]; + for (uint8_t i = start; i < 2 * PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); + } + *data = 0; + + // as the rmt is not running anymore, mark it as stopped + _rmtStopped = true; } static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, uint32_t *data) { - if (!fill_part_one) { - data += PART_SIZE; + if (!fill_part_one) { + data += PART_SIZE; + } + uint8_t rp = q->read_idx; + if (rp == q->next_write_idx) { + // no command in queue + if (fill_part_one) { + q->bufferContainsSteps[0] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // make a pause with approx. 1ms + // 258 ticks * 2 * 31 = 15996 @ 16MHz + // 347 ticks * 2 * 23 = 15962 @ 16MHz + *data++ = 0x010001 * (16000 / 2 / PART_SIZE); + } + } else { + q->stop_rmt(false); } - uint8_t rp = q->read_idx; - if (rp == q->next_write_idx) { - // no command in queue - if (fill_part_one) { - q->bufferContainsSteps[0] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // make a pause with approx. 1ms - // 258 ticks * 2 * 31 = 15996 @ 16MHz - // 347 ticks * 2 * 23 = 15962 @ 16MHz - *data++ = 0x010001 * (16000 / 2 / PART_SIZE); - } - } else { - q->stop_rmt(false); - } - return; + return; + } + // Process command + struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; + + if (e_curr->toggle_dir) { + // the command requests dir pin toggle + // This is ok only, if the ongoing command does not contain steps + if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { + // So we need a pause. change the finished read entry into a pause + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * + ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE)); + } + return; } - // Process command - struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; - - if (e_curr->toggle_dir) { - // the command requests dir pin toggle - // This is ok only, if the ongoing command does not contain steps - if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { - // So we need a pause. change the finished read entry into a pause - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * - ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE)); - } - return; - } - // The ongoing command does not contain steps, so change dir here should be - // ok - gpio_num_t dirPin = (gpio_num_t)q->dirPin; - gpio_set_level(dirPin, gpio_get_level(dirPin) ^ 1); - // and delete the request - e_curr->toggle_dir = 0; + // The ongoing command does not contain steps, so change dir here should be + // ok + gpio_num_t dirPin = (gpio_num_t)q->dirPin; + gpio_set_level(dirPin, gpio_get_level(dirPin) ^ 1); + // and delete the request + e_curr->toggle_dir = 0; + } + + uint8_t steps = e_curr->steps; + uint16_t ticks = e_curr->ticks; + // if (steps != 0) { + // PROBE_1_TOGGLE; + //} + uint32_t last_entry; + if (steps == 0) { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; + // Perhaps the rmt performs look ahead + ticks -= (PART_SIZE - 2) * 4 + 8; + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + last_entry = ticks_l; + last_entry <<= 16; + last_entry |= ticks_r; + *data++ = last_entry; + for (uint8_t i = 1; i < PART_SIZE - 1; i++) { + *data++ = 0x00020002; } - - uint8_t steps = e_curr->steps; - uint16_t ticks = e_curr->ticks; - // if (steps != 0) { - // PROBE_1_TOGGLE; - //} - uint32_t last_entry; - if (steps == 0) { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - // Perhaps the rmt performs look ahead - ticks -= (PART_SIZE - 2) * 4 + 8; - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - last_entry = ticks_l; - last_entry <<= 16; - last_entry |= ticks_r; - *data++ = last_entry; - for (uint8_t i = 1; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; + last_entry = 0x00040004; + } else { + q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; + if (ticks == 0xffff) { + // special treatment for this case, because an rmt entry can only cover up + // to 0xfffe ticks every step must be minimum split into two rmt entries, + // so at max PART/2 steps can be done. + if (steps < PART_SIZE / 2) { + for (uint8_t i = 1; i < steps; i++) { + // steps-1 iterations + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; } - last_entry = 0x00040004; - } else { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; - if (ticks == 0xffff) { - // special treatment for this case, because an rmt entry can only cover up - // to 0xfffe ticks every step must be minimum split into two rmt entries, - // so at max PART/2 steps can be done. - if (steps < PART_SIZE / 2) { - for (uint8_t i = 1; i < steps; i++) { - // steps-1 iterations - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - uint16_t delta = PART_SIZE - 2 * steps; - delta <<= 5; - *data++ = 0x20002000 - delta; - // 2*(steps - 1) + 1 already stored => 2*steps - 1 - // and after this for loop one entry added => 2*steps - for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { - *data++ = 0x00100010; - } - last_entry = 0x00100010; - steps = 0; - } else { - steps -= PART_SIZE / 2; - for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - last_entry = 0x20002000; - } - } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { - uint8_t steps_to_do = steps; - if (steps > PART_SIZE) { - steps_to_do /= 2; - } - - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 1; i < steps_to_do; i++) { - *data++ = rmt_entry; - } - uint32_t delta = (PART_SIZE - steps_to_do) * 4 + 8; - delta <<= 16; // shift in upper 16bit - *data++ = rmt_entry - delta; - for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; - } - last_entry = 0x00040004; - steps -= steps_to_do; - } else { - // either >= 2*PART_SIZE or = PART_SIZE - // every entry one step - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - *data++ = rmt_entry; - } - last_entry = rmt_entry; - steps -= PART_SIZE; + *data++ = 0x40007fff | 0x8000; + uint16_t delta = PART_SIZE - 2 * steps; + delta <<= 5; + *data++ = 0x20002000 - delta; + // 2*(steps - 1) + 1 already stored => 2*steps - 1 + // and after this for loop one entry added => 2*steps + for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { + *data++ = 0x00100010; } - } - // No tick lost mentioned for esp32s3 - // if (!fill_part_one) { - // Note: When enabling the continuous transmission mode by setting - // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the - // channel continuously, that is, from the first byte to the last one, - // then from the first to the last again, and so on. In this mode, there - // will be an idle level lasting one clk_div cycle between N and N+1 - // transmissions. - // last_entry -= 1; - // } - *data = last_entry; - - // Data is complete - if (steps == 0) { - // The command has been completed - if (e_curr->repeat_entry == 0) { - q->read_idx = rp + 1; + last_entry = 0x00100010; + steps = 0; + } else { + steps -= PART_SIZE / 2; + for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; } + *data++ = 0x40007fff | 0x8000; + last_entry = 0x20002000; + } + } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { + uint8_t steps_to_do = steps; + if (steps > PART_SIZE) { + steps_to_do /= 2; + } + + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 1; i < steps_to_do; i++) { + *data++ = rmt_entry; + } + uint32_t delta = (PART_SIZE - steps_to_do) * 4 + 8; + delta <<= 16; // shift in upper 16bit + *data++ = rmt_entry - delta; + for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { + *data++ = 0x00020002; + } + last_entry = 0x00040004; + steps -= steps_to_do; } else { - e_curr->steps = steps; + // either >= 2*PART_SIZE or = PART_SIZE + // every entry one step + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + uint32_t rmt_entry = ticks_l; + rmt_entry <<= 16; + rmt_entry |= ticks_r | 0x8000; // with step + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + *data++ = rmt_entry; + } + last_entry = rmt_entry; + steps -= PART_SIZE; + } + } + // No tick lost mentioned for esp32s3 + // if (!fill_part_one) { + // Note: When enabling the continuous transmission mode by setting + // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the + // channel continuously, that is, from the first byte to the last one, + // then from the first to the last again, and so on. In this mode, there + // will be an idle level lasting one clk_div cycle between N and N+1 + // transmissions. + // last_entry -= 1; + // } + *data = last_entry; + + // Data is complete + if (steps == 0) { + // The command has been completed + if (e_curr->repeat_entry == 0) { + q->read_idx = rp + 1; } + } else { + e_curr->steps = steps; + } } #if !defined(RMT_CHANNEL_MEM) && !defined(SUPPORT_ESP32S3_RMT) @@ -247,407 +247,407 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, // Afterwards alternating. This way the end interrupt is always "half buffer // away" from the threshold interrupt #ifdef TEST_MODE +#define PROCESS_CHANNEL(ch) \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + PROBE_1_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + /* demonstrate modification of RAM */ \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + mem[PART_SIZE] = 0x33ff33ff; \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ + } \ + RMT.chnconf0[ch].conf_update_n = 1; \ + RMT.chnconf0[ch].conf_update_n = 0; \ + } +#else #define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - PROBE_1_TOGGLE; \ + if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + disable_rmt_interrupts(q->channel); \ + q->_isRunning = false; \ + PROBE_1_TOGGLE; \ + PROBE_2_TOGGLE; \ + PROBE_3_TOGGLE; \ + } \ + if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ + uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \ + StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ + uint32_t *mem = FAS_RMT_MEM(ch); \ + if (old_limit == PART_SIZE + 1) { \ + /* second half of buffer sent */ \ + PROBE_2_TOGGLE; \ + apply_command(q, false, mem); \ + /* demonstrate modification of RAM */ \ + /*mem[PART_SIZE] = 0x33fff3ff; */ \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ + } else { \ + /* first half of buffer sent */ \ + PROBE_3_TOGGLE; \ + apply_command(q, true, mem); \ + RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \ - if (old_limit == PART_SIZE + 1) { \ - /* second half of buffer sent */ \ - PROBE_2_TOGGLE; \ - /* demonstrate modification of RAM */ \ - uint32_t *mem = FAS_RMT_MEM(ch); \ - mem[PART_SIZE] = 0x33ff33ff; \ - RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ - } else { \ - /* first half of buffer sent */ \ - PROBE_3_TOGGLE; \ - RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ - } \ - RMT.chnconf0[ch].conf_update_n = 1; \ - RMT.chnconf0[ch].conf_update_n = 0; \ - } -#else -#define PROCESS_CHANNEL(ch) \ - if (mask & RMT_CH##ch##_TX_END_INT_ST) { \ - StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - disable_rmt_interrupts(q->channel); \ - q->_isRunning = false; \ - PROBE_1_TOGGLE; \ - PROBE_2_TOGGLE; \ - PROBE_3_TOGGLE; \ - } \ - if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ - uint8_t old_limit = RMT.chn_tx_lim[ch].RMT_LIMIT; \ - StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ - uint32_t *mem = FAS_RMT_MEM(ch); \ - if (old_limit == PART_SIZE + 1) { \ - /* second half of buffer sent */ \ - PROBE_2_TOGGLE; \ - apply_command(q, false, mem); \ - /* demonstrate modification of RAM */ \ - /*mem[PART_SIZE] = 0x33fff3ff; */ \ - RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ - } else { \ - /* first half of buffer sent */ \ - PROBE_3_TOGGLE; \ - apply_command(q, true, mem); \ - RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ - } \ - RMT.chnconf0[ch].conf_update_n = 1; \ - RMT.chnconf0[ch].conf_update_n = 0; \ - } + RMT.chnconf0[ch].conf_update_n = 1; \ + RMT.chnconf0[ch].conf_update_n = 0; \ + } #endif static void IRAM_ATTR tx_intr_handler(void *arg) { - uint32_t mask = RMT.int_st.val; - RMT.int_clr.val = mask; - PROCESS_CHANNEL(0); - PROCESS_CHANNEL(1); + uint32_t mask = RMT.int_st.val; + RMT.int_clr.val = mask; + PROCESS_CHANNEL(0); + PROCESS_CHANNEL(1); #if QUEUES_RMT >= 4 - PROCESS_CHANNEL(2); - PROCESS_CHANNEL(3); + PROCESS_CHANNEL(2); + PROCESS_CHANNEL(3); #endif #if QUEUES_RMT == 8 - PROCESS_CHANNEL(4); - PROCESS_CHANNEL(5); - PROCESS_CHANNEL(6); - PROCESS_CHANNEL(7); + PROCESS_CHANNEL(4); + PROCESS_CHANNEL(5); + PROCESS_CHANNEL(6); + PROCESS_CHANNEL(7); #endif } void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { #ifdef TEST_PROBE_1 - if (channel_num == 0) { - pinMode(TEST_PROBE_1, OUTPUT); - PROBE_1(HIGH); - delay(10); - PROBE_1(LOW); - delay(5); - PROBE_1(HIGH); - delay(5); - PROBE_1(LOW); - delay(5); - } + if (channel_num == 0) { + pinMode(TEST_PROBE_1, OUTPUT); + PROBE_1(HIGH); + delay(10); + PROBE_1(LOW); + delay(5); + PROBE_1(HIGH); + delay(5); + PROBE_1(LOW); + delay(5); + } #endif #ifdef TEST_PROBE_2 - pinMode(TEST_PROBE_2, OUTPUT); - PROBE_2(LOW); + pinMode(TEST_PROBE_2, OUTPUT); + PROBE_2(LOW); #endif #ifdef TEST_PROBE_3 - pinMode(TEST_PROBE_3, OUTPUT); - PROBE_3(LOW); + pinMode(TEST_PROBE_3, OUTPUT); + PROBE_3(LOW); #endif - _initVars(); - _step_pin = step_pin; - // digitalWrite(step_pin, LOW); - // pinMode(step_pin, OUTPUT); + _initVars(); + _step_pin = step_pin; + // digitalWrite(step_pin, LOW); + // pinMode(step_pin, OUTPUT); + + channel = (rmt_channel_t)channel_num; + + if (channel_num == 0) { + periph_module_enable(PERIPH_RMT_MODULE); + } + // APB_CLOCK=80 MHz + // CLK_DIV = APB_CLOCK/5 = 16 MHz + // + // Relation 1 in esp32s3 technical reference: + // 3 * T_APB + 5 * T_RMT_CLK < period * T_CLK_DIV + // => 8 * T_APB < period * T_APB*5 + // => period > 8/5 + // => period >= 2 + // + // Relation 2 in esp32s3 technical reference before end marker: + // 6 * T_APB + 12 * T_RMT_CLK < period * T_CLK_DIV + // => 18 * T_APB < period * T_APB*5 + // => period > 18/5 + // => period >= 4 + // + disable_rmt_interrupts(channel); + rmt_set_source_clk(channel, RMT_BASECLK_APB); + rmt_set_clk_div(channel, 5); + rmt_set_mem_block_num(channel, 1); + rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); + rmt_tx_stop(channel); + rmt_memory_rw_rst(channel); + // rmt_rx_stop(channel); TX only channel ! + // rmt_tx_memory_reset is not defined in arduino V340 and based on test + // result not needed rmt_tx_memory_reset(channel); + if (channel_num == 0) { + rmt_isr_register(tx_intr_handler, NULL, + ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); + RMT.sys_conf.apb_fifo_mask = 1; // disable fifo mode + } + + _isRunning = false; + _rmtStopped = true; + + connect_rmt(); - channel = (rmt_channel_t)channel_num; - - if (channel_num == 0) { - periph_module_enable(PERIPH_RMT_MODULE); +#ifdef TEST_MODE + if (channel == 0) { + RMT.chnconf0[channel].mem_rd_rst_n = 1; + RMT.chnconf0[channel].mem_rd_rst_n = 0; + uint32_t *mem = FAS_RMT_MEM(channel); + // Fill the buffer with a significant pattern for debugging + for (uint8_t i = 0; i < PART_SIZE; i++) { + *mem++ = 0x7fffffff; } - // APB_CLOCK=80 MHz - // CLK_DIV = APB_CLOCK/5 = 16 MHz - // - // Relation 1 in esp32s3 technical reference: - // 3 * T_APB + 5 * T_RMT_CLK < period * T_CLK_DIV - // => 8 * T_APB < period * T_APB*5 - // => period > 8/5 - // => period >= 2 - // - // Relation 2 in esp32s3 technical reference before end marker: - // 6 * T_APB + 12 * T_RMT_CLK < period * T_CLK_DIV - // => 18 * T_APB < period * T_APB*5 - // => period > 18/5 - // => period >= 4 - // - disable_rmt_interrupts(channel); - rmt_set_source_clk(channel, RMT_BASECLK_APB); - rmt_set_clk_div(channel, 5); - rmt_set_mem_block_num(channel, 1); - rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); - rmt_tx_stop(channel); - rmt_memory_rw_rst(channel); - // rmt_rx_stop(channel); TX only channel ! - // rmt_tx_memory_reset is not defined in arduino V340 and based on test - // result not needed rmt_tx_memory_reset(channel); - if (channel_num == 0) { - rmt_isr_register(tx_intr_handler, NULL, - ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM, NULL); - RMT.sys_conf.apb_fifo_mask = 1; // disable fifo mode + for (uint8_t i = PART_SIZE; i < 2 * PART_SIZE; i++) { + *mem++ = 0x3fffdfff; } + // without end marker it does not loop + *mem++ = 0; + *mem++ = 0; - _isRunning = false; - _rmtStopped = true; + // conti mode is accepted with the conf_update 1 strobe + RMT.chnconf0[channel].tx_conti_mode_n = 1; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + enable_rmt_interrupts(channel); + // tx_start does not need conf_update + PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 + RMT.chnconf0[channel].tx_start_n = 1; - connect_rmt(); + delay(1000); + if (false) { + mem--; + mem--; + // destroy end marker => no end interrupt, no repeat + *mem++ = 0x00010001; + *mem = 0x00010001; + } + if (true) { + // just clear conti mode => causes end interrupt, no repeat + RMT.chnconf0[channel].tx_conti_mode_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + } + delay(1000); + // actually no need to enable/disable interrupts. + // and this seems to avoid some pitfalls -#ifdef TEST_MODE - if (channel == 0) { - RMT.chnconf0[channel].mem_rd_rst_n = 1; - RMT.chnconf0[channel].mem_rd_rst_n = 0; - uint32_t *mem = FAS_RMT_MEM(channel); - // Fill the buffer with a significant pattern for debugging - for (uint8_t i = 0; i < PART_SIZE; i++) { - *mem++ = 0x7fffffff; - } - for (uint8_t i = PART_SIZE; i < 2 * PART_SIZE; i++) { - *mem++ = 0x3fffdfff; - } - // without end marker it does not loop - *mem++ = 0; - *mem++ = 0; - - // conti mode is accepted with the conf_update 1 strobe - RMT.chnconf0[channel].tx_conti_mode_n = 1; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - enable_rmt_interrupts(channel); - // tx_start does not need conf_update - PROBE_1_TOGGLE; // end interrupt will toggle again PROBE_1 - RMT.chnconf0[channel].tx_start_n = 1; - - delay(1000); - if (false) { - mem--; - mem--; - // destroy end marker => no end interrupt, no repeat - *mem++ = 0x00010001; - *mem = 0x00010001; - } - if (true) { - // just clear conti mode => causes end interrupt, no repeat - RMT.chnconf0[channel].tx_conti_mode_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - } - delay(1000); - // actually no need to enable/disable interrupts. - // and this seems to avoid some pitfalls - - // This runs the RMT buffer once - RMT.chnconf0[channel].tx_conti_mode_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - RMT.chnconf0[channel].tx_start_n = 1; - while (true) { - delay(1000); - PROBE_1_TOGGLE; - } + // This runs the RMT buffer once + RMT.chnconf0[channel].tx_conti_mode_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + RMT.chnconf0[channel].tx_start_n = 1; + while (true) { + delay(1000); + PROBE_1_TOGGLE; } + } #endif } void StepperQueue::connect_rmt() { - RMT.chnconf0[channel].idle_out_lv_n = 0; - RMT.chnconf0[channel].idle_out_en_n = 1; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - // RMT.tx_conf[channel].mem_tx_wrap_en = 0; + RMT.chnconf0[channel].idle_out_lv_n = 0; + RMT.chnconf0[channel].idle_out_en_n = 1; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + // RMT.tx_conf[channel].mem_tx_wrap_en = 0; #ifndef __ESP32_IDF_V44__ - rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); + rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)_step_pin); #else - rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false); + rmt_set_gpio(channel, RMT_MODE_TX, (gpio_num_t)_step_pin, false); #endif #ifdef TEST_MODE - // here gpio is 0 - delay(1); - PROBE_3_TOGGLE; - RMT.tx_conf[channel].idle_out_lv_n = 1; - RMT.tx_conf[channel].conf_update_n = 1; - RMT.tx_conf[channel].conf_update_n = 0; - // here gpio is 1 - delay(2); - PROBE_3_TOGGLE; - RMT.chnconf0[channel].idle_out_lv_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - // here gpio is 0 - delay(2); - PROBE_3_TOGGLE; - RMT.chnconf0[channel].idle_out_en_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - // here gpio is 0 - delay(2); - PROBE_3_TOGGLE; - RMT.chnconf0[channel].idle_out_lv_n = 1; - RMT.chnconf0[channel].idle_out_en_n = 1; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - // here gpio is 1 - delay(2); - PROBE_3_TOGGLE; - RMT.chnconf0[channel].idle_out_lv_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - // here gpio is 0 - delay(2); - PROBE_3_TOGGLE; + // here gpio is 0 + delay(1); + PROBE_3_TOGGLE; + RMT.tx_conf[channel].idle_out_lv_n = 1; + RMT.tx_conf[channel].conf_update_n = 1; + RMT.tx_conf[channel].conf_update_n = 0; + // here gpio is 1 + delay(2); + PROBE_3_TOGGLE; + RMT.chnconf0[channel].idle_out_lv_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + // here gpio is 0 + delay(2); + PROBE_3_TOGGLE; + RMT.chnconf0[channel].idle_out_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + // here gpio is 0 + delay(2); + PROBE_3_TOGGLE; + RMT.chnconf0[channel].idle_out_lv_n = 1; + RMT.chnconf0[channel].idle_out_en_n = 1; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + // here gpio is 1 + delay(2); + PROBE_3_TOGGLE; + RMT.chnconf0[channel].idle_out_lv_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + // here gpio is 0 + delay(2); + PROBE_3_TOGGLE; #endif } void StepperQueue::disconnect_rmt() { - disable_rmt_interrupts(channel); + disable_rmt_interrupts(channel); #ifndef __ESP32_IDF_V44__ // rmt_set_pin(channel, RMT_MODE_TX, (gpio_num_t)-1); #else // rmt_set_gpio(channel, RMT_MODE_TX, GPIO_NUM_NC, false); #endif - RMT.chnconf0[channel].idle_out_en_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; + RMT.chnconf0[channel].idle_out_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; } void StepperQueue::startQueue_rmt() { #ifdef TRACE - USBSerial.println("START"); + USBSerial.println("START"); #endif #ifdef TEST_PROBE_2 - PROBE_2(LOW); + PROBE_2(LOW); #endif #ifdef TEST_PROBE_3 - PROBE_3(LOW); + PROBE_3(LOW); #endif #if defined(TEST_PROBE_1) && defined(TEST_PROBE_2) && defined(TEST_PROBE_3) - delay(1); - PROBE_1_TOGGLE; - delay(1); - PROBE_1_TOGGLE; - delay(1); - PROBE_1_TOGGLE; - delay(1); - PROBE_2_TOGGLE; - delay(2); - PROBE_2_TOGGLE; - delay(2); - PROBE_3_TOGGLE; - delay(3); - PROBE_3_TOGGLE; - delay(3); + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_1_TOGGLE; + delay(1); + PROBE_2_TOGGLE; + delay(2); + PROBE_2_TOGGLE; + delay(2); + PROBE_3_TOGGLE; + delay(3); + PROBE_3_TOGGLE; + delay(3); #endif - rmt_tx_stop(channel); - // rmt_rx_stop(channel); - RMT.chnconf0[channel].mem_rd_rst_n = 1; - RMT.chnconf0[channel].mem_rd_rst_n = 0; - uint32_t *mem = FAS_RMT_MEM(channel); + rmt_tx_stop(channel); + // rmt_rx_stop(channel); + RMT.chnconf0[channel].mem_rd_rst_n = 1; + RMT.chnconf0[channel].mem_rd_rst_n = 0; + uint32_t *mem = FAS_RMT_MEM(channel); // #define TRACE #ifdef TRACE - // Fill the buffer with a significant pattern for debugging - // Keep it for now - for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { - // mem[i] = 0x0fff8fff; - // mem[i + 1] = 0x7fff8fff; - } + // Fill the buffer with a significant pattern for debugging + // Keep it for now + for (uint8_t i = 0; i < 2 * PART_SIZE; i += 2) { + // mem[i] = 0x0fff8fff; + // mem[i + 1] = 0x7fff8fff; + } #endif - // Write end marker - mem[2 * PART_SIZE] = 0; - mem[2 * PART_SIZE + 1] = 0; - _isRunning = true; - _rmtStopped = false; - disable_rmt_interrupts(channel); - RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; + // Write end marker + mem[2 * PART_SIZE] = 0; + mem[2 * PART_SIZE + 1] = 0; + _isRunning = true; + _rmtStopped = false; + disable_rmt_interrupts(channel); + RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; #ifdef TRACE - USBSerial.print("Queue:"); - USBSerial.print(read_idx); - USBSerial.print('/'); - USBSerial.println(next_write_idx); - for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { - USBSerial.print(i); - USBSerial.print(' '); - USBSerial.println(mem[i], HEX); - } + USBSerial.print("Queue:"); + USBSerial.print(read_idx); + USBSerial.print('/'); + USBSerial.println(next_write_idx); + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } #endif - // set dirpin toggle here - uint8_t rp = read_idx; - if (rp == next_write_idx) { - // nothing to do ? - // Should not happen, so bail - return; - } - if (entry[rp & QUEUE_LEN_MASK].toggle_dir) { - gpio_set_level((gpio_num_t)dirPin, gpio_get_level((gpio_num_t)dirPin) ^ 1); - entry[rp & QUEUE_LEN_MASK].toggle_dir = false; - } - - bufferContainsSteps[0] = true; - bufferContainsSteps[1] = true; - apply_command(this, true, mem); + // set dirpin toggle here + uint8_t rp = read_idx; + if (rp == next_write_idx) { + // nothing to do ? + // Should not happen, so bail + return; + } + if (entry[rp & QUEUE_LEN_MASK].toggle_dir) { + gpio_set_level((gpio_num_t)dirPin, gpio_get_level((gpio_num_t)dirPin) ^ 1); + entry[rp & QUEUE_LEN_MASK].toggle_dir = false; + } + + bufferContainsSteps[0] = true; + bufferContainsSteps[1] = true; + apply_command(this, true, mem); #ifdef TRACE - USBSerial.print(RMT.chnconf0[channel].val, BIN); - USBSerial.println(' '); - USBSerial.print(RMT.chnconf0[channel].mem_tx_wrap_en_n); - USBSerial.println(' '); - for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { - USBSerial.print(i); - USBSerial.print(' '); - USBSerial.println(mem[i], HEX); - } - if (!isRunning()) { - USBSerial.println("STOPPED 1"); - } + USBSerial.print(RMT.chnconf0[channel].val, BIN); + USBSerial.println(' '); + USBSerial.print(RMT.chnconf0[channel].mem_tx_wrap_en_n); + USBSerial.println(' '); + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } + if (!isRunning()) { + USBSerial.println("STOPPED 1"); + } #endif - apply_command(this, false, mem); + apply_command(this, false, mem); #ifdef TRACE - USBSerial.print(RMT.chnconf0[channel].val, BIN); - USBSerial.print(' '); - USBSerial.print(RMT.chnconf0[channel].mem_tx_wrap_en_n); - USBSerial.println(' '); + USBSerial.print(RMT.chnconf0[channel].val, BIN); + USBSerial.print(' '); + USBSerial.print(RMT.chnconf0[channel].mem_tx_wrap_en_n); + USBSerial.println(' '); - for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { - USBSerial.print(i); - USBSerial.print(' '); - USBSerial.println(mem[i], HEX); - } + for (uint8_t i = 0; i < RMT_MEM_SIZE; i++) { + USBSerial.print(i); + USBSerial.print(' '); + USBSerial.println(mem[i], HEX); + } #endif - enable_rmt_interrupts(channel); + enable_rmt_interrupts(channel); #ifdef TRACE - USBSerial.print("Interrupt enable:"); - USBSerial.println(RMT.int_ena.val, BIN); - USBSerial.print("Interrupt status:"); - USBSerial.println(RMT.int_st.val, BIN); - USBSerial.print("Threshold: 0x"); - USBSerial.println(RMT.tx_lim[channel].val, HEX); + USBSerial.print("Interrupt enable:"); + USBSerial.println(RMT.int_ena.val, BIN); + USBSerial.print("Interrupt status:"); + USBSerial.println(RMT.int_st.val, BIN); + USBSerial.print("Threshold: 0x"); + USBSerial.println(RMT.tx_lim[channel].val, HEX); #endif - // This starts the rmt module - RMT.chnconf0[channel].tx_conti_mode_n = 1; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; - RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; - RMT.chnconf0[channel].conf_update_n = 1; - RMT.chnconf0[channel].conf_update_n = 0; + // This starts the rmt module + RMT.chnconf0[channel].tx_conti_mode_n = 1; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; + RMT.chnconf0[channel].mem_tx_wrap_en_n = 0; + RMT.chnconf0[channel].conf_update_n = 1; + RMT.chnconf0[channel].conf_update_n = 0; - PROBE_1_TOGGLE; - RMT.chnconf0[channel].tx_start_n = 1; + PROBE_1_TOGGLE; + RMT.chnconf0[channel].tx_start_n = 1; } void StepperQueue::forceStop_rmt() { - stop_rmt(true); + stop_rmt(true); - // and empty the buffer - read_idx = next_write_idx; + // and empty the buffer + read_idx = next_write_idx; } bool StepperQueue::isReadyForCommands_rmt() { - if (_isRunning) { - return !_rmtStopped; - } - return true; + if (_isRunning) { + return !_rmtStopped; + } + return true; } uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; } #endif diff --git a/src/fas_common.h b/src/fas_common.h index 87c0e481..a3e66cd4 100644 --- a/src/fas_common.h +++ b/src/fas_common.h @@ -11,20 +11,20 @@ // ticks is multiplied by (1/TICKS_PER_S) in s // If steps is 0, then a pause is generated struct stepper_command_s { - uint16_t ticks; - uint8_t steps; - bool count_up; + uint16_t ticks; + uint8_t steps; + bool count_up; }; struct actual_ticks_s { - uint32_t ticks; // ticks == 0 means standstill - bool count_up; + uint32_t ticks; // ticks == 0 means standstill + bool count_up; }; struct queue_end_s { - volatile int32_t pos; // in steps - volatile bool count_up; - volatile bool dir; + volatile int32_t pos; // in steps + volatile bool count_up; + volatile bool dir; }; // use own min/max/abs function, because the lib versions are messed up @@ -358,8 +358,8 @@ struct queue_end_s { // for AVR processors a reentrant version of disabling/enabling interrupts is // used #define fasDisableInterrupts() \ - uint8_t prevSREG = SREG; \ - cli() + uint8_t prevSREG = SREG; \ + cli() #define fasEnableInterrupts() SREG = prevSREG // Here are shorthand definitions for number of queues, the queues/channel @@ -392,8 +392,7 @@ struct queue_end_s { #define NUM_QUEUES 2 #define fas_queue_A fas_queue[0] #define fas_queue_B fas_queue[1] -enum channels { channelA, - channelB }; +enum channels { channelA, channelB }; //========================================================================== // // AVR derivate ATmega 2560 @@ -406,9 +405,7 @@ enum channels { channelA, #define fas_queue_A fas_queue[0] #define fas_queue_B fas_queue[1] #define fas_queue_C fas_queue[2] -enum channels { channelA, - channelB, - channelC }; +enum channels { channelA, channelB, channelC }; //========================================================================== // // AVR derivate ATmega 32U4 @@ -420,9 +417,7 @@ enum channels { channelA, #define fas_queue_A fas_queue[0] #define fas_queue_B fas_queue[1] #define fas_queue_C fas_queue[2] -enum channels { channelA, - channelB, - channelC }; +enum channels { channelA, channelB, channelC }; //========================================================================== // // For all unsupported AVR derivates From 114e87c204ffdf4455b853392dab2802f58ee132 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 11:28:53 +0100 Subject: [PATCH 43/85] update CHANGELOG and README --- CHANGELOG.md | 3 +++ README.md | 13 +++++++------ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2de6b152..fbe84bf6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,9 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works +pre-0.30.11: +- esp32s3: add support for rmt from patch #225 + 0.30.10: - Unify code in stepperConnectToPin to fix bug mentioned in #221 This ensures setting of per stepper speed limit is working for avr variants without side effect diff --git a/README.md b/README.md index f6c9b85f..7fb3b2ce 100644 --- a/README.md +++ b/README.md @@ -280,12 +280,12 @@ A note to `MIN_CMD_TICKS` using mcpwm/pcnt: The current implementation uses one What are the differences between mcpwm/pcnt and rmt ? -| | mcpwm/pcnt | rmt | -| :-------------------------- | :-------------------------------------- | :---------------------------------------------------------------------------- | -| Interrupt rate/stepper | one interrupt per command | min: one interrupt per command, max: one interrupt per 31 steps at high speed | -| Required interrupt response | at high speed: time between two steps | at high speed: time between 31 steps | -| Module usage | 1 or 2 mcpcms, up to 6 channels of pcnt | rmt | -| esp32 notes | availabe pcnt modules can be connected | no pcnt module used, so can be attached to rmt output as realtime position | +| | mcpwm/pcnt | rmt | +|:---------------------------|:----------------------------------------|:------------------------------------------------------------------------------| +|Interrupt rate/stepper | one interrupt per command | min: one interrupt per command, max: one interrupt per 31 steps at high speed | +|Required interrupt response | at high speed: time between two steps | at high speed: time between 31 steps | +|Module usage | 1 or 2 mcpcms, up to 6 channels of pcnt | rmt | +|esp32 notes | availabe pcnt modules can be connected | no pcnt module used, so can be attached to rmt output as realtime position | If the interrupt load is not an issue, then rmt is the better choice. With rmt the below (multi-axis application) mentioned loss of synchonicity at high speeds can be avoided. The rmt driver is - besides some rmt modules perks - less complex and way more straightforward. @@ -470,4 +470,5 @@ As mentioned by kthod861 in [Issue #110](https://github.com/gin66/FastAccelStepp - Thanks HeldeReis for the awesome ESP32-S3 port (https://github.com/gin66/FastAccelStepper/pull/162) - Thanks DaAwesomeP for the extension to ATmega 168/168P/328 (https://github.com/gin66/FastAccelStepper/pull/179) - Thanks turley for the patch for missing `_stepper_cnt` initialization (https://github.com/gin66/FastAccelStepper/pull/204) +- Thanks GarmischWg for adding rmt-support to ESP32-S3 (https://github.com/gin66/FastAccelStepper/pull/225) From 51d0a66285a867915df2e839849372b09b4d3932 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 11:33:37 +0100 Subject: [PATCH 44/85] esp32: determine from defined rmt/mcpwm queues, if driver type support should be enabled --- src/fas_common.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/fas_common.h b/src/fas_common.h index a3e66cd4..05b0064d 100644 --- a/src/fas_common.h +++ b/src/fas_common.h @@ -118,7 +118,6 @@ struct queue_end_s { #include #include -#define SUPPORT_SELECT_DRIVER_TYPE #define SUPPORT_ESP32_MCPWM_PCNT #define SUPPORT_ESP32_RMT #include @@ -442,4 +441,12 @@ enum channels { channelA, channelB, channelC }; #include #endif /* __ESP32_IDF_V44__ */ +//========================================================================== +// determine, if driver type selection should be supported +#if defined(QUEUES_MCPWM_PCNT) && defined(QUEUES_RMT) +#if (QUEUES_MCPWM_PCNT > 0) && (QUEUES_RMT > 0) +#define SUPPORT_SELECT_DRIVER_TYPE +#endif +#endif + #endif /* COMMON_H */ From a53089cb3d004217961e4bc19d64e9caf705daa0 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 12:09:43 +0100 Subject: [PATCH 45/85] test more versions for esp32 --- .../workflows/build_examples_esp32_V3_5_0.yml | 21 +++ .../workflows/build_examples_esp32_V4_0_0.yml | 21 +++ .../workflows/build_examples_esp32_V4_1_0.yml | 21 +++ .../workflows/build_examples_esp32_V4_2_0.yml | 21 +++ .../workflows/build_examples_esp32_V4_3_0.yml | 21 +++ .../workflows/build_examples_esp32_V4_4_0.yml | 21 +++ .../workflows/build_examples_esp32_V5_0_0.yml | 21 +++ .../workflows/build_examples_esp32_V5_1_1.yml | 21 +++ .../workflows/build_examples_esp32_V5_2_0.yml | 21 +++ .../workflows/build_examples_esp32_V5_3_0.yml | 21 +++ .../workflows/build_examples_esp32_V6_0_1.yml | 21 +++ .../workflows/build_examples_esp32_V6_1_0.yml | 21 +++ .../workflows/build_examples_esp32_V6_2_0.yml | 21 +++ .../workflows/build_examples_esp32_V6_3_2.yml | 21 +++ .../workflows/build_examples_esp32_V6_4_0.yml | 21 +++ .../workflows/build_examples_esp32_V6_5_0.yml | 21 +++ .../build_examples_esp32c3_V6_5_0.yml | 21 +++ .../build_examples_esp32s2_V6_5_0.yml | 21 +++ .../build_examples_esp32s3_V6_5_0.yml | 21 +++ extras/ci/platformio.ini | 152 ++++++++++++++++++ extras/scripts/build-workflows.py | 48 ++++++ 21 files changed, 599 insertions(+) create mode 100644 .github/workflows/build_examples_esp32_V3_5_0.yml create mode 100644 .github/workflows/build_examples_esp32_V4_0_0.yml create mode 100644 .github/workflows/build_examples_esp32_V4_1_0.yml create mode 100644 .github/workflows/build_examples_esp32_V4_2_0.yml create mode 100644 .github/workflows/build_examples_esp32_V4_3_0.yml create mode 100644 .github/workflows/build_examples_esp32_V4_4_0.yml create mode 100644 .github/workflows/build_examples_esp32_V5_0_0.yml create mode 100644 .github/workflows/build_examples_esp32_V5_1_1.yml create mode 100644 .github/workflows/build_examples_esp32_V5_2_0.yml create mode 100644 .github/workflows/build_examples_esp32_V5_3_0.yml create mode 100644 .github/workflows/build_examples_esp32_V6_0_1.yml create mode 100644 .github/workflows/build_examples_esp32_V6_1_0.yml create mode 100644 .github/workflows/build_examples_esp32_V6_2_0.yml create mode 100644 .github/workflows/build_examples_esp32_V6_3_2.yml create mode 100644 .github/workflows/build_examples_esp32_V6_4_0.yml create mode 100644 .github/workflows/build_examples_esp32_V6_5_0.yml create mode 100644 .github/workflows/build_examples_esp32c3_V6_5_0.yml create mode 100644 .github/workflows/build_examples_esp32s2_V6_5_0.yml create mode 100644 .github/workflows/build_examples_esp32s3_V6_5_0.yml create mode 100755 extras/scripts/build-workflows.py diff --git a/.github/workflows/build_examples_esp32_V3_5_0.yml b/.github/workflows/build_examples_esp32_V3_5_0.yml new file mode 100644 index 00000000..5506e17e --- /dev/null +++ b/.github/workflows/build_examples_esp32_V3_5_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V3_5_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V3_5_0 diff --git a/.github/workflows/build_examples_esp32_V4_0_0.yml b/.github/workflows/build_examples_esp32_V4_0_0.yml new file mode 100644 index 00000000..d35f8441 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V4_0_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V4_0_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V4_0_0 diff --git a/.github/workflows/build_examples_esp32_V4_1_0.yml b/.github/workflows/build_examples_esp32_V4_1_0.yml new file mode 100644 index 00000000..cfcf4fc7 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V4_1_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V4_1_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V4_1_0 diff --git a/.github/workflows/build_examples_esp32_V4_2_0.yml b/.github/workflows/build_examples_esp32_V4_2_0.yml new file mode 100644 index 00000000..786e17ae --- /dev/null +++ b/.github/workflows/build_examples_esp32_V4_2_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V4_2_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V4_2_0 diff --git a/.github/workflows/build_examples_esp32_V4_3_0.yml b/.github/workflows/build_examples_esp32_V4_3_0.yml new file mode 100644 index 00000000..25da79f6 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V4_3_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V4_3_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V4_3_0 diff --git a/.github/workflows/build_examples_esp32_V4_4_0.yml b/.github/workflows/build_examples_esp32_V4_4_0.yml new file mode 100644 index 00000000..f540ab09 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V4_4_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V4_4_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V4_4_0 diff --git a/.github/workflows/build_examples_esp32_V5_0_0.yml b/.github/workflows/build_examples_esp32_V5_0_0.yml new file mode 100644 index 00000000..baa6c5d0 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V5_0_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V5_0_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V5_0_0 diff --git a/.github/workflows/build_examples_esp32_V5_1_1.yml b/.github/workflows/build_examples_esp32_V5_1_1.yml new file mode 100644 index 00000000..c5f4f1aa --- /dev/null +++ b/.github/workflows/build_examples_esp32_V5_1_1.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V5_1_1 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V5_1_1 diff --git a/.github/workflows/build_examples_esp32_V5_2_0.yml b/.github/workflows/build_examples_esp32_V5_2_0.yml new file mode 100644 index 00000000..2d6b6e3a --- /dev/null +++ b/.github/workflows/build_examples_esp32_V5_2_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V5_2_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V5_2_0 diff --git a/.github/workflows/build_examples_esp32_V5_3_0.yml b/.github/workflows/build_examples_esp32_V5_3_0.yml new file mode 100644 index 00000000..dfb1f213 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V5_3_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V5_3_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V5_3_0 diff --git a/.github/workflows/build_examples_esp32_V6_0_1.yml b/.github/workflows/build_examples_esp32_V6_0_1.yml new file mode 100644 index 00000000..ea80e07d --- /dev/null +++ b/.github/workflows/build_examples_esp32_V6_0_1.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V6_0_1 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V6_0_1 diff --git a/.github/workflows/build_examples_esp32_V6_1_0.yml b/.github/workflows/build_examples_esp32_V6_1_0.yml new file mode 100644 index 00000000..5273a006 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V6_1_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V6_1_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V6_1_0 diff --git a/.github/workflows/build_examples_esp32_V6_2_0.yml b/.github/workflows/build_examples_esp32_V6_2_0.yml new file mode 100644 index 00000000..e0b8c478 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V6_2_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V6_2_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V6_2_0 diff --git a/.github/workflows/build_examples_esp32_V6_3_2.yml b/.github/workflows/build_examples_esp32_V6_3_2.yml new file mode 100644 index 00000000..1d282fc0 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V6_3_2.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V6_3_2 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V6_3_2 diff --git a/.github/workflows/build_examples_esp32_V6_4_0.yml b/.github/workflows/build_examples_esp32_V6_4_0.yml new file mode 100644 index 00000000..7ff61bd3 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V6_4_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V6_4_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V6_4_0 diff --git a/.github/workflows/build_examples_esp32_V6_5_0.yml b/.github/workflows/build_examples_esp32_V6_5_0.yml new file mode 100644 index 00000000..5b63c107 --- /dev/null +++ b/.github/workflows/build_examples_esp32_V6_5_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V6_5_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32V6_5_0 diff --git a/.github/workflows/build_examples_esp32c3_V6_5_0.yml b/.github/workflows/build_examples_esp32c3_V6_5_0.yml new file mode 100644 index 00000000..39a29c8f --- /dev/null +++ b/.github/workflows/build_examples_esp32c3_V6_5_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32c3 @ V6_5_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32c3V6_5_0 diff --git a/.github/workflows/build_examples_esp32s2_V6_5_0.yml b/.github/workflows/build_examples_esp32s2_V6_5_0.yml new file mode 100644 index 00000000..d41c5075 --- /dev/null +++ b/.github/workflows/build_examples_esp32s2_V6_5_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32s2 @ V6_5_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32s2V6_5_0 diff --git a/.github/workflows/build_examples_esp32s3_V6_5_0.yml b/.github/workflows/build_examples_esp32s3_V6_5_0.yml new file mode 100644 index 00000000..56df0669 --- /dev/null +++ b/.github/workflows/build_examples_esp32s3_V6_5_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32s3 @ V6_5_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32s3V6_5_0 diff --git a/extras/ci/platformio.ini b/extras/ci/platformio.ini index f5aeeb5f..8e5d3509 100644 --- a/extras/ci/platformio.ini +++ b/extras/ci/platformio.ini @@ -21,6 +21,158 @@ build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. +[env:esp32_V6_5_0] +platform = espressif32 @ 6.5.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_4_0] +platform = espressif32 @ 6.4.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_3_2] +platform = espressif32 @ 6.3.2 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_2_0] +platform = espressif32 @ 6.2.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_1_0] +platform = espressif32 @ 6.1.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_0_1] +platform = espressif32 @ 6.0.1 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V5_3_0] +platform = espressif32 @ 5.3.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V5_2_0] +platform = espressif32 @ 5.2.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V5_1_1] +platform = espressif32 @ 5.1.1 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V5_0_0] +platform = espressif32 @ 5.0.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V4_4_0] +platform = espressif32 @ 4.4.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V4_3_0] +platform = espressif32 @ 4.3.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V4_2_0] +platform = espressif32 @ 4.2.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V4_1_0] +platform = espressif32 @ 4.1.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V4_0_0] +platform = espressif32 @ 4.0.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V3_5_0] +platform = espressif32 @ 3.5.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s2_V6_5_0] +platform = espressif32 @ 6.5.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s3_V6_5_0] +platform = espressif32 @ 6.5.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32c3_V6_5_0] +platform = espressif32 @ 6.5.0 +board = esp32dev +framework = arduino +build_flags = -Werror -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + [env:esp32arduinolatest] platform = espressif32 board = esp32dev diff --git a/extras/scripts/build-workflows.py b/extras/scripts/build-workflows.py new file mode 100755 index 00000000..65ee7fab --- /dev/null +++ b/extras/scripts/build-workflows.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import configparser + +fname = "../ci/platformio.ini" +action_dirs = "../../.github/workflows/" + +template = """ +name: Build examples for {derivate} @ {version} + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh {derivate}{version} +""" + +# Create a ConfigParser object +config = configparser.ConfigParser() + +# Read the INI file +config.read(fname) + +# Loop through sections +for section in config.sections(): + s = section.replace("env:","") + flds = s.split("_") + derivate = flds[0] + version = s.replace(derivate + "_","") + if version[0] == "V": + print(f"Section: {derivate} {version}") + action = template.format(derivate=derivate,version=version) + fname = f"build_examples_{derivate}_{version}.yml" + with open(action_dirs + fname,"w") as fp: + fp.write(action) From de2d375f5ba415bfee744b4ca8cb3e7235a719a1 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 12:11:35 +0100 Subject: [PATCH 46/85] fix github actions for esp32 --- .github/workflows/build_examples_esp32_V3_5_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_0_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_1_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_2_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_3_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_4_0.yml | 2 +- .github/workflows/build_examples_esp32_V5_0_0.yml | 2 +- .github/workflows/build_examples_esp32_V5_1_1.yml | 2 +- .github/workflows/build_examples_esp32_V5_2_0.yml | 2 +- .github/workflows/build_examples_esp32_V5_3_0.yml | 2 +- .github/workflows/build_examples_esp32_V6_0_1.yml | 2 +- .github/workflows/build_examples_esp32_V6_1_0.yml | 2 +- .github/workflows/build_examples_esp32_V6_2_0.yml | 2 +- .github/workflows/build_examples_esp32_V6_3_2.yml | 2 +- .github/workflows/build_examples_esp32_V6_4_0.yml | 2 +- .github/workflows/build_examples_esp32_V6_5_0.yml | 2 +- .github/workflows/build_examples_esp32c3_V6_5_0.yml | 2 +- .github/workflows/build_examples_esp32s2_V6_5_0.yml | 2 +- .github/workflows/build_examples_esp32s3_V6_5_0.yml | 2 +- extras/scripts/build-workflows.py | 2 +- 20 files changed, 20 insertions(+), 20 deletions(-) diff --git a/.github/workflows/build_examples_esp32_V3_5_0.yml b/.github/workflows/build_examples_esp32_V3_5_0.yml index 5506e17e..6e71ae10 100644 --- a/.github/workflows/build_examples_esp32_V3_5_0.yml +++ b/.github/workflows/build_examples_esp32_V3_5_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V3_5_0 + run: bash extras/scripts/build-platformio.sh esp32_V3_5_0 diff --git a/.github/workflows/build_examples_esp32_V4_0_0.yml b/.github/workflows/build_examples_esp32_V4_0_0.yml index d35f8441..f77cbbe8 100644 --- a/.github/workflows/build_examples_esp32_V4_0_0.yml +++ b/.github/workflows/build_examples_esp32_V4_0_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V4_0_0 + run: bash extras/scripts/build-platformio.sh esp32_V4_0_0 diff --git a/.github/workflows/build_examples_esp32_V4_1_0.yml b/.github/workflows/build_examples_esp32_V4_1_0.yml index cfcf4fc7..74d4c51c 100644 --- a/.github/workflows/build_examples_esp32_V4_1_0.yml +++ b/.github/workflows/build_examples_esp32_V4_1_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V4_1_0 + run: bash extras/scripts/build-platformio.sh esp32_V4_1_0 diff --git a/.github/workflows/build_examples_esp32_V4_2_0.yml b/.github/workflows/build_examples_esp32_V4_2_0.yml index 786e17ae..1614e6c6 100644 --- a/.github/workflows/build_examples_esp32_V4_2_0.yml +++ b/.github/workflows/build_examples_esp32_V4_2_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V4_2_0 + run: bash extras/scripts/build-platformio.sh esp32_V4_2_0 diff --git a/.github/workflows/build_examples_esp32_V4_3_0.yml b/.github/workflows/build_examples_esp32_V4_3_0.yml index 25da79f6..3c222756 100644 --- a/.github/workflows/build_examples_esp32_V4_3_0.yml +++ b/.github/workflows/build_examples_esp32_V4_3_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V4_3_0 + run: bash extras/scripts/build-platformio.sh esp32_V4_3_0 diff --git a/.github/workflows/build_examples_esp32_V4_4_0.yml b/.github/workflows/build_examples_esp32_V4_4_0.yml index f540ab09..453c13a9 100644 --- a/.github/workflows/build_examples_esp32_V4_4_0.yml +++ b/.github/workflows/build_examples_esp32_V4_4_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V4_4_0 + run: bash extras/scripts/build-platformio.sh esp32_V4_4_0 diff --git a/.github/workflows/build_examples_esp32_V5_0_0.yml b/.github/workflows/build_examples_esp32_V5_0_0.yml index baa6c5d0..31cdd78b 100644 --- a/.github/workflows/build_examples_esp32_V5_0_0.yml +++ b/.github/workflows/build_examples_esp32_V5_0_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V5_0_0 + run: bash extras/scripts/build-platformio.sh esp32_V5_0_0 diff --git a/.github/workflows/build_examples_esp32_V5_1_1.yml b/.github/workflows/build_examples_esp32_V5_1_1.yml index c5f4f1aa..8743c2a8 100644 --- a/.github/workflows/build_examples_esp32_V5_1_1.yml +++ b/.github/workflows/build_examples_esp32_V5_1_1.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V5_1_1 + run: bash extras/scripts/build-platformio.sh esp32_V5_1_1 diff --git a/.github/workflows/build_examples_esp32_V5_2_0.yml b/.github/workflows/build_examples_esp32_V5_2_0.yml index 2d6b6e3a..4302b4e3 100644 --- a/.github/workflows/build_examples_esp32_V5_2_0.yml +++ b/.github/workflows/build_examples_esp32_V5_2_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V5_2_0 + run: bash extras/scripts/build-platformio.sh esp32_V5_2_0 diff --git a/.github/workflows/build_examples_esp32_V5_3_0.yml b/.github/workflows/build_examples_esp32_V5_3_0.yml index dfb1f213..4ba455e7 100644 --- a/.github/workflows/build_examples_esp32_V5_3_0.yml +++ b/.github/workflows/build_examples_esp32_V5_3_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V5_3_0 + run: bash extras/scripts/build-platformio.sh esp32_V5_3_0 diff --git a/.github/workflows/build_examples_esp32_V6_0_1.yml b/.github/workflows/build_examples_esp32_V6_0_1.yml index ea80e07d..48dbd58a 100644 --- a/.github/workflows/build_examples_esp32_V6_0_1.yml +++ b/.github/workflows/build_examples_esp32_V6_0_1.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V6_0_1 + run: bash extras/scripts/build-platformio.sh esp32_V6_0_1 diff --git a/.github/workflows/build_examples_esp32_V6_1_0.yml b/.github/workflows/build_examples_esp32_V6_1_0.yml index 5273a006..0e2572ce 100644 --- a/.github/workflows/build_examples_esp32_V6_1_0.yml +++ b/.github/workflows/build_examples_esp32_V6_1_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V6_1_0 + run: bash extras/scripts/build-platformio.sh esp32_V6_1_0 diff --git a/.github/workflows/build_examples_esp32_V6_2_0.yml b/.github/workflows/build_examples_esp32_V6_2_0.yml index e0b8c478..d43cc073 100644 --- a/.github/workflows/build_examples_esp32_V6_2_0.yml +++ b/.github/workflows/build_examples_esp32_V6_2_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V6_2_0 + run: bash extras/scripts/build-platformio.sh esp32_V6_2_0 diff --git a/.github/workflows/build_examples_esp32_V6_3_2.yml b/.github/workflows/build_examples_esp32_V6_3_2.yml index 1d282fc0..6ff2d197 100644 --- a/.github/workflows/build_examples_esp32_V6_3_2.yml +++ b/.github/workflows/build_examples_esp32_V6_3_2.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V6_3_2 + run: bash extras/scripts/build-platformio.sh esp32_V6_3_2 diff --git a/.github/workflows/build_examples_esp32_V6_4_0.yml b/.github/workflows/build_examples_esp32_V6_4_0.yml index 7ff61bd3..ce885a3b 100644 --- a/.github/workflows/build_examples_esp32_V6_4_0.yml +++ b/.github/workflows/build_examples_esp32_V6_4_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V6_4_0 + run: bash extras/scripts/build-platformio.sh esp32_V6_4_0 diff --git a/.github/workflows/build_examples_esp32_V6_5_0.yml b/.github/workflows/build_examples_esp32_V6_5_0.yml index 5b63c107..3c5fd12c 100644 --- a/.github/workflows/build_examples_esp32_V6_5_0.yml +++ b/.github/workflows/build_examples_esp32_V6_5_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32V6_5_0 + run: bash extras/scripts/build-platformio.sh esp32_V6_5_0 diff --git a/.github/workflows/build_examples_esp32c3_V6_5_0.yml b/.github/workflows/build_examples_esp32c3_V6_5_0.yml index 39a29c8f..1daf5228 100644 --- a/.github/workflows/build_examples_esp32c3_V6_5_0.yml +++ b/.github/workflows/build_examples_esp32c3_V6_5_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32c3V6_5_0 + run: bash extras/scripts/build-platformio.sh esp32c3_V6_5_0 diff --git a/.github/workflows/build_examples_esp32s2_V6_5_0.yml b/.github/workflows/build_examples_esp32s2_V6_5_0.yml index d41c5075..d22a68d7 100644 --- a/.github/workflows/build_examples_esp32s2_V6_5_0.yml +++ b/.github/workflows/build_examples_esp32s2_V6_5_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32s2V6_5_0 + run: bash extras/scripts/build-platformio.sh esp32s2_V6_5_0 diff --git a/.github/workflows/build_examples_esp32s3_V6_5_0.yml b/.github/workflows/build_examples_esp32s3_V6_5_0.yml index 56df0669..33e700d2 100644 --- a/.github/workflows/build_examples_esp32s3_V6_5_0.yml +++ b/.github/workflows/build_examples_esp32s3_V6_5_0.yml @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32s3V6_5_0 + run: bash extras/scripts/build-platformio.sh esp32s3_V6_5_0 diff --git a/extras/scripts/build-workflows.py b/extras/scripts/build-workflows.py index 65ee7fab..129c7705 100755 --- a/extras/scripts/build-workflows.py +++ b/extras/scripts/build-workflows.py @@ -25,7 +25,7 @@ - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh {derivate}{version} + run: bash extras/scripts/build-platformio.sh {derivate}_{version} """ # Create a ConfigParser object From b086dc98166763f05198aad941df9f463cc4eeb2 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 12:27:39 +0100 Subject: [PATCH 47/85] work on workflows --- .../workflows/build_examples_esp32_V4_0_0.yml | 21 ------------------- extras/ci/platformio.ini | 10 ++++----- extras/scripts/build-workflows.py | 3 +++ 3 files changed, 8 insertions(+), 26 deletions(-) delete mode 100644 .github/workflows/build_examples_esp32_V4_0_0.yml diff --git a/.github/workflows/build_examples_esp32_V4_0_0.yml b/.github/workflows/build_examples_esp32_V4_0_0.yml deleted file mode 100644 index f77cbbe8..00000000 --- a/.github/workflows/build_examples_esp32_V4_0_0.yml +++ /dev/null @@ -1,21 +0,0 @@ - -name: Build examples for esp32 @ V4_0_0 - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -jobs: - build: - - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Make directories - run: bash extras/scripts/build-pio-dirs.sh - - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32_V4_0_0 diff --git a/extras/ci/platformio.ini b/extras/ci/platformio.ini index 8e5d3509..c5831a51 100644 --- a/extras/ci/platformio.ini +++ b/extras/ci/platformio.ini @@ -25,7 +25,7 @@ lib_extra_dirs = ../../.. platform = espressif32 @ 6.5.0 board = esp32dev framework = arduino -build_flags = -Werror -Wall +build_flags = -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. @@ -133,7 +133,7 @@ build_flags = -Werror -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. -[env:esp32_V4_0_0] +[env:esp32_V4_0_0_disabled] platform = espressif32 @ 4.0.0 board = esp32dev framework = arduino @@ -153,7 +153,7 @@ lib_extra_dirs = ../../.. platform = espressif32 @ 6.5.0 board = esp32dev framework = arduino -build_flags = -Werror -Wall +build_flags = -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. @@ -161,7 +161,7 @@ lib_extra_dirs = ../../.. platform = espressif32 @ 6.5.0 board = esp32dev framework = arduino -build_flags = -Werror -Wall +build_flags = -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. @@ -169,7 +169,7 @@ lib_extra_dirs = ../../.. platform = espressif32 @ 6.5.0 board = esp32dev framework = arduino -build_flags = -Werror -Wall +build_flags = -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. diff --git a/extras/scripts/build-workflows.py b/extras/scripts/build-workflows.py index 129c7705..9eb00228 100755 --- a/extras/scripts/build-workflows.py +++ b/extras/scripts/build-workflows.py @@ -40,6 +40,9 @@ flds = s.split("_") derivate = flds[0] version = s.replace(derivate + "_","") + if "disabled" in s: + print(f"Skip: {derivate} {version}") + continue if version[0] == "V": print(f"Section: {derivate} {version}") action = template.format(derivate=derivate,version=version) From f7017e4215c1e17ebcd5984f4713c9ee62ce6ecc Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 12:34:23 +0100 Subject: [PATCH 48/85] delete old esp32 workflows and reduce warnings --- .github/workflows/build_examples_esp32.yml | 21 ------------------- .../build_examples_esp32arduinoV340.yml | 21 ------------------- .../build_examples_esp32arduinolatest.yml | 21 ------------------- .github/workflows/build_examples_esp32c3.yml | 21 ------------------- .github/workflows/build_examples_esp32s2.yml | 21 ------------------- .github/workflows/build_examples_esp32s3.yml | 21 ------------------- extras/ci/platformio.ini | 20 ++---------------- 7 files changed, 2 insertions(+), 144 deletions(-) delete mode 100644 .github/workflows/build_examples_esp32.yml delete mode 100644 .github/workflows/build_examples_esp32arduinoV340.yml delete mode 100644 .github/workflows/build_examples_esp32arduinolatest.yml delete mode 100644 .github/workflows/build_examples_esp32c3.yml delete mode 100644 .github/workflows/build_examples_esp32s2.yml delete mode 100644 .github/workflows/build_examples_esp32s3.yml diff --git a/.github/workflows/build_examples_esp32.yml b/.github/workflows/build_examples_esp32.yml deleted file mode 100644 index dbd57243..00000000 --- a/.github/workflows/build_examples_esp32.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build examples for esp32arduino @ latest - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -jobs: - build: - - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Make directories - run: bash extras/scripts/build-pio-dirs.sh - - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32 - diff --git a/.github/workflows/build_examples_esp32arduinoV340.yml b/.github/workflows/build_examples_esp32arduinoV340.yml deleted file mode 100644 index 1fa02a69..00000000 --- a/.github/workflows/build_examples_esp32arduinoV340.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build examples for esp32arduino @ 3.4.0 - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -jobs: - build: - - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Make directories - run: bash extras/scripts/build-pio-dirs.sh - - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32arduinoV340 - diff --git a/.github/workflows/build_examples_esp32arduinolatest.yml b/.github/workflows/build_examples_esp32arduinolatest.yml deleted file mode 100644 index e3c04a3f..00000000 --- a/.github/workflows/build_examples_esp32arduinolatest.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build examples for esp32arduino @ latest - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -jobs: - build: - - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Make directories - run: bash extras/scripts/build-pio-dirs.sh - - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32arduinolatest - diff --git a/.github/workflows/build_examples_esp32c3.yml b/.github/workflows/build_examples_esp32c3.yml deleted file mode 100644 index 49e1011a..00000000 --- a/.github/workflows/build_examples_esp32c3.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build examples for esp32c3arduino @ latest - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -jobs: - build: - - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Make directories - run: bash extras/scripts/build-pio-dirs.sh - - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32c3 - diff --git a/.github/workflows/build_examples_esp32s2.yml b/.github/workflows/build_examples_esp32s2.yml deleted file mode 100644 index 4b066209..00000000 --- a/.github/workflows/build_examples_esp32s2.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build examples for esp32s2arduino @ latest - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -jobs: - build: - - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Make directories - run: bash extras/scripts/build-pio-dirs.sh - - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32s2 - diff --git a/.github/workflows/build_examples_esp32s3.yml b/.github/workflows/build_examples_esp32s3.yml deleted file mode 100644 index 3afb9429..00000000 --- a/.github/workflows/build_examples_esp32s3.yml +++ /dev/null @@ -1,21 +0,0 @@ -name: Build examples for esp32s3arduino @ latest - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -jobs: - build: - - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - name: Make directories - run: bash extras/scripts/build-pio-dirs.sh - - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32s3 - diff --git a/extras/ci/platformio.ini b/extras/ci/platformio.ini index c5831a51..4148091a 100644 --- a/extras/ci/platformio.ini +++ b/extras/ci/platformio.ini @@ -17,7 +17,7 @@ platform = espressif32 board = esp32dev framework = arduino -build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types +build_flags = -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. @@ -145,7 +145,7 @@ lib_extra_dirs = ../../.. platform = espressif32 @ 3.5.0 board = esp32dev framework = arduino -build_flags = -Werror -Wall +build_flags = -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. @@ -173,22 +173,6 @@ build_flags = -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. -[env:esp32arduinolatest] -platform = espressif32 -board = esp32dev -framework = arduino -build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -board_build.f_cpu = 240000000L -lib_extra_dirs = ../../.. - -[env:esp32arduinoV340] -platform = espressif32 @ 3.4.0 -board = esp32dev -framework = arduino -build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -board_build.f_cpu = 240000000L -lib_extra_dirs = ../../.. - [env:esp32idf] platform = espressif32 board = esp32dev From 3b6bcec682fbc3cec810c04bc2d72c1f753a3f9f Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 12:45:16 +0100 Subject: [PATCH 49/85] update README for workflows --- README.md | 37 +++++++++++++++++++++++++++++++++---- 1 file changed, 33 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 7fb3b2ce..b9deba6b 100644 --- a/README.md +++ b/README.md @@ -14,15 +14,44 @@ No issue with platformio. Check the [related issue](https://github.com/arduino/l ![Run tests](https://github.com/gin66/FastAccelStepper/workflows/Run%20tests/badge.svg?no_cache_0.28.1) ![Simvar tests](https://github.com/gin66/FastAccelStepper/workflows/Run%20tests%20with%20simavr/badge.svg?no_cache_0.28.1) -[![Build examples for esp32](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32.yml) -[![Build examples for esp32s2](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32s2.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32s2.yml) -[![Build examples for esp32arduino @ 3.4.0](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32arduinoV340.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32arduinoV340.yml) -[![Build examples for esp32arduino @ latest](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32arduinolatest.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32arduinolatest.yml) +## Build for ESP33-platform + +Build examples for different versions: + +### esp32 +[![`V6_5_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_5_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_5_0.yml) +[![`V6_4_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_4_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_4_0.yml) +[![`V6_3_2`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_3_2.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_3_2.yml) +[![`V6_2_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_2_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_2_0.yml) +[![`V6_1_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_1_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_1_0.yml) +[![`V6_0_1`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_0_1.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_0_1.yml) +[![`V5_3_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V5_3_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V5_3_0.yml) +[![`V5_2_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V5_2_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V5_2_0.yml) +[![`V5_1_1`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V5_1_1.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V5_1_1.yml) +[![`V5_0_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V5_0_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V5_0_0.yml) +[![`V4_4_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V4_4_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V4_4_0.yml) +[![`V4_3_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V4_3_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V4_3_0.yml) +[![`V4_2_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V4_2_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V4_2_0.yml) +[![`V4_1_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V4_1_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V4_1_0.yml) +[![`V3_5_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V3_5_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V3_5_0.yml) + +### esp32s2 +[![`V6_5_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32s2_V6_5_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32s2_V6_5_0.yml) + +### esp32s3 +[![`V6_5_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32s3_V6_5_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32s3_V6_5_0.yml) + +### esp32c3 +[![`V6_5_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32c3_V6_5_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32c3_V6_5_0.yml) + +## Build for avr-platform [![Build examples for Atmega2560](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_atmega2560.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_atmega2560.yml) [![Build examples for Atmel SAM](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_atmelsam.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_atmelsam.yml) [![Build examples for Atmega168/328](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_nanoatmega328.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_nanoatmega328.yml) [![Build examples for Atmega32U4](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_atmega32u4.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_nanoatmega32u4.yml) +## Overview + This is a high speed alternative for the [AccelStepper library](http://www.airspayce.com/mikem/arduino/AccelStepper/). Supported are avr (ATmega 168/328/P, ATmega2560, ATmega32u4), esp32, esp32s2, esp32s3, esp32c3 and atmelsam due. From c1f89e49f916bb47891bc9cdf209e76fabd3e887 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 19 Jan 2024 12:48:52 +0100 Subject: [PATCH 50/85] bump version --- CHANGELOG.md | 2 +- README.md | 4 ++-- library.properties | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index fbe84bf6..9069d0b9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.11: +0.30.11: - esp32s3: add support for rmt from patch #225 0.30.10: diff --git a/README.md b/README.md index b9deba6b..e0319d8b 100644 --- a/README.md +++ b/README.md @@ -14,9 +14,9 @@ No issue with platformio. Check the [related issue](https://github.com/arduino/l ![Run tests](https://github.com/gin66/FastAccelStepper/workflows/Run%20tests/badge.svg?no_cache_0.28.1) ![Simvar tests](https://github.com/gin66/FastAccelStepper/workflows/Run%20tests%20with%20simavr/badge.svg?no_cache_0.28.1) -## Build for ESP33-platform +## Build for ESP32-platform -Build examples for different versions: +Build examples for different versions of espressif-arduino platform: ### esp32 [![`V6_5_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_5_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_5_0.yml) diff --git a/library.properties b/library.properties index 814926c0..ab02498f 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.10 +version=0.30.11 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes From 55e13d372e4fbe9446586ac4e934df5ef6990f96 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sun, 24 Mar 2024 10:36:49 +0100 Subject: [PATCH 51/85] fix rmt_memory_rw_rst() deprecation warning --- src/StepperISR_esp32_rmt.cpp | 2 +- src/StepperISR_esp32c3_rmt.cpp | 2 +- src/StepperISR_esp32s3_rmt.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/StepperISR_esp32_rmt.cpp b/src/StepperISR_esp32_rmt.cpp index 64fffa53..fffda9c8 100644 --- a/src/StepperISR_esp32_rmt.cpp +++ b/src/StepperISR_esp32_rmt.cpp @@ -407,7 +407,7 @@ void StepperQueue::startQueue_rmt() { #endif rmt_tx_stop(channel); // rmt_rx_stop(channel); - rmt_memory_rw_rst(channel); + rmt_tx_memory_reset(channel); // the following assignment should not be needed; // RMT.data_ch[channel] = 0; uint32_t *mem = FAS_RMT_MEM(channel); diff --git a/src/StepperISR_esp32c3_rmt.cpp b/src/StepperISR_esp32c3_rmt.cpp index 34e6efe4..dea0d67a 100644 --- a/src/StepperISR_esp32c3_rmt.cpp +++ b/src/StepperISR_esp32c3_rmt.cpp @@ -371,7 +371,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { rmt_set_mem_block_num(channel, 1); rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); rmt_tx_stop(channel); - rmt_memory_rw_rst(channel); + rmt_tx_memory_reset(channel); // rmt_rx_stop(channel); TX only channel ! // rmt_tx_memory_reset is not defined in arduino V340 and based on test // result not needed rmt_tx_memory_reset(channel); diff --git a/src/StepperISR_esp32s3_rmt.cpp b/src/StepperISR_esp32s3_rmt.cpp index 3281a325..996e430f 100644 --- a/src/StepperISR_esp32s3_rmt.cpp +++ b/src/StepperISR_esp32s3_rmt.cpp @@ -371,7 +371,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { rmt_set_mem_block_num(channel, 1); rmt_set_tx_carrier(channel, false, 0, 0, RMT_CARRIER_LEVEL_LOW); rmt_tx_stop(channel); - rmt_memory_rw_rst(channel); + rmt_tx_memory_reset(channel); // rmt_rx_stop(channel); TX only channel ! // rmt_tx_memory_reset is not defined in arduino V340 and based on test // result not needed rmt_tx_memory_reset(channel); From c8b69399520552cf7cfaaf4600125a69b0a7cd5f Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Sun, 24 Mar 2024 11:09:38 +0100 Subject: [PATCH 52/85] rmt_tx_memory_rst() is not supported on esp32@V3_5_0. as it is not needed, comment it out --- CHANGELOG.md | 3 +++ src/StepperISR_esp32_rmt.cpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9069d0b9..d7554fb9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,9 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works +pre-0.30.12: +- esp32: fix deprecation warning for `rmt_memory_rw_rst()` + 0.30.11: - esp32s3: add support for rmt from patch #225 diff --git a/src/StepperISR_esp32_rmt.cpp b/src/StepperISR_esp32_rmt.cpp index fffda9c8..e2b4ca59 100644 --- a/src/StepperISR_esp32_rmt.cpp +++ b/src/StepperISR_esp32_rmt.cpp @@ -407,7 +407,7 @@ void StepperQueue::startQueue_rmt() { #endif rmt_tx_stop(channel); // rmt_rx_stop(channel); - rmt_tx_memory_reset(channel); + // rmt_tx_memory_reset(channel); // the following assignment should not be needed; // RMT.data_ch[channel] = 0; uint32_t *mem = FAS_RMT_MEM(channel); From c827ba2ec090667bcfaacddeed2e416c8a6d9472 Mon Sep 17 00:00:00 2001 From: Wotever Date: Mon, 22 Apr 2024 10:58:03 +0200 Subject: [PATCH 53/85] Fix attempt for https://github.com/gin66/FastAccelStepper/issues/250 On quick direction changes a direction toggle could be missed. My understanding is that this code branch "restarts" the motion right at a stop event. But the direction change would not be applied either doing a few steps in the wrong direction, or if a large move was initiated at this time a much more impacting movement in the wrong direction. If it's motion restart it feels correct to toggle direction before that ultimate step, but i'm not 100% sure. --- src/StepperISR_avr.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index 7590778d..7764c2da 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -26,6 +26,7 @@ #define Stepper_IsDisconnected(T, X) \ ((TCCR##T##A & (_BV(COM##T##X##0) | _BV(COM##T##X##1))) == 0) #define Stepper_IsOneIfOutput(T, X) ((TCCR##T##A & _BV(COM##T##X##0)) != 0) +#define Stepper_ToggleDirection(CHANNEL) *fas_queue_##CHANNEL._dirPinPort ^= fas_queue_##CHANNEL._dirPinMask #ifdef SIMAVR_TIME_MEASUREMENT #define prepareISRtimeMeasurement() DDRB |= 0x18 @@ -119,6 +120,8 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { #endif } + + // The interrupt is called on compare event, which eventually // generates a L->H transition. In any case, the current command's // wait time has still to be executed for the next command, if any. @@ -161,6 +164,9 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { */ \ fas_queue_##CHANNEL._prepareForStop = false; \ if (e->steps > 0) { \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ + } \ /* That's the problem, so generate a step */ \ Stepper_One(T, CHANNEL); \ ForceCompare(T, CHANNEL); \ @@ -188,7 +194,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { Stepper_One(T, CHANNEL); \ } \ if (e->toggle_dir) { \ - *fas_queue_##CHANNEL._dirPinPort ^= fas_queue_##CHANNEL._dirPinMask; \ + Stepper_ToggleDirection(CHANNEL); \ } \ } else { \ fas_queue_##CHANNEL._prepareForStop = true; \ From 726793e7ba6c837f9c62f3c9fcc20fac5258e75e Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 26 Apr 2024 22:34:38 +0200 Subject: [PATCH 54/85] create test basis for issue #250 --- .gitignore | 1 + examples/Issue250/Issue250.ino | 58 ++++++++ .../simavr_based/test_issue250/.gitignore | 1 + .../tests/simavr_based/test_issue250/Makefile | 132 ++++++++++++++++++ .../simavr_based/test_issue250/expect.txt | 18 +++ .../simavr_based/test_issue250/platformio.ini | 31 ++++ 6 files changed, 241 insertions(+) create mode 100644 examples/Issue250/Issue250.ino create mode 100644 extras/tests/simavr_based/test_issue250/.gitignore create mode 100644 extras/tests/simavr_based/test_issue250/Makefile create mode 100644 extras/tests/simavr_based/test_issue250/expect.txt create mode 100644 extras/tests/simavr_based/test_issue250/platformio.ini diff --git a/.gitignore b/.gitignore index 301098ca..782b9324 100644 --- a/.gitignore +++ b/.gitignore @@ -32,3 +32,4 @@ extras/tests/simavr_based/test_sd*/Makefile extras/tests/simavr_based/test_seq*/Makefile extras/tests/esp32_hw_based/*.log library.properties +examples diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino new file mode 100644 index 00000000..5022489d --- /dev/null +++ b/examples/Issue250/Issue250.ino @@ -0,0 +1,58 @@ +#include "FastAccelStepper.h" + +#ifdef SIMULATOR +#include +#endif + +// As in StepperDemo for Motor 1 on AVR +#define dirPinStepper 5 +#define enablePinStepper 6 +#define stepPinStepper 9 // OC1A in case of AVR + +// As in StepperDemo for Motor 1 on ESP32 +//#define dirPinStepper 18 +//#define enablePinStepper 26 +//#define stepPinStepper 19 + +FastAccelStepperEngine engine = FastAccelStepperEngine(); +FastAccelStepper *stepper = NULL; + +void setup() { + Serial.begin(115200); + engine.init(); + stepper = engine.stepperConnectToPin(stepPinStepper); + if (stepper) { + stepper->setDirectionPin(dirPinStepper); + stepper->setEnablePin(enablePinStepper); + stepper->setAutoEnable(true); + + // If auto enable/disable need delays, just add (one or both): + stepper->setDelayToEnable(50); + stepper->setDelayToDisable(50); + + stepper->setSpeedInHz(5000); + stepper->setAcceleration(100000); + + } +} + +uint16_t loopcnt = 0; + +void loop() { + loopcnt++; + Serial.print("Loop="); + Serial.println(loopcnt); + uint32_t delayForward = (rand() % 500) + 50; + uint32_t delayBackward = (rand() % 500) + 50; + stepper->runForward(); + delay(delayForward); + stepper->runBackward(); + delay(delayBackward); + if (loopcnt == 100) { +#ifdef SIMULATOR + stepper->moveTo(0, true); + noInterrupts(); + sleep_cpu(); +#endif + } +} diff --git a/extras/tests/simavr_based/test_issue250/.gitignore b/extras/tests/simavr_based/test_issue250/.gitignore new file mode 100644 index 00000000..8eba6c8d --- /dev/null +++ b/extras/tests/simavr_based/test_issue250/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue250/Makefile b/extras/tests/simavr_based/test_issue250/Makefile new file mode 100644 index 00000000..676c04e3 --- /dev/null +++ b/extras/tests/simavr_based/test_issue250/Makefile @@ -0,0 +1,132 @@ +# +# In order to execute the test for one directory use: +# +# make -C test_sd_01b_328p -f ../Makefile.test + +SRC=$(wildcard ../../../src/*) $(wildcard src/*) + +# platformio should contain only one env section. +# This section states the dut name +# atmega168 +# atmega168p +# atmega328 +# atmega328p +# atmega2560_timer1 +# atmega2560_timer3 +# atmega2560_timer4 +# atmega2560_timer5 +# + +DUT=$(shell gawk '/env:/{print(substr($$1,6,length($$1)-6))}' platformio.ini) + +TRACES=-at StepISR=trace@0x25/0x08 # PB3 +TRACES+=-at FillISR=trace@0x25/0x10 # PB4 + +# +ifeq ($(DUT),atmega2560_timer1) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 ATMega2560 +TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 ATMega2560 +TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer3) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x02e/0x08 #OC3A PE3 5 ATMega2560 +TRACES+=-at StepB=trace@0x02e/0x10 #OC3B PE4 2 ATMega2560 +TRACES+=-at StepC=trace@0x02e/0x20 #OC3C PE5 3 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer4) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x102/0x08 #OC4A PH3 6 ATMega2560 +TRACES+=-at StepB=trace@0x102/0x10 #OC4B PH4 7 ATMega2560 +TRACES+=-at StepC=trace@0x102/0x20 #OC4C PH5 8 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer5) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x10b/0x08 #OC5A PL3 46 ATMega2560 +TRACES+=-at StepB=trace@0x10b/0x10 #OC5B PL4 45 ATMega2560 +TRACES+=-at StepC=trace@0x10b/0x20 #OC5C PL5 44 ATMega2560 + +else ifeq ($(DUT),atmega168) +DEVICE=atmega168 +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168 +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168 + +else ifeq ($(DUT),atmega168p) +DEVICE=atmega168p +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168p +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168p + +else ifeq ($(DUT),atmega328) +DEVICE=atmega328 +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328 +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328 + +else ifeq ($(DUT),atmega328p) +DEVICE=atmega328p +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328p +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328p + +else ifeq ($(DUT),atmega32u4) +DEVICE=atmega32u4 +TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 +TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 +#TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 + +endif + +ifeq ($(DEVICE),atmega2560) +TRACES+=-at DirA=trace@0x2b/0x01 # Pin 21 PD0 +TRACES+=-at DirB=trace@0x2b/0x02 # Pin 20 PD1 +TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 +TRACES+=-at EnableA=trace@0x2b/0x04 # Pin 19 PD2 +TRACES+=-at EnableB=trace@0x2b/0x08 # Pin 18 PD3 +TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 + +else ifeq ($(DEVICE),$(filter $(DEVICE),atmega168 atmega168p atmega328 atmega328p)) +TRACES+=-at DirA=trace@0x2b/0x20 # Pin 5 PD5 +TRACES+=-at DirB=trace@0x2b/0x80 # Pin 7 PD7 +TRACES+=-at EnableA=trace@0x2b/0x40 # Pin 6 PD6 +TRACES+=-at EnableB=trace@0x25/0x01 # Pin 8 PB0 + +else ifeq ($(DUT),atmega32u4) +TRACES+=-at DirA=trace@0x25/0x10 # Pin 26 PB4 +TRACES+=-at DirB=trace@0x25/0x08 # Pin 14 PB3 +#TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 +TRACES+=-at EnableA=trace@0x25/0x04 # Pin 16 PB2 +TRACES+=-at EnableB=trace@0x25/0x02 # Pin 15 PB1 +#TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 + +endif + +FIRMWARE=".pio/build/$(DUT)/firmware.elf" + +DIR=$(shell env pwd) + +ifndef SILENCE + SILENCE=0 +endif + +test: .tested + +.tested: result.txt expect.txt ../judge.awk + echo DUT=$(DUT) + rm -f .tested + gawk -f ../judge.awk -v DIR=$(DIR) result.txt expect.txt + test -f .tested + +result.txt: x.vcd + gawk -v SILENCE=$(SILENCE) -f ../eval.awk x.vcd >/dev/null + cat expect.txt + +x.vcd: $(SRC) ../run_avr platformio.ini src/Issue250.ino + ~/.platformio/penv/bin/pio run -e $(DUT) || ~/.local/bin/pio run -e $(DUT) || env pio run -e $(DUT) + ../run_avr $(FIRMWARE) -m $(DEVICE) -o x.vcd $(TRACES) + +src/Issue250.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue250/Issue250.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue250/expect.txt b/extras/tests/simavr_based/test_issue250/expect.txt new file mode 100644 index 00000000..e09dbda1 --- /dev/null +++ b/extras/tests/simavr_based/test_issue250/expect.txt @@ -0,0 +1,18 @@ + DirA: 100*L->H, 100*H->L + DirB: 0*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 0*L->H, 0*H->L + StepA: 239368*L->H, 239368*H->L, Max High=12us Total High=1270614us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in DirA max=550071 us, total=26774896 us + +Time in EnableA max=4198 us, total=4198 us + +Time in FillISR max=1727 us, total=4397545 us + +Time in StepA max=12 us, total=1270614 us + +Time in StepISR max=8 us, total=1110595 us + diff --git a/extras/tests/simavr_based/test_issue250/platformio.ini b/extras/tests/simavr_based/test_issue250/platformio.ini new file mode 100644 index 00000000..9eae37d7 --- /dev/null +++ b/extras/tests/simavr_based/test_issue250/platformio.ini @@ -0,0 +1,31 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[platformio] + +# There should be only one env section for the DUT under test. +# One of +# atmega168p +# atmega328p +# atmega2560_timer1 +# atmega2560_timer3 +# atmega2560_timer4 +# atmega2560_timer5 +# +[common] +# This is the line input to StepperDemo: +build_flags = -D SIMULATOR -D SIMAVR_TIME_MEASUREMENT + +[env:atmega328p] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. From 5f4bfc754fae30ff5640dbcf754c5a65163b138c Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 26 Apr 2024 22:42:21 +0200 Subject: [PATCH 55/85] further work on test for issue #250 --- examples/Issue250/Issue250.ino | 4 +++- extras/tests/simavr_based/judge_pos0.awk | 22 +++++++++++++++++++ .../tests/simavr_based/test_issue250/Makefile | 4 ++-- .../simavr_based/test_issue250/expect.txt | 18 --------------- 4 files changed, 27 insertions(+), 21 deletions(-) create mode 100644 extras/tests/simavr_based/judge_pos0.awk delete mode 100644 extras/tests/simavr_based/test_issue250/expect.txt diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino index 5022489d..25d84a0d 100644 --- a/examples/Issue250/Issue250.ino +++ b/examples/Issue250/Issue250.ino @@ -47,7 +47,9 @@ void loop() { stepper->runForward(); delay(delayForward); stepper->runBackward(); - delay(delayBackward); + uint32_t start_ms = millis(); + while (millis() < delayBackward + start_ms) { + } if (loopcnt == 100) { #ifdef SIMULATOR stepper->moveTo(0, true); diff --git a/extras/tests/simavr_based/judge_pos0.awk b/extras/tests/simavr_based/judge_pos0.awk new file mode 100644 index 00000000..db0488ca --- /dev/null +++ b/extras/tests/simavr_based/judge_pos0.awk @@ -0,0 +1,22 @@ +BEGIN { + ok = 0 +} + +FNR == NR { + # result.txt + lines[FNR] = $0 +} + +/^Position\[A\]/ { + print +} +$0 == "Position[A]=0" { + ok = 1 +} + +END { + if (ok == 1) { + print("PASS") + print("PASS") > ".tested" + } +} diff --git a/extras/tests/simavr_based/test_issue250/Makefile b/extras/tests/simavr_based/test_issue250/Makefile index 676c04e3..03c51dc3 100644 --- a/extras/tests/simavr_based/test_issue250/Makefile +++ b/extras/tests/simavr_based/test_issue250/Makefile @@ -109,10 +109,10 @@ endif test: .tested -.tested: result.txt expect.txt ../judge.awk +.tested: result.txt ../judge_pos0.awk echo DUT=$(DUT) rm -f .tested - gawk -f ../judge.awk -v DIR=$(DIR) result.txt expect.txt + gawk -f ../judge_pos0.awk -v DIR=$(DIR) result.txt test -f .tested result.txt: x.vcd diff --git a/extras/tests/simavr_based/test_issue250/expect.txt b/extras/tests/simavr_based/test_issue250/expect.txt deleted file mode 100644 index e09dbda1..00000000 --- a/extras/tests/simavr_based/test_issue250/expect.txt +++ /dev/null @@ -1,18 +0,0 @@ - DirA: 100*L->H, 100*H->L - DirB: 0*L->H, 0*H->L - EnableA: 1*L->H, 1*H->L - EnableB: 0*L->H, 0*H->L - StepA: 239368*L->H, 239368*H->L, Max High=12us Total High=1270614us - StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us -Position[A]=0 - -Time in DirA max=550071 us, total=26774896 us - -Time in EnableA max=4198 us, total=4198 us - -Time in FillISR max=1727 us, total=4397545 us - -Time in StepA max=12 us, total=1270614 us - -Time in StepISR max=8 us, total=1110595 us - From 22fd7ab452342ef4e75ff592fbcb459715dc20e3 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 26 Apr 2024 22:48:10 +0200 Subject: [PATCH 56/85] first test failure for issue #250 --- examples/Issue250/Issue250.ino | 12 ++++++++---- extras/tests/simavr_based/test_issue250/Makefile | 1 - 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino index 25d84a0d..3dfd0af4 100644 --- a/examples/Issue250/Issue250.ino +++ b/examples/Issue250/Issue250.ino @@ -1,6 +1,7 @@ #include "FastAccelStepper.h" #ifdef SIMULATOR +#include #include #endif @@ -30,7 +31,7 @@ void setup() { stepper->setDelayToEnable(50); stepper->setDelayToDisable(50); - stepper->setSpeedInHz(5000); + stepper->setSpeedInHz(40000); stepper->setAcceleration(100000); } @@ -42,15 +43,18 @@ void loop() { loopcnt++; Serial.print("Loop="); Serial.println(loopcnt); - uint32_t delayForward = (rand() % 500) + 50; - uint32_t delayBackward = (rand() % 500) + 50; + uint32_t delayForward = (rand() % 50) + 50; + uint32_t delayBackward = (rand() % 50) + 50; stepper->runForward(); delay(delayForward); stepper->runBackward(); uint32_t start_ms = millis(); while (millis() < delayBackward + start_ms) { + noInterrupts(); + _delay_us(25); + interrupts(); } - if (loopcnt == 100) { + if (loopcnt == 1000) { #ifdef SIMULATOR stepper->moveTo(0, true); noInterrupts(); diff --git a/extras/tests/simavr_based/test_issue250/Makefile b/extras/tests/simavr_based/test_issue250/Makefile index 03c51dc3..ba9091d3 100644 --- a/extras/tests/simavr_based/test_issue250/Makefile +++ b/extras/tests/simavr_based/test_issue250/Makefile @@ -117,7 +117,6 @@ test: .tested result.txt: x.vcd gawk -v SILENCE=$(SILENCE) -f ../eval.awk x.vcd >/dev/null - cat expect.txt x.vcd: $(SRC) ../run_avr platformio.ini src/Issue250.ino ~/.platformio/penv/bin/pio run -e $(DUT) || ~/.local/bin/pio run -e $(DUT) || env pio run -e $(DUT) From 2d3cecc0be66cb4224a2690419908b84ccdc5e2b Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 26 Apr 2024 22:54:09 +0200 Subject: [PATCH 57/85] test for issue #250 fails faster --- examples/Issue250/Issue250.ino | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino index 3dfd0af4..4ac51591 100644 --- a/examples/Issue250/Issue250.ino +++ b/examples/Issue250/Issue250.ino @@ -43,18 +43,24 @@ void loop() { loopcnt++; Serial.print("Loop="); Serial.println(loopcnt); + uint32_t start_ms; uint32_t delayForward = (rand() % 50) + 50; uint32_t delayBackward = (rand() % 50) + 50; stepper->runForward(); - delay(delayForward); + start_ms = millis(); + while (millis() < delayForward + start_ms) { + noInterrupts(); + _delay_us(25); + interrupts(); + } stepper->runBackward(); - uint32_t start_ms = millis(); + start_ms = millis(); while (millis() < delayBackward + start_ms) { noInterrupts(); _delay_us(25); interrupts(); } - if (loopcnt == 1000) { + if (loopcnt == 200) { #ifdef SIMULATOR stepper->moveTo(0, true); noInterrupts(); From 23dbe2227f2f6faa902946668fdeb459c5103903 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 26 Apr 2024 23:02:37 +0200 Subject: [PATCH 58/85] fix build errors for issue #250 on non-avr devices --- examples/Issue250/Issue250.ino | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino index 4ac51591..6c85c104 100644 --- a/examples/Issue250/Issue250.ino +++ b/examples/Issue250/Issue250.ino @@ -50,14 +50,18 @@ void loop() { start_ms = millis(); while (millis() < delayForward + start_ms) { noInterrupts(); +#ifdef SIMULATOR _delay_us(25); +#endif interrupts(); } stepper->runBackward(); start_ms = millis(); while (millis() < delayBackward + start_ms) { noInterrupts(); +#ifdef SIMULATOR _delay_us(25); +#endif interrupts(); } if (loopcnt == 200) { From 8e2eefd3eb40876274e93f3f44139d8944e748c8 Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 26 Apr 2024 23:13:21 +0200 Subject: [PATCH 59/85] update to checkout v4 in github actions --- .github/workflows/arduino_lint.yml | 2 +- .github/workflows/build_examples_atmega2560.yml | 2 +- .github/workflows/build_examples_atmega32u4.yml | 2 +- .github/workflows/build_examples_atmelsam.yml | 2 +- .github/workflows/build_examples_esp32_V3_5_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_1_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_2_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_3_0.yml | 2 +- .github/workflows/build_examples_esp32_V4_4_0.yml | 2 +- .github/workflows/build_examples_esp32_V5_0_0.yml | 2 +- .github/workflows/build_examples_esp32_V5_1_1.yml | 2 +- .github/workflows/build_examples_esp32_V5_2_0.yml | 2 +- .github/workflows/build_examples_esp32_V5_3_0.yml | 2 +- .github/workflows/build_examples_esp32_V6_0_1.yml | 2 +- .github/workflows/build_examples_esp32_V6_1_0.yml | 2 +- .github/workflows/build_examples_esp32_V6_2_0.yml | 2 +- .github/workflows/build_examples_esp32_V6_3_2.yml | 2 +- .github/workflows/build_examples_esp32_V6_4_0.yml | 2 +- .github/workflows/build_examples_esp32_V6_5_0.yml | 2 +- .github/workflows/build_examples_esp32c3_V6_5_0.yml | 2 +- .github/workflows/build_examples_esp32s2_V6_5_0.yml | 2 +- .github/workflows/build_examples_esp32s3_V6_5_0.yml | 2 +- .github/workflows/build_examples_nanoatmega168.yml | 2 +- .github/workflows/build_examples_nanoatmega328.yml | 2 +- .github/workflows/test.yml | 2 +- .github/workflows/test_avr.yml | 2 +- 26 files changed, 26 insertions(+), 26 deletions(-) diff --git a/.github/workflows/arduino_lint.yml b/.github/workflows/arduino_lint.yml index 0e285da2..061e2240 100644 --- a/.github/workflows/arduino_lint.yml +++ b/.github/workflows/arduino_lint.yml @@ -3,7 +3,7 @@ jobs: lint: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: arduino/arduino-lint-action@v1 with: library-manager: update diff --git a/.github/workflows/build_examples_atmega2560.yml b/.github/workflows/build_examples_atmega2560.yml index cce882dc..a83027ae 100644 --- a/.github/workflows/build_examples_atmega2560.yml +++ b/.github/workflows/build_examples_atmega2560.yml @@ -13,7 +13,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_atmega32u4.yml b/.github/workflows/build_examples_atmega32u4.yml index cb6c3ba3..db651ec5 100644 --- a/.github/workflows/build_examples_atmega32u4.yml +++ b/.github/workflows/build_examples_atmega32u4.yml @@ -13,7 +13,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_atmelsam.yml b/.github/workflows/build_examples_atmelsam.yml index 335815e8..551a1f3b 100644 --- a/.github/workflows/build_examples_atmelsam.yml +++ b/.github/workflows/build_examples_atmelsam.yml @@ -13,7 +13,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V3_5_0.yml b/.github/workflows/build_examples_esp32_V3_5_0.yml index 6e71ae10..05f1d4b6 100644 --- a/.github/workflows/build_examples_esp32_V3_5_0.yml +++ b/.github/workflows/build_examples_esp32_V3_5_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V4_1_0.yml b/.github/workflows/build_examples_esp32_V4_1_0.yml index 74d4c51c..53f08270 100644 --- a/.github/workflows/build_examples_esp32_V4_1_0.yml +++ b/.github/workflows/build_examples_esp32_V4_1_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V4_2_0.yml b/.github/workflows/build_examples_esp32_V4_2_0.yml index 1614e6c6..866ce092 100644 --- a/.github/workflows/build_examples_esp32_V4_2_0.yml +++ b/.github/workflows/build_examples_esp32_V4_2_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V4_3_0.yml b/.github/workflows/build_examples_esp32_V4_3_0.yml index 3c222756..e782d357 100644 --- a/.github/workflows/build_examples_esp32_V4_3_0.yml +++ b/.github/workflows/build_examples_esp32_V4_3_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V4_4_0.yml b/.github/workflows/build_examples_esp32_V4_4_0.yml index 453c13a9..f718a697 100644 --- a/.github/workflows/build_examples_esp32_V4_4_0.yml +++ b/.github/workflows/build_examples_esp32_V4_4_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V5_0_0.yml b/.github/workflows/build_examples_esp32_V5_0_0.yml index 31cdd78b..2e5fea1c 100644 --- a/.github/workflows/build_examples_esp32_V5_0_0.yml +++ b/.github/workflows/build_examples_esp32_V5_0_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V5_1_1.yml b/.github/workflows/build_examples_esp32_V5_1_1.yml index 8743c2a8..43ea4ac1 100644 --- a/.github/workflows/build_examples_esp32_V5_1_1.yml +++ b/.github/workflows/build_examples_esp32_V5_1_1.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V5_2_0.yml b/.github/workflows/build_examples_esp32_V5_2_0.yml index 4302b4e3..037d2c7a 100644 --- a/.github/workflows/build_examples_esp32_V5_2_0.yml +++ b/.github/workflows/build_examples_esp32_V5_2_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V5_3_0.yml b/.github/workflows/build_examples_esp32_V5_3_0.yml index 4ba455e7..daabf976 100644 --- a/.github/workflows/build_examples_esp32_V5_3_0.yml +++ b/.github/workflows/build_examples_esp32_V5_3_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V6_0_1.yml b/.github/workflows/build_examples_esp32_V6_0_1.yml index 48dbd58a..b598c90b 100644 --- a/.github/workflows/build_examples_esp32_V6_0_1.yml +++ b/.github/workflows/build_examples_esp32_V6_0_1.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V6_1_0.yml b/.github/workflows/build_examples_esp32_V6_1_0.yml index 0e2572ce..8472315d 100644 --- a/.github/workflows/build_examples_esp32_V6_1_0.yml +++ b/.github/workflows/build_examples_esp32_V6_1_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V6_2_0.yml b/.github/workflows/build_examples_esp32_V6_2_0.yml index d43cc073..ad19942a 100644 --- a/.github/workflows/build_examples_esp32_V6_2_0.yml +++ b/.github/workflows/build_examples_esp32_V6_2_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V6_3_2.yml b/.github/workflows/build_examples_esp32_V6_3_2.yml index 6ff2d197..37eaeba6 100644 --- a/.github/workflows/build_examples_esp32_V6_3_2.yml +++ b/.github/workflows/build_examples_esp32_V6_3_2.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V6_4_0.yml b/.github/workflows/build_examples_esp32_V6_4_0.yml index ce885a3b..729ef1e8 100644 --- a/.github/workflows/build_examples_esp32_V6_4_0.yml +++ b/.github/workflows/build_examples_esp32_V6_4_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32_V6_5_0.yml b/.github/workflows/build_examples_esp32_V6_5_0.yml index 3c5fd12c..05c86d0d 100644 --- a/.github/workflows/build_examples_esp32_V6_5_0.yml +++ b/.github/workflows/build_examples_esp32_V6_5_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32c3_V6_5_0.yml b/.github/workflows/build_examples_esp32c3_V6_5_0.yml index 1daf5228..e1e87e8e 100644 --- a/.github/workflows/build_examples_esp32c3_V6_5_0.yml +++ b/.github/workflows/build_examples_esp32c3_V6_5_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32s2_V6_5_0.yml b/.github/workflows/build_examples_esp32s2_V6_5_0.yml index d22a68d7..009bf496 100644 --- a/.github/workflows/build_examples_esp32s2_V6_5_0.yml +++ b/.github/workflows/build_examples_esp32s2_V6_5_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_esp32s3_V6_5_0.yml b/.github/workflows/build_examples_esp32s3_V6_5_0.yml index 33e700d2..266caad6 100644 --- a/.github/workflows/build_examples_esp32s3_V6_5_0.yml +++ b/.github/workflows/build_examples_esp32s3_V6_5_0.yml @@ -14,7 +14,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_nanoatmega168.yml b/.github/workflows/build_examples_nanoatmega168.yml index 95bc4469..60b1d51d 100644 --- a/.github/workflows/build_examples_nanoatmega168.yml +++ b/.github/workflows/build_examples_nanoatmega168.yml @@ -13,7 +13,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/build_examples_nanoatmega328.yml b/.github/workflows/build_examples_nanoatmega328.yml index 35340d14..d82e1cd4 100644 --- a/.github/workflows/build_examples_nanoatmega328.yml +++ b/.github/workflows/build_examples_nanoatmega328.yml @@ -13,7 +13,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 2b93a42e..61dd9302 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -12,6 +12,6 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: make run: make -C extras/tests/pc_based diff --git a/.github/workflows/test_avr.yml b/.github/workflows/test_avr.yml index 9b0b404e..e28f1fb7 100644 --- a/.github/workflows/test_avr.yml +++ b/.github/workflows/test_avr.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-20.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: get platformio run: curl -fsSL https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py -o get-platformio.py - name: install platformio From a0da791ed77dc524502c23d340cdbc8e15a772dd Mon Sep 17 00:00:00 2001 From: Jochen Kiemes Date: Fri, 26 Apr 2024 23:18:27 +0200 Subject: [PATCH 60/85] add empty expect.txt for test_issue250 to satisfy simavr toplevel Makefile --- extras/tests/simavr_based/test_issue250/expect.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 extras/tests/simavr_based/test_issue250/expect.txt diff --git a/extras/tests/simavr_based/test_issue250/expect.txt b/extras/tests/simavr_based/test_issue250/expect.txt new file mode 100644 index 00000000..e69de29b From 19e6dfea8432caaf5311f163cce324a4ab4d3224 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 09:33:00 +0000 Subject: [PATCH 61/85] add make rules for simavr-tests to automatically create makefiles/links --- extras/tests/simavr_based/Makefile | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/extras/tests/simavr_based/Makefile b/extras/tests/simavr_based/Makefile index 44454b2e..807c0931 100644 --- a/extras/tests/simavr_based/Makefile +++ b/extras/tests/simavr_based/Makefile @@ -19,15 +19,21 @@ pmf: externalCall: make -C test_externalCall -%/.tested: $(SRC) run_avr %/expect.txt %/platformio.ini +%/.tested: $(SRC) run_avr %/expect.txt %/platformio.ini .makefiles .links make SILENCE=$(SILENCE) -C $(dir $@) +.makefiles: makefiles + makefiles: $(addsuffix /Makefile,$(TESTS)) + touch .makefiles %/Makefile: cd $(dir $@); ln -s ../Makefile.test Makefile +.links: links + links: $(SD_SRC_DIRS) + touch .links %/src/.dir: mkdir -p $(dir $@) @@ -82,5 +88,5 @@ clean: rm -fR */.pio */.tested */x.vcd */result.txt find . -type l -delete find . -type d -empty -delete - make links makefiles + rm .links .makefiles From df95a603e11a917892ab03417559482345845d84 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 10:01:56 +0000 Subject: [PATCH 62/85] make all tests pass and disable test for issue #250 --- .../.gitignore | 0 .../Makefile | 0 .../expect.txt | 0 .../platformio.ini | 0 .../test_sd_04_timing_2560/expect.txt | 16 +++++----- .../test_sd_04_timing_328p/expect.txt | 14 ++++----- .../test_sd_04_timing_328p_37k/expect.txt | 6 ++-- src/AVRStepperPins.h | 31 ++++++++++++++++++- 8 files changed, 48 insertions(+), 19 deletions(-) rename extras/tests/simavr_based/{test_issue250 => off_test_issue250}/.gitignore (100%) rename extras/tests/simavr_based/{test_issue250 => off_test_issue250}/Makefile (100%) rename extras/tests/simavr_based/{test_issue250 => off_test_issue250}/expect.txt (100%) rename extras/tests/simavr_based/{test_issue250 => off_test_issue250}/platformio.ini (100%) diff --git a/extras/tests/simavr_based/test_issue250/.gitignore b/extras/tests/simavr_based/off_test_issue250/.gitignore similarity index 100% rename from extras/tests/simavr_based/test_issue250/.gitignore rename to extras/tests/simavr_based/off_test_issue250/.gitignore diff --git a/extras/tests/simavr_based/test_issue250/Makefile b/extras/tests/simavr_based/off_test_issue250/Makefile similarity index 100% rename from extras/tests/simavr_based/test_issue250/Makefile rename to extras/tests/simavr_based/off_test_issue250/Makefile diff --git a/extras/tests/simavr_based/test_issue250/expect.txt b/extras/tests/simavr_based/off_test_issue250/expect.txt similarity index 100% rename from extras/tests/simavr_based/test_issue250/expect.txt rename to extras/tests/simavr_based/off_test_issue250/expect.txt diff --git a/extras/tests/simavr_based/test_issue250/platformio.ini b/extras/tests/simavr_based/off_test_issue250/platformio.ini similarity index 100% rename from extras/tests/simavr_based/test_issue250/platformio.ini rename to extras/tests/simavr_based/off_test_issue250/platformio.ini diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 81103a66..6827ceff 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=27us Total High=390370us - StepB: 64000*L->H, 64000*H->L, Max High=33us Total High=393554us - StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=618397us + StepA: 64000*L->H, 64000*H->L, Max High=31us Total High=588641us + StepB: 64000*L->H, 64000*H->L, Max High=32us Total High=414769us + StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=431717us Position[A]=64000 Position[B]=64000 @@ -17,13 +17,13 @@ Time in EnableA max=233598 us, total=233598 us Time in EnableB max=246080 us, total=246080 us -Time in EnableC max=254236 us, total=254236 us +Time in EnableC max=254235 us, total=254235 us -Time in FillISR max=3102 us, total=2092792 us +Time in FillISR max=2804 us, total=2134900 us -Time in StepA max=27 us, total=390370 us +Time in StepA max=31 us, total=588641 us -Time in StepB max=33 us, total=393554 us +Time in StepB max=32 us, total=414769 us -Time in StepC max=40 us, total=618397 us +Time in StepC max=40 us, total=431717 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index e8fd56dd..98543e7c 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,21 +2,21 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5545us - StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=5535us + StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5340us + StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5816us Position[A]=1000 Position[B]=1000 Time in EnableA max=225398 us, total=225398 us -Time in EnableB max=238117 us, total=238117 us +Time in EnableB max=238116 us, total=238116 us -Time in FillISR max=2644 us, total=47661 us +Time in FillISR max=2642 us, total=47691 us -Time in StepA max=14 us, total=5545 us +Time in StepA max=14 us, total=5340 us -Time in StepB max=15 us, total=5535 us +Time in StepB max=16 us, total=5816 us -Time in StepISR max=7 us, total=9224 us +Time in StepISR max=7 us, total=9363 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index ddcc9626..09f3b8de 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=5270us + StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=5028us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2015 us, total=27557 us +Time in FillISR max=2014 us, total=27533 us -Time in StepA max=10 us, total=5270 us +Time in StepA max=10 us, total=5028 us Time in StepISR max=7 us, total=4581 us diff --git a/src/AVRStepperPins.h b/src/AVRStepperPins.h index 264e0f76..f4e3714d 100644 --- a/src/AVRStepperPins.h +++ b/src/AVRStepperPins.h @@ -31,7 +31,7 @@ #endif #define stepPinStepper1A 11 /* OC1A */ #define stepPinStepper1B 12 /* OC1B */ -#define stepPinStepper1C 13 /* OC1B */ +#define stepPinStepper1C 13 /* OC1C */ #define stepPinStepper3A 5 /* OC3A */ #define stepPinStepper3B 2 /* OC3B */ #define stepPinStepper3C 3 /* OC3C */ @@ -41,6 +41,35 @@ #define stepPinStepper5A 46 /* OC5A */ #define stepPinStepper5B 45 /* OC5B */ #define stepPinStepper5C 44 /* OC5C */ + +#define OC1A_PORT PORTB +#define OC1A_PORT_BIT 0x20 +#define OC1B_PORT PORTB +#define OC1B_PORT_BIT 0x40 +#define OC1C_PORT PORTB +#define OC1C_PORT_BIT 0x80 + +#define OC3A_PORT PORTC +#define OC3A_PORT_BIT 0x08 +#define OC3B_PORT PORTC +#define OC3B_PORT_BIT 0x10 +#define OC3C_PORT PORTC +#define OC3C_PORT_BIT 0x20 + +#define OC4A_PORT PORTH +#define OC4A_PORT_BIT 0x08 +#define OC4B_PORT PORTH +#define OC4B_PORT_BIT 0x10 +#define OC4C_PORT PORTH +#define OC4C_PORT_BIT 0x20 + +#define OC5A_PORT PORTL +#define OC5A_PORT_BIT 0x08 +#define OC5B_PORT PORTL +#define OC5B_PORT_BIT 0x10 +#define OC5C_PORT PORTL +#define OC5C_PORT_BIT 0x20 + #elif defined(__AVR_ATmega32U4__) #define FAS_TIMER_MODULE 1 #define stepPinStepper1A 9 /* OC1A */ From 405fd1a7f96f51598a3eae67c472c34ffcdc3e4f Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 10:17:57 +0000 Subject: [PATCH 63/85] avr: complete output compare port bit definitions --- extras/tests/simavr_based/.gitignore | 2 ++ src/AVRStepperPins.h | 10 ++++++++++ 2 files changed, 12 insertions(+) create mode 100644 extras/tests/simavr_based/.gitignore diff --git a/extras/tests/simavr_based/.gitignore b/extras/tests/simavr_based/.gitignore new file mode 100644 index 00000000..8089a5ac --- /dev/null +++ b/extras/tests/simavr_based/.gitignore @@ -0,0 +1,2 @@ +.links +.makefiles diff --git a/src/AVRStepperPins.h b/src/AVRStepperPins.h index f4e3714d..2fed0011 100644 --- a/src/AVRStepperPins.h +++ b/src/AVRStepperPins.h @@ -25,6 +25,10 @@ #define FAS_TIMER_MODULE 1 #define stepPinStepper1A 9 /* OC1A */ #define stepPinStepper1B 10 /* OC1B */ +#define OC1A_PORT PORTB +#define OC1A_PORT_BIT 0x20 +#define OC1B_PORT PORTB +#define OC1B_PORT_BIT 0x40 #elif defined(__AVR_ATmega2560__) #ifndef FAS_TIMER_MODULE #define FAS_TIMER_MODULE 4 @@ -75,6 +79,12 @@ #define stepPinStepper1A 9 /* OC1A */ #define stepPinStepper1B 10 /* OC1B */ #define stepPinStepper1C 11 /* OC1C */ +#define OC1A_PORT PORTB +#define OC1A_PORT_BIT 0x20 +#define OC1B_PORT PORTB +#define OC1B_PORT_BIT 0x40 +#define OC1C_PORT PORTB +#define OC1C_PORT_BIT 0x80 #endif #if (FAS_TIMER_MODULE == 1) From 5af160b056f55426794dcab4fbdafcda93e88414 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 10:36:30 +0000 Subject: [PATCH 64/85] add OCxy_PIN definitions --- src/AVRStepperPins.h | 51 +++++++++++++++++++++++++++++--------------- 1 file changed, 34 insertions(+), 17 deletions(-) diff --git a/src/AVRStepperPins.h b/src/AVRStepperPins.h index 2fed0011..4fec3e35 100644 --- a/src/AVRStepperPins.h +++ b/src/AVRStepperPins.h @@ -26,9 +26,11 @@ #define stepPinStepper1A 9 /* OC1A */ #define stepPinStepper1B 10 /* OC1B */ #define OC1A_PORT PORTB -#define OC1A_PORT_BIT 0x20 +#define OC1A_PIN PINB +#define OC1A_BIT 0x20 #define OC1B_PORT PORTB -#define OC1B_PORT_BIT 0x40 +#define OC1B_PIN PINB +#define OC1B_BIT 0x40 #elif defined(__AVR_ATmega2560__) #ifndef FAS_TIMER_MODULE #define FAS_TIMER_MODULE 4 @@ -47,32 +49,44 @@ #define stepPinStepper5C 44 /* OC5C */ #define OC1A_PORT PORTB -#define OC1A_PORT_BIT 0x20 +#define OC1A_PIN PINB +#define OC1A_BIT 0x20 #define OC1B_PORT PORTB -#define OC1B_PORT_BIT 0x40 +#define OC1B_PIN PINB +#define OC1B_BIT 0x40 #define OC1C_PORT PORTB -#define OC1C_PORT_BIT 0x80 +#define OC1C_PIN PINB +#define OC1C_BIT 0x80 #define OC3A_PORT PORTC -#define OC3A_PORT_BIT 0x08 +#define OC3A_PIN PINC +#define OC3A_BIT 0x08 #define OC3B_PORT PORTC -#define OC3B_PORT_BIT 0x10 +#define OC3B_PIN PINC +#define OC3B_BIT 0x10 #define OC3C_PORT PORTC -#define OC3C_PORT_BIT 0x20 +#define OC3C_PIN PINC +#define OC3C_BIT 0x20 #define OC4A_PORT PORTH -#define OC4A_PORT_BIT 0x08 +#define OC4A_PIN PINH +#define OC4A_BIT 0x08 #define OC4B_PORT PORTH -#define OC4B_PORT_BIT 0x10 +#define OC4B_PIN PINH +#define OC4B_BIT 0x10 #define OC4C_PORT PORTH -#define OC4C_PORT_BIT 0x20 +#define OC4C_PIN PINH +#define OC4C_BIT 0x20 #define OC5A_PORT PORTL -#define OC5A_PORT_BIT 0x08 +#define OC5A_PIN PINL +#define OC5A_BIT 0x08 #define OC5B_PORT PORTL -#define OC5B_PORT_BIT 0x10 +#define OC5B_PIN PINL +#define OC5B_BIT 0x10 #define OC5C_PORT PORTL -#define OC5C_PORT_BIT 0x20 +#define OC5C_PIN PINL +#define OC5C_BIT 0x20 #elif defined(__AVR_ATmega32U4__) #define FAS_TIMER_MODULE 1 @@ -80,11 +94,14 @@ #define stepPinStepper1B 10 /* OC1B */ #define stepPinStepper1C 11 /* OC1C */ #define OC1A_PORT PORTB -#define OC1A_PORT_BIT 0x20 +#define OC1A_PIN PINB +#define OC1A_BIT 0x20 #define OC1B_PORT PORTB -#define OC1B_PORT_BIT 0x40 +#define OC1B_PIN PINB +#define OC1B_BIT 0x40 #define OC1C_PORT PORTB -#define OC1C_PORT_BIT 0x80 +#define OC1C_PIN PINB +#define OC1C_BIT 0x80 #endif #if (FAS_TIMER_MODULE == 1) From 6d7ae500c833a48cd42caf69fae3de64e985a1ab Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 11:02:27 +0000 Subject: [PATCH 65/85] avr: use port pin toggle feature instead read-modify-write by xor port bit --- .../test_sd_04_timing_2560/expect.txt | 16 ++++++++-------- .../test_sd_04_timing_328p/expect.txt | 12 ++++++------ src/StepperISR.h | 12 ++++++++++-- src/StepperISR_avr.cpp | 11 +++++------ src/fas_common.h | 2 +- 5 files changed, 30 insertions(+), 23 deletions(-) diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 6827ceff..dc7ac190 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=31us Total High=588641us - StepB: 64000*L->H, 64000*H->L, Max High=32us Total High=414769us - StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=431717us + StepA: 64000*L->H, 64000*H->L, Max High=29us Total High=380000us + StepB: 64000*L->H, 64000*H->L, Max High=34us Total High=507758us + StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=629247us Position[A]=64000 Position[B]=64000 @@ -17,13 +17,13 @@ Time in EnableA max=233598 us, total=233598 us Time in EnableB max=246080 us, total=246080 us -Time in EnableC max=254235 us, total=254235 us +Time in EnableC max=254236 us, total=254236 us -Time in FillISR max=2804 us, total=2134900 us +Time in FillISR max=2849 us, total=2127254 us -Time in StepA max=31 us, total=588641 us +Time in StepA max=29 us, total=380000 us -Time in StepB max=32 us, total=414769 us +Time in StepB max=34 us, total=507758 us -Time in StepC max=40 us, total=431717 us +Time in StepC max=40 us, total=629247 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 98543e7c..4bcba4ba 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,8 +2,8 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5340us - StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5816us + StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5335us + StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5809us Position[A]=1000 Position[B]=1000 @@ -12,11 +12,11 @@ Time in EnableA max=225398 us, total=225398 us Time in EnableB max=238116 us, total=238116 us -Time in FillISR max=2642 us, total=47691 us +Time in FillISR max=2642 us, total=47687 us -Time in StepA max=14 us, total=5340 us +Time in StepA max=14 us, total=5335 us -Time in StepB max=16 us, total=5816 us +Time in StepB max=16 us, total=5809 us -Time in StepISR max=7 us, total=9363 us +Time in StepISR max=7 us, total=9300 us diff --git a/src/StepperISR.h b/src/StepperISR.h index db431fd1..bae6e405 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -80,11 +80,13 @@ class StepperQueue { bool bufferContainsSteps[2]; #endif #if defined(SUPPORT_DIR_PIN_MASK) - // avr uses uint8_t and sam needs uint32_t - // so make the SUPPORT_DIR_PIN_MASK dual use volatile SUPPORT_DIR_PIN_MASK* _dirPinPort; SUPPORT_DIR_PIN_MASK _dirPinMask; #endif +#if defined(SUPPORT_DIR_TOGGLE_PIN_MASK) + volatile SUPPORT_DIR_TOGGLE_PIN_MASK* _dirTogglePinPort; + SUPPORT_DIR_TOGGLE_PIN_MASK _dirTogglePinMask; +#endif #if defined(SUPPORT_AVR) volatile bool _prepareForStop; volatile bool _isRunning; @@ -180,6 +182,12 @@ class StepperQueue { _dirPinPort = portOutputRegister(digitalPinToPort(dir_pin)); _dirPinMask = digitalPinToBitMask(dir_pin); } +#endif +#if defined(SUPPORT_DIR_TOGGLE_PIN_MASK) + if ((dir_pin != PIN_UNDEFINED) && ((dir_pin & PIN_EXTERNAL_FLAG) == 0)) { + _dirTogglePinPort = portInputRegister(digitalPinToPort(dir_pin)); + _dirTogglePinMask = digitalPinToBitMask(dir_pin); + } #endif } void adjustSpeedToStepperCount(uint8_t steppers); diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index 7764c2da..2d7ec739 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -26,7 +26,11 @@ #define Stepper_IsDisconnected(T, X) \ ((TCCR##T##A & (_BV(COM##T##X##0) | _BV(COM##T##X##1))) == 0) #define Stepper_IsOneIfOutput(T, X) ((TCCR##T##A & _BV(COM##T##X##0)) != 0) -#define Stepper_ToggleDirection(CHANNEL) *fas_queue_##CHANNEL._dirPinPort ^= fas_queue_##CHANNEL._dirPinMask +#define Stepper_ToggleDirection(CHANNEL) *fas_queue_##CHANNEL._dirTogglePinPort = fas_queue_##CHANNEL._dirTogglePinMask +#define PREPARE_DIRECTION_PIN(CHANNEL) \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ + } #ifdef SIMAVR_TIME_MEASUREMENT #define prepareISRtimeMeasurement() DDRB |= 0x18 @@ -238,11 +242,6 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) rp = fas_queue_##CHANNEL.read_idx; \ e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; -#define PREPARE_DIRECTION_PIN(CHANNEL) \ - if (e->toggle_dir) { \ - *fas_queue_##CHANNEL._dirPinPort ^= fas_queue_##CHANNEL._dirPinMask; \ - } - #define AVR_START_QUEUE(T, CHANNEL) \ _isRunning = true; \ _prepareForStop = false; \ diff --git a/src/fas_common.h b/src/fas_common.h index 05b0064d..0d209599 100644 --- a/src/fas_common.h +++ b/src/fas_common.h @@ -375,7 +375,7 @@ struct queue_end_s { #define noop_or_wait -#define SUPPORT_DIR_PIN_MASK uint8_t +#define SUPPORT_DIR_TOGGLE_PIN_MASK uint8_t #define SUPPORT_QUEUE_ENTRY_END_POS_U16 From 8c97518708c08afb58aaa97b4580f8fcc78f1aa3 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 11:33:51 +0000 Subject: [PATCH 66/85] move toggle dir before setting compare action for stepper pin --- src/StepperISR_avr.cpp | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index 2d7ec739..dcf74af4 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -124,8 +124,6 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { #endif } - - // The interrupt is called on compare event, which eventually // generates a L->H transition. In any case, the current command's // wait time has still to be executed for the next command, if any. @@ -167,10 +165,10 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { /* if this new command requires a step, then this step would be lost \ */ \ fas_queue_##CHANNEL._prepareForStop = false; \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ + } \ if (e->steps > 0) { \ - if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ - } \ /* That's the problem, so generate a step */ \ Stepper_One(T, CHANNEL); \ ForceCompare(T, CHANNEL); \ @@ -194,13 +192,14 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { if (rp != fas_queue_##CHANNEL.next_write_idx) { \ /* command in queue */ \ e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ - if (e->steps != 0) { \ - Stepper_One(T, CHANNEL); \ - } \ if (e->toggle_dir) { \ Stepper_ToggleDirection(CHANNEL); \ } \ + if (e->steps != 0) { \ + Stepper_One(T, CHANNEL); \ + } \ } else { \ + /* queue is empty, so wait this command to complete, then disconnect */ \ fas_queue_##CHANNEL._prepareForStop = true; \ } \ exitStepperISR(); \ @@ -243,8 +242,6 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; #define AVR_START_QUEUE(T, CHANNEL) \ - _isRunning = true; \ - _prepareForStop = false; \ /* ensure no compare event */ \ SetTimerCompareRelative(T, CHANNEL, 32768); \ /* set output one, if steps to be generated */ \ @@ -260,23 +257,35 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) /* start */ \ SetTimerCompareRelative(T, CHANNEL, 10); +#define DISABLE(T, CHANNEL) \ + DisableCompareInterrupt(T, CHANNEL) + void StepperQueue::startQueue() { uint8_t rp; struct queue_entry* e; switch (channel) { case channelA: + DISABLE(FAS_TIMER_MODULE, A); + _isRunning = true; + _prepareForStop = false; GET_ENTRY_PTR(FAS_TIMER_MODULE, A) PREPARE_DIRECTION_PIN(A) AVR_START_QUEUE(FAS_TIMER_MODULE, A) break; case channelB: + DISABLE(FAS_TIMER_MODULE, B); + _isRunning = true; + _prepareForStop = false; GET_ENTRY_PTR(FAS_TIMER_MODULE, B) PREPARE_DIRECTION_PIN(B) AVR_START_QUEUE(FAS_TIMER_MODULE, B) break; #ifdef stepPinStepperC case channelC: + DISABLE(FAS_TIMER_MODULE, C); + _isRunning = true; + _prepareForStop = false; GET_ENTRY_PTR(FAS_TIMER_MODULE, C) PREPARE_DIRECTION_PIN(C) AVR_START_QUEUE(FAS_TIMER_MODULE, C) From a0de189440c6cb98600fa132d3a43e9709e55f57 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 13:10:18 +0000 Subject: [PATCH 67/85] remove unnecessary definitions in AVRStepperPins.h --- src/AVRStepperPins.h | 55 -------------------------------------------- 1 file changed, 55 deletions(-) diff --git a/src/AVRStepperPins.h b/src/AVRStepperPins.h index 4fec3e35..55f2a008 100644 --- a/src/AVRStepperPins.h +++ b/src/AVRStepperPins.h @@ -25,12 +25,6 @@ #define FAS_TIMER_MODULE 1 #define stepPinStepper1A 9 /* OC1A */ #define stepPinStepper1B 10 /* OC1B */ -#define OC1A_PORT PORTB -#define OC1A_PIN PINB -#define OC1A_BIT 0x20 -#define OC1B_PORT PORTB -#define OC1B_PIN PINB -#define OC1B_BIT 0x40 #elif defined(__AVR_ATmega2560__) #ifndef FAS_TIMER_MODULE #define FAS_TIMER_MODULE 4 @@ -48,60 +42,11 @@ #define stepPinStepper5B 45 /* OC5B */ #define stepPinStepper5C 44 /* OC5C */ -#define OC1A_PORT PORTB -#define OC1A_PIN PINB -#define OC1A_BIT 0x20 -#define OC1B_PORT PORTB -#define OC1B_PIN PINB -#define OC1B_BIT 0x40 -#define OC1C_PORT PORTB -#define OC1C_PIN PINB -#define OC1C_BIT 0x80 - -#define OC3A_PORT PORTC -#define OC3A_PIN PINC -#define OC3A_BIT 0x08 -#define OC3B_PORT PORTC -#define OC3B_PIN PINC -#define OC3B_BIT 0x10 -#define OC3C_PORT PORTC -#define OC3C_PIN PINC -#define OC3C_BIT 0x20 - -#define OC4A_PORT PORTH -#define OC4A_PIN PINH -#define OC4A_BIT 0x08 -#define OC4B_PORT PORTH -#define OC4B_PIN PINH -#define OC4B_BIT 0x10 -#define OC4C_PORT PORTH -#define OC4C_PIN PINH -#define OC4C_BIT 0x20 - -#define OC5A_PORT PORTL -#define OC5A_PIN PINL -#define OC5A_BIT 0x08 -#define OC5B_PORT PORTL -#define OC5B_PIN PINL -#define OC5B_BIT 0x10 -#define OC5C_PORT PORTL -#define OC5C_PIN PINL -#define OC5C_BIT 0x20 - #elif defined(__AVR_ATmega32U4__) #define FAS_TIMER_MODULE 1 #define stepPinStepper1A 9 /* OC1A */ #define stepPinStepper1B 10 /* OC1B */ #define stepPinStepper1C 11 /* OC1C */ -#define OC1A_PORT PORTB -#define OC1A_PIN PINB -#define OC1A_BIT 0x20 -#define OC1B_PORT PORTB -#define OC1B_PIN PINB -#define OC1B_BIT 0x40 -#define OC1C_PORT PORTB -#define OC1C_PIN PINB -#define OC1C_BIT 0x80 #endif #if (FAS_TIMER_MODULE == 1) From f4cd8e6475e09509c5a4bc769d1a2dd9c7a23a16 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 13:12:01 +0000 Subject: [PATCH 68/85] work on test for issue #250 => steps lost if interrupts are blocked for >30us --- examples/Issue250/Issue250.ino | 55 ++++++++++++++++++------- extras/tests/simavr_based/Makefile.test | 2 +- 2 files changed, 41 insertions(+), 16 deletions(-) diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino index 6c85c104..1271c76a 100644 --- a/examples/Issue250/Issue250.ino +++ b/examples/Issue250/Issue250.ino @@ -39,34 +39,59 @@ void setup() { uint16_t loopcnt = 0; +#ifndef BLOCK_INTERRUPT_US +#define BLOCK_INTERRUPT_US 30 +#endif +#ifndef TOGGLE_DIRECTION +#define TOGGLE_DIRECTION false +#endif +#ifndef USE_MOVETO +#define USE_MOVETO true +#endif + +uint32_t previous_runtime = 0; + void loop() { loopcnt++; Serial.print("Loop="); Serial.println(loopcnt); - uint32_t start_ms; - uint32_t delayForward = (rand() % 50) + 50; - uint32_t delayBackward = (rand() % 50) + 50; - stepper->runForward(); - start_ms = millis(); - while (millis() < delayForward + start_ms) { - noInterrupts(); -#ifdef SIMULATOR - _delay_us(25); -#endif - interrupts(); + uint32_t runtime = (rand() % 50) + 50; + if (((loopcnt & 1) == 1) || !TOGGLE_DIRECTION) { + if (USE_MOVETO) { + stepper->moveTo(10000); + } + else { + stepper->runForward(); + } } - stepper->runBackward(); - start_ms = millis(); - while (millis() < delayBackward + start_ms) { + else { + if (USE_MOVETO) { + stepper->moveTo(-10000); + } + else { + stepper->runBackward(); + } + } + uint32_t start_ms = millis(); + while (millis() < 2*previous_runtime + runtime + start_ms) { noInterrupts(); #ifdef SIMULATOR - _delay_us(25); + _delay_us(BLOCK_INTERRUPT_US); #endif interrupts(); } + previous_runtime = runtime; + if (loopcnt == 200) { + stepper->stopMove(); + while(stepper->isRunning()) {} #ifdef SIMULATOR + Serial.print("Reached Position="); + Serial.println(stepper->getCurrentPosition()); stepper->moveTo(0, true); + Serial.print("Position="); + Serial.println(stepper->getCurrentPosition()); + delay(100); noInterrupts(); sleep_cpu(); #endif diff --git a/extras/tests/simavr_based/Makefile.test b/extras/tests/simavr_based/Makefile.test index 6b24eeae..f621550b 100644 --- a/extras/tests/simavr_based/Makefile.test +++ b/extras/tests/simavr_based/Makefile.test @@ -87,7 +87,7 @@ else ifeq ($(DEVICE),$(filter $(DEVICE),atmega168 atmega168p atmega328 atmega328 TRACES+=-at DirA=trace@0x2b/0x20 # Pin 5 PD5 TRACES+=-at DirB=trace@0x2b/0x80 # Pin 7 PD7 TRACES+=-at EnableA=trace@0x2b/0x40 # Pin 6 PD6 -TRACES+=-at EnableB=trace@0x25/0x01 # Pin 8 PB0 +TRACES+=-at EnableB=trace@0x25/0x01 # Pin 0 PB0 else ifeq ($(DUT),atmega32u4) TRACES+=-at DirA=trace@0x25/0x10 # Pin 26 PB4 From 4ed8197cbff757c1dec8a27d9d209859ccf866fa Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 13:47:42 +0000 Subject: [PATCH 69/85] finalize fix/test for issue #250 --- CHANGELOG.md | 5 ++++- README.md | 2 +- examples/Issue250/Issue250.ino | 5 +++-- .../.gitignore | 0 .../{off_test_issue250 => test_issue250}/Makefile | 0 .../expect.txt | 0 .../platformio.ini | 0 .../simavr_based/test_sd_04_timing_2560/expect.txt | 14 +++++++------- .../simavr_based/test_sd_04_timing_328p/expect.txt | 14 +++++++------- .../test_sd_04_timing_328p_37k/expect.txt | 8 ++++---- 10 files changed, 26 insertions(+), 22 deletions(-) rename extras/tests/simavr_based/{off_test_issue250 => test_issue250}/.gitignore (100%) rename extras/tests/simavr_based/{off_test_issue250 => test_issue250}/Makefile (100%) rename extras/tests/simavr_based/{off_test_issue250 => test_issue250}/expect.txt (100%) rename extras/tests/simavr_based/{off_test_issue250 => test_issue250}/platformio.ini (100%) diff --git a/CHANGELOG.md b/CHANGELOG.md index d7554fb9..96a3e498 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,5 @@ TODO: -- Different behavior avr vs pc-based tests to be analyzed +- Different behavior avr vs pc-based testgis to be analyzed - #include-file structure needs to be cleaned up - setCurrentPosition() should adjust the maintained last position bits in queue. can be called a bug - rename PoorManFloat to e.g. Log2Representation @@ -8,6 +8,9 @@ TODO: pre-0.30.12: - esp32: fix deprecation warning for `rmt_memory_rw_rst()` +- simavr-tests: +- avr: Fix issue #250 +- avr: In course of issue #250, interrupt blocks of 20us from application are tested. 0.30.11: - esp32s3: add support for rmt from patch #225 diff --git a/README.md b/README.md index e0319d8b..2915abba 100644 --- a/README.md +++ b/README.md @@ -500,4 +500,4 @@ As mentioned by kthod861 in [Issue #110](https://github.com/gin66/FastAccelStepp - Thanks DaAwesomeP for the extension to ATmega 168/168P/328 (https://github.com/gin66/FastAccelStepper/pull/179) - Thanks turley for the patch for missing `_stepper_cnt` initialization (https://github.com/gin66/FastAccelStepper/pull/204) - Thanks GarmischWg for adding rmt-support to ESP32-S3 (https://github.com/gin66/FastAccelStepper/pull/225) - +- THanks SHWotever for avr patch to fix missing direction pin toggle (https://github.com/gin66/FastAccelStepper/pull/252) diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino index 1271c76a..536570d0 100644 --- a/examples/Issue250/Issue250.ino +++ b/examples/Issue250/Issue250.ino @@ -39,11 +39,12 @@ void setup() { uint16_t loopcnt = 0; +// fails with blocking 30us #ifndef BLOCK_INTERRUPT_US -#define BLOCK_INTERRUPT_US 30 +#define BLOCK_INTERRUPT_US 20 #endif #ifndef TOGGLE_DIRECTION -#define TOGGLE_DIRECTION false +#define TOGGLE_DIRECTION true #endif #ifndef USE_MOVETO #define USE_MOVETO true diff --git a/extras/tests/simavr_based/off_test_issue250/.gitignore b/extras/tests/simavr_based/test_issue250/.gitignore similarity index 100% rename from extras/tests/simavr_based/off_test_issue250/.gitignore rename to extras/tests/simavr_based/test_issue250/.gitignore diff --git a/extras/tests/simavr_based/off_test_issue250/Makefile b/extras/tests/simavr_based/test_issue250/Makefile similarity index 100% rename from extras/tests/simavr_based/off_test_issue250/Makefile rename to extras/tests/simavr_based/test_issue250/Makefile diff --git a/extras/tests/simavr_based/off_test_issue250/expect.txt b/extras/tests/simavr_based/test_issue250/expect.txt similarity index 100% rename from extras/tests/simavr_based/off_test_issue250/expect.txt rename to extras/tests/simavr_based/test_issue250/expect.txt diff --git a/extras/tests/simavr_based/off_test_issue250/platformio.ini b/extras/tests/simavr_based/test_issue250/platformio.ini similarity index 100% rename from extras/tests/simavr_based/off_test_issue250/platformio.ini rename to extras/tests/simavr_based/test_issue250/platformio.ini diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index dc7ac190..04aa0651 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=29us Total High=380000us - StepB: 64000*L->H, 64000*H->L, Max High=34us Total High=507758us - StepC: 64000*L->H, 64000*H->L, Max High=40us Total High=629247us + StepA: 64000*L->H, 64000*H->L, Max High=25us Total High=396526us + StepB: 64000*L->H, 64000*H->L, Max High=35us Total High=418067us + StepC: 64000*L->H, 64000*H->L, Max High=41us Total High=635308us Position[A]=64000 Position[B]=64000 @@ -19,11 +19,11 @@ Time in EnableB max=246080 us, total=246080 us Time in EnableC max=254236 us, total=254236 us -Time in FillISR max=2849 us, total=2127254 us +Time in FillISR max=2866 us, total=2158540 us -Time in StepA max=29 us, total=380000 us +Time in StepA max=25 us, total=396526 us -Time in StepB max=34 us, total=507758 us +Time in StepB max=35 us, total=418067 us -Time in StepC max=40 us, total=629247 us +Time in StepC max=41 us, total=635308 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 4bcba4ba..2a62855a 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,21 +2,21 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=5335us - StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=5809us + StepA: 1000*L->H, 1000*H->L, Max High=15us Total High=5588us + StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=5832us Position[A]=1000 Position[B]=1000 Time in EnableA max=225398 us, total=225398 us -Time in EnableB max=238116 us, total=238116 us +Time in EnableB max=238117 us, total=238117 us -Time in FillISR max=2642 us, total=47687 us +Time in FillISR max=2645 us, total=47801 us -Time in StepA max=14 us, total=5335 us +Time in StepA max=15 us, total=5588 us -Time in StepB max=16 us, total=5809 us +Time in StepB max=15 us, total=5832 us -Time in StepISR max=7 us, total=9300 us +Time in StepISR max=7 us, total=9366 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index 09f3b8de..6b154c7c 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=5028us + StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=5265us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2014 us, total=27533 us +Time in FillISR max=2015 us, total=27562 us -Time in StepA max=10 us, total=5028 us +Time in StepA max=10 us, total=5265 us -Time in StepISR max=7 us, total=4581 us +Time in StepISR max=7 us, total=4614 us From 310c6f69d2bb9fd4bfcb309235fe6349bab98e75 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 13:52:47 +0000 Subject: [PATCH 70/85] add build examples test for esp32 V6.6.0 --- README.md | 1 + examples/Issue250/Issue250.ino | 61 ++++++++++++++++------------------ extras/ci/platformio.ini | 8 +++++ src/StepperISR_avr.cpp | 14 ++++---- 4 files changed, 45 insertions(+), 39 deletions(-) diff --git a/README.md b/README.md index 2915abba..9035ef52 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,7 @@ No issue with platformio. Check the [related issue](https://github.com/arduino/l Build examples for different versions of espressif-arduino platform: ### esp32 +[![`V6_6_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_6_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_6_0.yml) [![`V6_5_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_5_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_5_0.yml) [![`V6_4_0`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_4_0.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_4_0.yml) [![`V6_3_2`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_3_2.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_V6_3_2.yml) diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino index 536570d0..b2bb298f 100644 --- a/examples/Issue250/Issue250.ino +++ b/examples/Issue250/Issue250.ino @@ -6,9 +6,9 @@ #endif // As in StepperDemo for Motor 1 on AVR -#define dirPinStepper 5 +#define dirPinStepper 5 #define enablePinStepper 6 -#define stepPinStepper 9 // OC1A in case of AVR +#define stepPinStepper 9 // OC1A in case of AVR // As in StepperDemo for Motor 1 on ESP32 //#define dirPinStepper 18 @@ -33,14 +33,13 @@ void setup() { stepper->setSpeedInHz(40000); stepper->setAcceleration(100000); - } } uint16_t loopcnt = 0; // fails with blocking 30us -#ifndef BLOCK_INTERRUPT_US +#ifndef BLOCK_INTERRUPT_US #define BLOCK_INTERRUPT_US 20 #endif #ifndef TOGGLE_DIRECTION @@ -58,43 +57,41 @@ void loop() { Serial.println(loopcnt); uint32_t runtime = (rand() % 50) + 50; if (((loopcnt & 1) == 1) || !TOGGLE_DIRECTION) { - if (USE_MOVETO) { - stepper->moveTo(10000); - } - else { - stepper->runForward(); - } - } - else { - if (USE_MOVETO) { - stepper->moveTo(-10000); - } - else { - stepper->runBackward(); - } + if (USE_MOVETO) { + stepper->moveTo(10000); + } else { + stepper->runForward(); + } + } else { + if (USE_MOVETO) { + stepper->moveTo(-10000); + } else { + stepper->runBackward(); + } } uint32_t start_ms = millis(); - while (millis() < 2*previous_runtime + runtime + start_ms) { - noInterrupts(); + while (millis() < 2 * previous_runtime + runtime + start_ms) { + noInterrupts(); #ifdef SIMULATOR - _delay_us(BLOCK_INTERRUPT_US); + _delay_us(BLOCK_INTERRUPT_US); #endif - interrupts(); + interrupts(); } previous_runtime = runtime; if (loopcnt == 200) { - stepper->stopMove(); - while(stepper->isRunning()) {} + stepper->stopMove(); + while (stepper->isRunning()) { + } #ifdef SIMULATOR - Serial.print("Reached Position="); - Serial.println(stepper->getCurrentPosition()); - stepper->moveTo(0, true); - Serial.print("Position="); - Serial.println(stepper->getCurrentPosition()); - delay(100); - noInterrupts(); - sleep_cpu(); + Serial.print("Reached Position="); + Serial.println(stepper->getCurrentPosition()); + stepper->moveTo(0, true); + Serial.print("Position="); + Serial.println(stepper->getCurrentPosition()); + delay(100); + noInterrupts(); + sleep_cpu(); #endif } } diff --git a/extras/ci/platformio.ini b/extras/ci/platformio.ini index 4148091a..0c59cb23 100644 --- a/extras/ci/platformio.ini +++ b/extras/ci/platformio.ini @@ -21,6 +21,14 @@ build_flags = -Wall board_build.f_cpu = 240000000L lib_extra_dirs = ../../.. +[env:esp32_V6_6_0] +platform = espressif32 @ 6.6.0 +board = esp32dev +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + [env:esp32_V6_5_0] platform = espressif32 @ 6.5.0 board = esp32dev diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index dcf74af4..aaa7318d 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -26,10 +26,11 @@ #define Stepper_IsDisconnected(T, X) \ ((TCCR##T##A & (_BV(COM##T##X##0) | _BV(COM##T##X##1))) == 0) #define Stepper_IsOneIfOutput(T, X) ((TCCR##T##A & _BV(COM##T##X##0)) != 0) -#define Stepper_ToggleDirection(CHANNEL) *fas_queue_##CHANNEL._dirTogglePinPort = fas_queue_##CHANNEL._dirTogglePinMask -#define PREPARE_DIRECTION_PIN(CHANNEL) \ - if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ +#define Stepper_ToggleDirection(CHANNEL) \ + *fas_queue_##CHANNEL._dirTogglePinPort = fas_queue_##CHANNEL._dirTogglePinMask +#define PREPARE_DIRECTION_PIN(CHANNEL) \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ } #ifdef SIMAVR_TIME_MEASUREMENT @@ -166,7 +167,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { */ \ fas_queue_##CHANNEL._prepareForStop = false; \ if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ + Stepper_ToggleDirection(CHANNEL); \ } \ if (e->steps > 0) { \ /* That's the problem, so generate a step */ \ @@ -257,8 +258,7 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) /* start */ \ SetTimerCompareRelative(T, CHANNEL, 10); -#define DISABLE(T, CHANNEL) \ - DisableCompareInterrupt(T, CHANNEL) +#define DISABLE(T, CHANNEL) DisableCompareInterrupt(T, CHANNEL) void StepperQueue::startQueue() { uint8_t rp; From b88c5e277eb7d5cf212bf10d004d2faf497a2b8d Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 13:54:46 +0000 Subject: [PATCH 71/85] bump version to 0.30.12 --- CHANGELOG.md | 2 +- library.properties | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 96a3e498..3ced2142 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.12: +0.30.12: - esp32: fix deprecation warning for `rmt_memory_rw_rst()` - simavr-tests: - avr: Fix issue #250 diff --git a/library.properties b/library.properties index ab02498f..6361e7db 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.11 +version=0.30.12 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes From f009898e60cb94ad2328f120f10952df9a104f2b Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 13:55:43 +0000 Subject: [PATCH 72/85] add missing esp32 v6.6.0 github action --- .../workflows/build_examples_esp32_V6_6_0.yml | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/build_examples_esp32_V6_6_0.yml diff --git a/.github/workflows/build_examples_esp32_V6_6_0.yml b/.github/workflows/build_examples_esp32_V6_6_0.yml new file mode 100644 index 00000000..05c86d0d --- /dev/null +++ b/.github/workflows/build_examples_esp32_V6_6_0.yml @@ -0,0 +1,21 @@ + +name: Build examples for esp32 @ V6_5_0 + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Make directories + run: bash extras/scripts/build-pio-dirs.sh + - name: Build on PlatformIO + run: bash extras/scripts/build-platformio.sh esp32_V6_5_0 From e41828436da7bb8e57efb5af64856be1ed42b491 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 13:57:53 +0000 Subject: [PATCH 73/85] fix build examples script for esp32 V6.6.0 --- .github/workflows/build_examples_esp32_V6_6_0.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build_examples_esp32_V6_6_0.yml b/.github/workflows/build_examples_esp32_V6_6_0.yml index 05c86d0d..8c066c59 100644 --- a/.github/workflows/build_examples_esp32_V6_6_0.yml +++ b/.github/workflows/build_examples_esp32_V6_6_0.yml @@ -1,5 +1,5 @@ -name: Build examples for esp32 @ V6_5_0 +name: Build examples for esp32 @ V6_6_0 on: push: @@ -18,4 +18,4 @@ jobs: - name: Make directories run: bash extras/scripts/build-pio-dirs.sh - name: Build on PlatformIO - run: bash extras/scripts/build-platformio.sh esp32_V6_5_0 + run: bash extras/scripts/build-platformio.sh esp32_V6_6_0 From c96166d99c0211e331b68affbbaa4efd92ed3597 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 14:07:18 +0000 Subject: [PATCH 74/85] update Changelog --- CHANGELOG.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3ced2142..d5af3994 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,7 +8,8 @@ TODO: 0.30.12: - esp32: fix deprecation warning for `rmt_memory_rw_rst()` -- simavr-tests: +- esp32: add build test for platform espressif v6.6.0 with arduino core (#251) +- simavr-tests: automatically create links/makefiles - avr: Fix issue #250 - avr: In course of issue #250, interrupt blocks of 20us from application are tested. From ea44b43b42b459e96b3702df0b27c0bb7762de4e Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 23:05:14 +0000 Subject: [PATCH 75/85] avr: simplify stepper ISR. still test failures --- examples/Issue250/Issue250.ino | 5 +- .../test_issue250_30us/.gitignore | 1 + .../simavr_based/test_issue250_30us/Makefile | 131 ++++++++++++++++++ .../test_issue250_30us/expect.txt | 0 .../test_issue250_30us/platformio.ini | 32 +++++ src/StepperISR.h | 2 +- src/StepperISR_avr.cpp | 97 +++++-------- 7 files changed, 200 insertions(+), 68 deletions(-) create mode 100644 extras/tests/simavr_based/test_issue250_30us/.gitignore create mode 100644 extras/tests/simavr_based/test_issue250_30us/Makefile create mode 100644 extras/tests/simavr_based/test_issue250_30us/expect.txt create mode 100644 extras/tests/simavr_based/test_issue250_30us/platformio.ini diff --git a/examples/Issue250/Issue250.ino b/examples/Issue250/Issue250.ino index b2bb298f..ef67c37f 100644 --- a/examples/Issue250/Issue250.ino +++ b/examples/Issue250/Issue250.ino @@ -48,6 +48,9 @@ uint16_t loopcnt = 0; #ifndef USE_MOVETO #define USE_MOVETO true #endif +#ifndef LOOPS +#define LOOPS 200 +#endif uint32_t previous_runtime = 0; @@ -79,7 +82,7 @@ void loop() { } previous_runtime = runtime; - if (loopcnt == 200) { + if (loopcnt == LOOPS) { stepper->stopMove(); while (stepper->isRunning()) { } diff --git a/extras/tests/simavr_based/test_issue250_30us/.gitignore b/extras/tests/simavr_based/test_issue250_30us/.gitignore new file mode 100644 index 00000000..8eba6c8d --- /dev/null +++ b/extras/tests/simavr_based/test_issue250_30us/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue250_30us/Makefile b/extras/tests/simavr_based/test_issue250_30us/Makefile new file mode 100644 index 00000000..ba9091d3 --- /dev/null +++ b/extras/tests/simavr_based/test_issue250_30us/Makefile @@ -0,0 +1,131 @@ +# +# In order to execute the test for one directory use: +# +# make -C test_sd_01b_328p -f ../Makefile.test + +SRC=$(wildcard ../../../src/*) $(wildcard src/*) + +# platformio should contain only one env section. +# This section states the dut name +# atmega168 +# atmega168p +# atmega328 +# atmega328p +# atmega2560_timer1 +# atmega2560_timer3 +# atmega2560_timer4 +# atmega2560_timer5 +# + +DUT=$(shell gawk '/env:/{print(substr($$1,6,length($$1)-6))}' platformio.ini) + +TRACES=-at StepISR=trace@0x25/0x08 # PB3 +TRACES+=-at FillISR=trace@0x25/0x10 # PB4 + +# +ifeq ($(DUT),atmega2560_timer1) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 ATMega2560 +TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 ATMega2560 +TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer3) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x02e/0x08 #OC3A PE3 5 ATMega2560 +TRACES+=-at StepB=trace@0x02e/0x10 #OC3B PE4 2 ATMega2560 +TRACES+=-at StepC=trace@0x02e/0x20 #OC3C PE5 3 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer4) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x102/0x08 #OC4A PH3 6 ATMega2560 +TRACES+=-at StepB=trace@0x102/0x10 #OC4B PH4 7 ATMega2560 +TRACES+=-at StepC=trace@0x102/0x20 #OC4C PH5 8 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer5) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x10b/0x08 #OC5A PL3 46 ATMega2560 +TRACES+=-at StepB=trace@0x10b/0x10 #OC5B PL4 45 ATMega2560 +TRACES+=-at StepC=trace@0x10b/0x20 #OC5C PL5 44 ATMega2560 + +else ifeq ($(DUT),atmega168) +DEVICE=atmega168 +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168 +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168 + +else ifeq ($(DUT),atmega168p) +DEVICE=atmega168p +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168p +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168p + +else ifeq ($(DUT),atmega328) +DEVICE=atmega328 +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328 +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328 + +else ifeq ($(DUT),atmega328p) +DEVICE=atmega328p +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328p +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328p + +else ifeq ($(DUT),atmega32u4) +DEVICE=atmega32u4 +TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 +TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 +#TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 + +endif + +ifeq ($(DEVICE),atmega2560) +TRACES+=-at DirA=trace@0x2b/0x01 # Pin 21 PD0 +TRACES+=-at DirB=trace@0x2b/0x02 # Pin 20 PD1 +TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 +TRACES+=-at EnableA=trace@0x2b/0x04 # Pin 19 PD2 +TRACES+=-at EnableB=trace@0x2b/0x08 # Pin 18 PD3 +TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 + +else ifeq ($(DEVICE),$(filter $(DEVICE),atmega168 atmega168p atmega328 atmega328p)) +TRACES+=-at DirA=trace@0x2b/0x20 # Pin 5 PD5 +TRACES+=-at DirB=trace@0x2b/0x80 # Pin 7 PD7 +TRACES+=-at EnableA=trace@0x2b/0x40 # Pin 6 PD6 +TRACES+=-at EnableB=trace@0x25/0x01 # Pin 8 PB0 + +else ifeq ($(DUT),atmega32u4) +TRACES+=-at DirA=trace@0x25/0x10 # Pin 26 PB4 +TRACES+=-at DirB=trace@0x25/0x08 # Pin 14 PB3 +#TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 +TRACES+=-at EnableA=trace@0x25/0x04 # Pin 16 PB2 +TRACES+=-at EnableB=trace@0x25/0x02 # Pin 15 PB1 +#TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 + +endif + +FIRMWARE=".pio/build/$(DUT)/firmware.elf" + +DIR=$(shell env pwd) + +ifndef SILENCE + SILENCE=0 +endif + +test: .tested + +.tested: result.txt ../judge_pos0.awk + echo DUT=$(DUT) + rm -f .tested + gawk -f ../judge_pos0.awk -v DIR=$(DIR) result.txt + test -f .tested + +result.txt: x.vcd + gawk -v SILENCE=$(SILENCE) -f ../eval.awk x.vcd >/dev/null + +x.vcd: $(SRC) ../run_avr platformio.ini src/Issue250.ino + ~/.platformio/penv/bin/pio run -e $(DUT) || ~/.local/bin/pio run -e $(DUT) || env pio run -e $(DUT) + ../run_avr $(FIRMWARE) -m $(DEVICE) -o x.vcd $(TRACES) + +src/Issue250.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue250/Issue250.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue250_30us/expect.txt b/extras/tests/simavr_based/test_issue250_30us/expect.txt new file mode 100644 index 00000000..e69de29b diff --git a/extras/tests/simavr_based/test_issue250_30us/platformio.ini b/extras/tests/simavr_based/test_issue250_30us/platformio.ini new file mode 100644 index 00000000..4136b312 --- /dev/null +++ b/extras/tests/simavr_based/test_issue250_30us/platformio.ini @@ -0,0 +1,32 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[platformio] + +# There should be only one env section for the DUT under test. +# One of +# atmega168p +# atmega328p +# atmega2560_timer1 +# atmega2560_timer3 +# atmega2560_timer4 +# atmega2560_timer5 +# +[common] +# This is the line input to StepperDemo: +#build_flags = -D SIMULATOR -D SIMAVR_TIME_MEASUREMENT -D BLOCK_INTERRUPT_US=30 +build_flags = -D SIMULATOR -D SIMAVR_TIME_MEASUREMENT -D BLOCK_INTERRUPT_US=30 -D LOOPS=100 -D TOGGLE_DIRECTION=false -D USE_MOVETO=false + +[env:atmega328p] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. diff --git a/src/StepperISR.h b/src/StepperISR.h index bae6e405..23fc9d6f 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -91,7 +91,7 @@ class StepperQueue { volatile bool _prepareForStop; volatile bool _isRunning; inline bool isRunning() { return _isRunning; } - inline bool isReadyForCommands() { return true; } + inline bool isReadyForCommands() { return !_prepareForStop; } enum channels channel; #endif #if defined(SUPPORT_SAM) diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index aaa7318d..5f5cd487 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -12,12 +12,10 @@ // Toggle is 0 1 // Disconnect is 0 0 // -#define Stepper_OneToZero(T, X) TCCR##T##A = TCCR##T##A & ~_BV(COM##T##X##0) #define Stepper_Zero(T, X) \ TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##1)) & ~_BV(COM##T##X##0) #define Stepper_Toggle(T, X) \ TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##0)) & ~_BV(COM##T##X##1) -#define Stepper_One(T, X) TCCR##T##A |= _BV(COM##T##X##1) | _BV(COM##T##X##0) #define Stepper_Disconnect(T, X) \ TCCR##T##A &= ~(_BV(COM##T##X##1) | _BV(COM##T##X##0)) #define Stepper_IsOne(T, X) \ @@ -71,6 +69,7 @@ #define EnableCompareInterrupt(T, X) TIMSK##T |= _BV(OCIE##T##X) #define ClearInterruptFlag(T, X) TIFR##T = _BV(OCF##T##X) #define SetTimerCompareRelative(T, X, D) OCR##T##X = TCNT##T + D +#define InterruptFlagIsSet(T, X) ((TIFR##T & _BV(OCF##T##X)) != 0) #define ConfigureTimer(T) \ { \ @@ -149,60 +148,36 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { return; \ } \ struct queue_entry* e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ - /* There is a risk, that this new compare time is delayed by one cycle */ \ - OCR##T##CHANNEL += e->ticks; \ - if (Stepper_IsOneIfOutput(T, CHANNEL)) { \ - /* Clear output bit by another compare event */ \ - Stepper_OneToZero(T, CHANNEL); \ - ForceCompare(T, CHANNEL); \ - if (e->steps-- > 1) { \ - /* perform another step with this queue entry */ \ - Stepper_One(T, CHANNEL); \ - exitStepperISR(); \ - return; \ - } \ - } else if (fas_queue_##CHANNEL._prepareForStop) { \ - /* new command received after running out of commands */ \ - /* if this new command requires a step, then this step would be lost \ - */ \ - fas_queue_##CHANNEL._prepareForStop = false; \ - if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ + /* There is a risk, that the new compare time is delayed by one cycle */ \ + uint16_t ticks = e->ticks; \ + /* Clear output bit by another compare event */ \ + ForceCompare(T, CHANNEL); \ + if (e->steps == 0) { \ + /* mo more steps with this queue entry */ \ + if (TEST_NOT_REPEATING_ENTRY) { \ + rp++; \ + fas_queue_##CHANNEL.read_idx = rp; \ + e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ } \ - if (e->steps > 0) { \ - /* That's the problem, so generate a step */ \ - Stepper_One(T, CHANNEL); \ - ForceCompare(T, CHANNEL); \ - /* Use a high time of 3us */ \ - delayMicroseconds(3); \ - /* Clear output bit by another toggle */ \ - Stepper_OneToZero(T, CHANNEL); \ - ForceCompare(T, CHANNEL); \ - if (e->steps-- > 1) { \ - /* perform another step with this queue entry */ \ - Stepper_One(T, CHANNEL); \ - exitStepperISR(); \ - return; \ + Stepper_Zero(T, CHANNEL); \ + if (rp != fas_queue_##CHANNEL.next_write_idx) { \ + /* command in queue */ \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ + } \ + if (e->steps != 0) { \ + Stepper_Toggle(T, CHANNEL); \ + e->steps--; \ } \ + } else { \ + /* queue empty: wait this command to complete, then disconnect */ \ + fas_queue_##CHANNEL._prepareForStop = true; \ } \ } \ - if (TEST_NOT_REPEATING_ENTRY) { \ - rp++; \ - } \ - fas_queue_##CHANNEL.read_idx = rp; \ - if (rp != fas_queue_##CHANNEL.next_write_idx) { \ - /* command in queue */ \ - e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ - if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ - } \ - if (e->steps != 0) { \ - Stepper_One(T, CHANNEL); \ - } \ - } else { \ - /* queue is empty, so wait this command to complete, then disconnect */ \ - fas_queue_##CHANNEL._prepareForStop = true; \ + else { \ + e->steps--; \ } \ + SetTimerCompareRelative(T, CHANNEL, ticks); \ exitStepperISR(); \ } @@ -246,10 +221,10 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) /* ensure no compare event */ \ SetTimerCompareRelative(T, CHANNEL, 32768); \ /* set output one, if steps to be generated */ \ - if (e->steps > 0) { \ - Stepper_One(T, CHANNEL); \ - } else { \ - Stepper_Zero(T, CHANNEL); \ + Stepper_Zero(T, CHANNEL); \ + if (e->steps != 0) { \ + Stepper_Toggle(T, CHANNEL); \ + e->steps--; \ } \ /* clear interrupt flag */ \ ClearInterruptFlag(T, CHANNEL); \ @@ -258,34 +233,24 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) /* start */ \ SetTimerCompareRelative(T, CHANNEL, 10); -#define DISABLE(T, CHANNEL) DisableCompareInterrupt(T, CHANNEL) - void StepperQueue::startQueue() { uint8_t rp; struct queue_entry* e; + _isRunning = true; switch (channel) { case channelA: - DISABLE(FAS_TIMER_MODULE, A); - _isRunning = true; - _prepareForStop = false; GET_ENTRY_PTR(FAS_TIMER_MODULE, A) PREPARE_DIRECTION_PIN(A) AVR_START_QUEUE(FAS_TIMER_MODULE, A) break; case channelB: - DISABLE(FAS_TIMER_MODULE, B); - _isRunning = true; - _prepareForStop = false; GET_ENTRY_PTR(FAS_TIMER_MODULE, B) PREPARE_DIRECTION_PIN(B) AVR_START_QUEUE(FAS_TIMER_MODULE, B) break; #ifdef stepPinStepperC case channelC: - DISABLE(FAS_TIMER_MODULE, C); - _isRunning = true; - _prepareForStop = false; GET_ENTRY_PTR(FAS_TIMER_MODULE, C) PREPARE_DIRECTION_PIN(C) AVR_START_QUEUE(FAS_TIMER_MODULE, C) From 071557b87a1aaae9e38b7798646c7e2ef879be02 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 23:14:15 +0000 Subject: [PATCH 76/85] simavr: make proper works now after make clean --- extras/tests/simavr_based/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extras/tests/simavr_based/Makefile b/extras/tests/simavr_based/Makefile index 807c0931..b8502ba0 100644 --- a/extras/tests/simavr_based/Makefile +++ b/extras/tests/simavr_based/Makefile @@ -88,5 +88,5 @@ clean: rm -fR */.pio */.tested */x.vcd */result.txt find . -type l -delete find . -type d -empty -delete - rm .links .makefiles + rm -f .links .makefiles From efe5589d7b5b6166414f51ec0cfb684435856285 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Sun, 28 Apr 2024 23:31:12 +0000 Subject: [PATCH 77/85] aver: SetTimerCompareRelative() in ISR is not correct --- src/StepperISR_avr.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index 5f5cd487..85173223 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -177,7 +177,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { else { \ e->steps--; \ } \ - SetTimerCompareRelative(T, CHANNEL, ticks); \ + OCR##T##CHANNEL += ticks; \ exitStepperISR(); \ } @@ -231,7 +231,9 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) /* enable compare interrupt */ \ EnableCompareInterrupt(T, CHANNEL); \ /* start */ \ - SetTimerCompareRelative(T, CHANNEL, 10); + noInterrupts(); \ + SetTimerCompareRelative(T, CHANNEL, 10); \ + interrupts(); void StepperQueue::startQueue() { uint8_t rp; From 1ef4030a45f34eb589ac7acc38c08b1de178b5ee Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Tue, 30 Apr 2024 22:48:26 +0000 Subject: [PATCH 78/85] revert ISR changes. rename prepareForStop to noMoreCommands --- src/StepperISR.cpp | 2 +- src/StepperISR.h | 4 +-- src/StepperISR_avr.cpp | 78 ++++++++++++++++++++++++++++-------------- 3 files changed, 55 insertions(+), 29 deletions(-) diff --git a/src/StepperISR.cpp b/src/StepperISR.cpp index de814e77..dddcde9d 100644 --- a/src/StepperISR.cpp +++ b/src/StepperISR.cpp @@ -326,7 +326,7 @@ void StepperQueue::_initVars() { dirHighCountsUp = true; #if defined(ARDUINO_ARCH_AVR) _isRunning = false; - _prepareForStop = false; + _noMoreCommands = false; #endif #if defined(SUPPORT_ESP32) _isRunning = false; diff --git a/src/StepperISR.h b/src/StepperISR.h index 23fc9d6f..aabc8ae6 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -88,10 +88,10 @@ class StepperQueue { SUPPORT_DIR_TOGGLE_PIN_MASK _dirTogglePinMask; #endif #if defined(SUPPORT_AVR) - volatile bool _prepareForStop; + volatile bool _noMoreCommands; volatile bool _isRunning; inline bool isRunning() { return _isRunning; } - inline bool isReadyForCommands() { return !_prepareForStop; } + inline bool isReadyForCommands() { return true; } enum channels channel; #endif #if defined(SUPPORT_SAM) diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index 85173223..fec16123 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -12,10 +12,12 @@ // Toggle is 0 1 // Disconnect is 0 0 // +#define Stepper_OneToZero(T, X) TCCR##T##A = TCCR##T##A & ~_BV(COM##T##X##0) #define Stepper_Zero(T, X) \ TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##1)) & ~_BV(COM##T##X##0) #define Stepper_Toggle(T, X) \ TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##0)) & ~_BV(COM##T##X##1) +#define Stepper_One(T, X) TCCR##T##A |= _BV(COM##T##X##1) | _BV(COM##T##X##0) #define Stepper_Disconnect(T, X) \ TCCR##T##A &= ~(_BV(COM##T##X##1) | _BV(COM##T##X##0)) #define Stepper_IsOne(T, X) \ @@ -143,41 +145,65 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { /* disable compare interrupt */ \ DisableCompareInterrupt(T, CHANNEL); \ fas_queue_##CHANNEL._isRunning = false; \ - fas_queue_##CHANNEL._prepareForStop = false; \ + fas_queue_##CHANNEL._noMoreCommands = false; \ exitStepperISR(); \ return; \ } \ struct queue_entry* e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ - /* There is a risk, that the new compare time is delayed by one cycle */ \ - uint16_t ticks = e->ticks; \ - /* Clear output bit by another compare event */ \ - ForceCompare(T, CHANNEL); \ - if (e->steps == 0) { \ - /* mo more steps with this queue entry */ \ - if (TEST_NOT_REPEATING_ENTRY) { \ - rp++; \ - fas_queue_##CHANNEL.read_idx = rp; \ - e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ + /* There is a risk, that this new compare time is delayed by one cycle */ \ + OCR##T##CHANNEL += e->ticks; \ + if (Stepper_IsOneIfOutput(T, CHANNEL)) { \ + /* Clear output bit by another compare event */ \ + Stepper_OneToZero(T, CHANNEL); \ + ForceCompare(T, CHANNEL); \ + if (e->steps-- > 1) { \ + /* perform another step with this queue entry */ \ + Stepper_One(T, CHANNEL); \ + exitStepperISR(); \ + return; \ } \ - Stepper_Zero(T, CHANNEL); \ - if (rp != fas_queue_##CHANNEL.next_write_idx) { \ - /* command in queue */ \ - if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ - } \ - if (e->steps != 0) { \ - Stepper_Toggle(T, CHANNEL); \ - e->steps--; \ + } else if (fas_queue_##CHANNEL._noMoreCommands) { \ + /* new command received after running out of commands */ \ + /* if this new command requires a step, then this step would be lost \ + */ \ + fas_queue_##CHANNEL._noMoreCommands = false; \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ + } \ + if (e->steps > 0) { \ + /* That's the problem, so generate a step */ \ + Stepper_One(T, CHANNEL); \ + ForceCompare(T, CHANNEL); \ + /* Use a high time of 3us */ \ + delayMicroseconds(3); \ + /* Clear output bit by another toggle */ \ + Stepper_OneToZero(T, CHANNEL); \ + ForceCompare(T, CHANNEL); \ + if (e->steps-- > 1) { \ + /* perform another step with this queue entry */ \ + Stepper_One(T, CHANNEL); \ + exitStepperISR(); \ + return; \ } \ - } else { \ - /* queue empty: wait this command to complete, then disconnect */ \ - fas_queue_##CHANNEL._prepareForStop = true; \ } \ } \ - else { \ - e->steps--; \ + if (TEST_NOT_REPEATING_ENTRY) { \ + rp++; \ + } \ + fas_queue_##CHANNEL.read_idx = rp; \ + if (rp != fas_queue_##CHANNEL.next_write_idx) { \ + /* command in queue */ \ + e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ + } \ + if (e->steps != 0) { \ + Stepper_One(T, CHANNEL); \ + } \ + } else { \ + /* queue is empty, so wait this command to complete, then disconnect */ \ + fas_queue_##CHANNEL._noMoreCommands = true; \ } \ - OCR##T##CHANNEL += ticks; \ exitStepperISR(); \ } From db22d0835b0cda9cdb6ca0f5e81c1439c43300bf Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Wed, 1 May 2024 00:50:34 +0000 Subject: [PATCH 79/85] avr: all enabled tests pass with modified ISR --- .../simavr_based/test_issue208/expect.txt | 14 +- .../test_issue250_30us/.gitignore | 1 - .../simavr_based/test_issue250_30us/Makefile | 131 ----------------- .../test_issue250_30us/expect.txt | 0 .../test_issue250_30us/platformio.ini | 32 ---- .../test_sd_04_timing_2560/expect.txt | 16 +- .../test_sd_04_timing_328p/expect.txt | 14 +- .../test_sd_04_timing_328p_37k/expect.txt | 8 +- src/StepperISR_avr.cpp | 138 ++++++++---------- 9 files changed, 90 insertions(+), 264 deletions(-) delete mode 100644 extras/tests/simavr_based/test_issue250_30us/.gitignore delete mode 100644 extras/tests/simavr_based/test_issue250_30us/Makefile delete mode 100644 extras/tests/simavr_based/test_issue250_30us/expect.txt delete mode 100644 extras/tests/simavr_based/test_issue250_30us/platformio.ini diff --git a/extras/tests/simavr_based/test_issue208/expect.txt b/extras/tests/simavr_based/test_issue208/expect.txt index d36fdf33..eac920c6 100644 --- a/extras/tests/simavr_based/test_issue208/expect.txt +++ b/extras/tests/simavr_based/test_issue208/expect.txt @@ -2,17 +2,17 @@ DirB: 1*L->H, 0*H->L EnableA: 0*L->H, 0*H->L EnableB: 0*L->H, 0*H->L - StepA: 235908*L->H, 235908*H->L, Max High=11us Total High=1237142us - StepB: 2*L->H, 2*H->L, Max High=13us Total High=17us -Position[A]=37462 + StepA: 235907*L->H, 235907*H->L, Max High=10us Total High=927140us + StepB: 2*L->H, 2*H->L, Max High=12us Total High=17us +Position[A]=37463 Position[B]=2 -Time in FillISR max=769 us, total=1237221 us +Time in FillISR max=768 us, total=1213765 us -Time in StepA max=11 us, total=1237142 us +Time in StepA max=10 us, total=927140 us -Time in StepB max=13 us, total=17 us +Time in StepB max=12 us, total=17 us -Time in StepISR max=7 us, total=1021992 us +Time in StepISR max=6 us, total=912200 us diff --git a/extras/tests/simavr_based/test_issue250_30us/.gitignore b/extras/tests/simavr_based/test_issue250_30us/.gitignore deleted file mode 100644 index 8eba6c8d..00000000 --- a/extras/tests/simavr_based/test_issue250_30us/.gitignore +++ /dev/null @@ -1 +0,0 @@ -src/ diff --git a/extras/tests/simavr_based/test_issue250_30us/Makefile b/extras/tests/simavr_based/test_issue250_30us/Makefile deleted file mode 100644 index ba9091d3..00000000 --- a/extras/tests/simavr_based/test_issue250_30us/Makefile +++ /dev/null @@ -1,131 +0,0 @@ -# -# In order to execute the test for one directory use: -# -# make -C test_sd_01b_328p -f ../Makefile.test - -SRC=$(wildcard ../../../src/*) $(wildcard src/*) - -# platformio should contain only one env section. -# This section states the dut name -# atmega168 -# atmega168p -# atmega328 -# atmega328p -# atmega2560_timer1 -# atmega2560_timer3 -# atmega2560_timer4 -# atmega2560_timer5 -# - -DUT=$(shell gawk '/env:/{print(substr($$1,6,length($$1)-6))}' platformio.ini) - -TRACES=-at StepISR=trace@0x25/0x08 # PB3 -TRACES+=-at FillISR=trace@0x25/0x10 # PB4 - -# -ifeq ($(DUT),atmega2560_timer1) -DEVICE=atmega2560 -TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 ATMega2560 -TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 ATMega2560 -TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 ATMega2560 -# -else ifeq ($(DUT),atmega2560_timer3) -DEVICE=atmega2560 -TRACES+=-at StepA=trace@0x02e/0x08 #OC3A PE3 5 ATMega2560 -TRACES+=-at StepB=trace@0x02e/0x10 #OC3B PE4 2 ATMega2560 -TRACES+=-at StepC=trace@0x02e/0x20 #OC3C PE5 3 ATMega2560 -# -else ifeq ($(DUT),atmega2560_timer4) -DEVICE=atmega2560 -TRACES+=-at StepA=trace@0x102/0x08 #OC4A PH3 6 ATMega2560 -TRACES+=-at StepB=trace@0x102/0x10 #OC4B PH4 7 ATMega2560 -TRACES+=-at StepC=trace@0x102/0x20 #OC4C PH5 8 ATMega2560 -# -else ifeq ($(DUT),atmega2560_timer5) -DEVICE=atmega2560 -TRACES+=-at StepA=trace@0x10b/0x08 #OC5A PL3 46 ATMega2560 -TRACES+=-at StepB=trace@0x10b/0x10 #OC5B PL4 45 ATMega2560 -TRACES+=-at StepC=trace@0x10b/0x20 #OC5C PL5 44 ATMega2560 - -else ifeq ($(DUT),atmega168) -DEVICE=atmega168 -TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168 -TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168 - -else ifeq ($(DUT),atmega168p) -DEVICE=atmega168p -TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168p -TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168p - -else ifeq ($(DUT),atmega328) -DEVICE=atmega328 -TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328 -TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328 - -else ifeq ($(DUT),atmega328p) -DEVICE=atmega328p -TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328p -TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328p - -else ifeq ($(DUT),atmega32u4) -DEVICE=atmega32u4 -TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 -TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 -#TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 - -endif - -ifeq ($(DEVICE),atmega2560) -TRACES+=-at DirA=trace@0x2b/0x01 # Pin 21 PD0 -TRACES+=-at DirB=trace@0x2b/0x02 # Pin 20 PD1 -TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 -TRACES+=-at EnableA=trace@0x2b/0x04 # Pin 19 PD2 -TRACES+=-at EnableB=trace@0x2b/0x08 # Pin 18 PD3 -TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 - -else ifeq ($(DEVICE),$(filter $(DEVICE),atmega168 atmega168p atmega328 atmega328p)) -TRACES+=-at DirA=trace@0x2b/0x20 # Pin 5 PD5 -TRACES+=-at DirB=trace@0x2b/0x80 # Pin 7 PD7 -TRACES+=-at EnableA=trace@0x2b/0x40 # Pin 6 PD6 -TRACES+=-at EnableB=trace@0x25/0x01 # Pin 8 PB0 - -else ifeq ($(DUT),atmega32u4) -TRACES+=-at DirA=trace@0x25/0x10 # Pin 26 PB4 -TRACES+=-at DirB=trace@0x25/0x08 # Pin 14 PB3 -#TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 -TRACES+=-at EnableA=trace@0x25/0x04 # Pin 16 PB2 -TRACES+=-at EnableB=trace@0x25/0x02 # Pin 15 PB1 -#TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 - -endif - -FIRMWARE=".pio/build/$(DUT)/firmware.elf" - -DIR=$(shell env pwd) - -ifndef SILENCE - SILENCE=0 -endif - -test: .tested - -.tested: result.txt ../judge_pos0.awk - echo DUT=$(DUT) - rm -f .tested - gawk -f ../judge_pos0.awk -v DIR=$(DIR) result.txt - test -f .tested - -result.txt: x.vcd - gawk -v SILENCE=$(SILENCE) -f ../eval.awk x.vcd >/dev/null - -x.vcd: $(SRC) ../run_avr platformio.ini src/Issue250.ino - ~/.platformio/penv/bin/pio run -e $(DUT) || ~/.local/bin/pio run -e $(DUT) || env pio run -e $(DUT) - ../run_avr $(FIRMWARE) -m $(DEVICE) -o x.vcd $(TRACES) - -src/Issue250.ino: - mkdir -p src - cd src; ln -s ../../../../../examples/Issue250/Issue250.ino . - -clean: - rm -fR .pio .tested x.vcd result.txt - diff --git a/extras/tests/simavr_based/test_issue250_30us/expect.txt b/extras/tests/simavr_based/test_issue250_30us/expect.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/extras/tests/simavr_based/test_issue250_30us/platformio.ini b/extras/tests/simavr_based/test_issue250_30us/platformio.ini deleted file mode 100644 index 4136b312..00000000 --- a/extras/tests/simavr_based/test_issue250_30us/platformio.ini +++ /dev/null @@ -1,32 +0,0 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[platformio] - -# There should be only one env section for the DUT under test. -# One of -# atmega168p -# atmega328p -# atmega2560_timer1 -# atmega2560_timer3 -# atmega2560_timer4 -# atmega2560_timer5 -# -[common] -# This is the line input to StepperDemo: -#build_flags = -D SIMULATOR -D SIMAVR_TIME_MEASUREMENT -D BLOCK_INTERRUPT_US=30 -build_flags = -D SIMULATOR -D SIMAVR_TIME_MEASUREMENT -D BLOCK_INTERRUPT_US=30 -D LOOPS=100 -D TOGGLE_DIRECTION=false -D USE_MOVETO=false - -[env:atmega328p] -platform = atmelavr -board = nanoatmega328 -framework = arduino -build_flags = -Werror -Wall ${common.build_flags} -lib_extra_dirs = ../../../../.. diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 04aa0651..559ecba9 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=25us Total High=396526us - StepB: 64000*L->H, 64000*H->L, Max High=35us Total High=418067us - StepC: 64000*L->H, 64000*H->L, Max High=41us Total High=635308us + StepA: 64000*L->H, 64000*H->L, Max High=26us Total High=479601us + StepB: 64000*L->H, 64000*H->L, Max High=30us Total High=368752us + StepC: 64000*L->H, 64000*H->L, Max High=36us Total High=379262us Position[A]=64000 Position[B]=64000 @@ -17,13 +17,13 @@ Time in EnableA max=233598 us, total=233598 us Time in EnableB max=246080 us, total=246080 us -Time in EnableC max=254236 us, total=254236 us +Time in EnableC max=254234 us, total=254234 us -Time in FillISR max=2866 us, total=2158540 us +Time in FillISR max=2805 us, total=2111191 us -Time in StepA max=25 us, total=396526 us +Time in StepA max=26 us, total=479601 us -Time in StepB max=35 us, total=418067 us +Time in StepB max=30 us, total=368752 us -Time in StepC max=41 us, total=635308 us +Time in StepC max=36 us, total=379262 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 2a62855a..573eddea 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,21 +2,21 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=15us Total High=5588us - StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=5832us + StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=4334us + StepB: 1000*L->H, 1000*H->L, Max High=14us Total High=4915us Position[A]=1000 Position[B]=1000 Time in EnableA max=225398 us, total=225398 us -Time in EnableB max=238117 us, total=238117 us +Time in EnableB max=238115 us, total=238115 us -Time in FillISR max=2645 us, total=47801 us +Time in FillISR max=2640 us, total=47588 us -Time in StepA max=15 us, total=5588 us +Time in StepA max=14 us, total=4334 us -Time in StepB max=15 us, total=5832 us +Time in StepB max=14 us, total=4915 us -Time in StepISR max=7 us, total=9366 us +Time in StepISR max=6 us, total=8679 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index 6b154c7c..e6e67ac0 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=5265us + StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=3969us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2015 us, total=27562 us +Time in FillISR max=2013 us, total=27494 us -Time in StepA max=10 us, total=5265 us +Time in StepA max=11 us, total=3969 us -Time in StepISR max=7 us, total=4614 us +Time in StepISR max=5 us, total=4031 us diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index fec16123..1292acd0 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -12,9 +12,10 @@ // Toggle is 0 1 // Disconnect is 0 0 // -#define Stepper_OneToZero(T, X) TCCR##T##A = TCCR##T##A & ~_BV(COM##T##X##0) #define Stepper_Zero(T, X) \ TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##1)) & ~_BV(COM##T##X##0) +// Force compare of Stepper_Toggle appears to be broken in simavr +// In other words, use of Stepper_Toggle yields errors in simavr, for which root cause is unclear #define Stepper_Toggle(T, X) \ TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##0)) & ~_BV(COM##T##X##1) #define Stepper_One(T, X) TCCR##T##A |= _BV(COM##T##X##1) | _BV(COM##T##X##0) @@ -133,78 +134,68 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { // Remark: Interrupt Flag is automatically cleared on ISR execution // // If reaching here without further commands, then the queue is done -#define AVR_STEPPER_ISR(T, CHANNEL) \ - ISR(TIMER##T##_COMP##CHANNEL##_vect) { \ - enterStepperISR(); \ - uint8_t rp = fas_queue_##CHANNEL.read_idx; \ - if (rp == fas_queue_##CHANNEL.next_write_idx) { \ - /* queue is empty => set to disconnect */ \ - Stepper_Disconnect(T, CHANNEL); \ - /* force compare to ensure disconnect */ \ - ForceCompare(T, CHANNEL); \ - /* disable compare interrupt */ \ - DisableCompareInterrupt(T, CHANNEL); \ - fas_queue_##CHANNEL._isRunning = false; \ - fas_queue_##CHANNEL._noMoreCommands = false; \ - exitStepperISR(); \ - return; \ - } \ - struct queue_entry* e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ - /* There is a risk, that this new compare time is delayed by one cycle */ \ - OCR##T##CHANNEL += e->ticks; \ - if (Stepper_IsOneIfOutput(T, CHANNEL)) { \ - /* Clear output bit by another compare event */ \ - Stepper_OneToZero(T, CHANNEL); \ - ForceCompare(T, CHANNEL); \ - if (e->steps-- > 1) { \ - /* perform another step with this queue entry */ \ - Stepper_One(T, CHANNEL); \ - exitStepperISR(); \ - return; \ - } \ - } else if (fas_queue_##CHANNEL._noMoreCommands) { \ - /* new command received after running out of commands */ \ - /* if this new command requires a step, then this step would be lost \ - */ \ - fas_queue_##CHANNEL._noMoreCommands = false; \ - if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ - } \ - if (e->steps > 0) { \ - /* That's the problem, so generate a step */ \ - Stepper_One(T, CHANNEL); \ - ForceCompare(T, CHANNEL); \ - /* Use a high time of 3us */ \ - delayMicroseconds(3); \ - /* Clear output bit by another toggle */ \ - Stepper_OneToZero(T, CHANNEL); \ - ForceCompare(T, CHANNEL); \ - if (e->steps-- > 1) { \ - /* perform another step with this queue entry */ \ - Stepper_One(T, CHANNEL); \ - exitStepperISR(); \ - return; \ - } \ - } \ - } \ - if (TEST_NOT_REPEATING_ENTRY) { \ - rp++; \ - } \ - fas_queue_##CHANNEL.read_idx = rp; \ - if (rp != fas_queue_##CHANNEL.next_write_idx) { \ - /* command in queue */ \ - e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ - if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ - } \ - if (e->steps != 0) { \ - Stepper_One(T, CHANNEL); \ - } \ - } else { \ - /* queue is empty, so wait this command to complete, then disconnect */ \ - fas_queue_##CHANNEL._noMoreCommands = true; \ - } \ - exitStepperISR(); \ +#define AVR_STEPPER_ISR(T, CHANNEL) \ + ISR(TIMER##T##_COMP##CHANNEL##_vect) { \ + enterStepperISR(); \ + uint8_t rp = fas_queue_##CHANNEL.read_idx; \ + uint8_t wp = fas_queue_##CHANNEL.next_write_idx; \ + if (rp == wp) { \ + /* queue is empty => set to disconnect */ \ + /* disable compare interrupt */ \ + DisableCompareInterrupt(T, CHANNEL); \ + Stepper_Disconnect(T, CHANNEL); \ + /* force compare to ensure disconnect */ \ + ForceCompare(T, CHANNEL); \ + fas_queue_##CHANNEL._isRunning = false; \ + fas_queue_##CHANNEL._noMoreCommands = false; \ + exitStepperISR(); \ + return; \ + } \ + struct queue_entry* e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ + /* There is a risk, that this new compare time is delayed by one cycle */ \ + uint16_t ticks = e->ticks; \ + /* Set output to zero, this works in any case. In case of pause: no-op */ \ + Stepper_Zero(T, CHANNEL); \ + ForceCompare(T, CHANNEL); \ + if (e->steps > 1) { \ + /* perform another step with this queue entry, toggle mode is active */ \ + e->steps--; \ + Stepper_Toggle(T, CHANNEL); \ + } else { \ + /* either pause command or no more steps */ \ + if (fas_queue_##CHANNEL._noMoreCommands) { \ + /* new command received after running out of commands */ \ + /* if this new command requires a step, then this step would be lost */ \ + fas_queue_##CHANNEL._noMoreCommands = false; \ + if (e->steps != 0) { \ + /* New command needs steps, so do it immediately */ \ + ticks = 10; \ + } \ + } \ + else if (TEST_NOT_REPEATING_ENTRY) { \ + rp++; \ + fas_queue_##CHANNEL.read_idx = rp; \ + if (rp == wp) { \ + /* queue is empty, wait this command to complete, then disconnect */ \ + Stepper_Zero(T, CHANNEL); \ + fas_queue_##CHANNEL._noMoreCommands = true; \ + OCR##T##CHANNEL += ticks; \ + exitStepperISR(); \ + return; \ + } \ + e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ + } \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ + } \ + if (e->steps != 0) { \ + Stepper_Toggle(T, CHANNEL); \ + } else { \ + Stepper_Zero(T, CHANNEL); \ + } \ + } \ + OCR##T##CHANNEL += ticks; \ + exitStepperISR(); \ } #define AVR_STEPPER_ISR_GEN(T, CHANNEL) AVR_STEPPER_ISR(T, CHANNEL) @@ -250,7 +241,6 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) Stepper_Zero(T, CHANNEL); \ if (e->steps != 0) { \ Stepper_Toggle(T, CHANNEL); \ - e->steps--; \ } \ /* clear interrupt flag */ \ ClearInterruptFlag(T, CHANNEL); \ From ff281d074023f26977fb029b9aac8d21ddb1b8fd Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Wed, 1 May 2024 00:55:18 +0000 Subject: [PATCH 80/85] avr: avoid use of Stepper_Toggle --- .../off_test_issue250_30us/.gitignore | 1 + .../off_test_issue250_30us/Makefile | 131 ++++++++++++++++++ .../off_test_issue250_30us/expect.txt | 0 .../off_test_issue250_30us/platformio.ini | 32 +++++ src/StepperISR_avr.cpp | 20 +-- 5 files changed, 174 insertions(+), 10 deletions(-) create mode 100644 extras/tests/simavr_based/off_test_issue250_30us/.gitignore create mode 100644 extras/tests/simavr_based/off_test_issue250_30us/Makefile create mode 100644 extras/tests/simavr_based/off_test_issue250_30us/expect.txt create mode 100644 extras/tests/simavr_based/off_test_issue250_30us/platformio.ini diff --git a/extras/tests/simavr_based/off_test_issue250_30us/.gitignore b/extras/tests/simavr_based/off_test_issue250_30us/.gitignore new file mode 100644 index 00000000..8eba6c8d --- /dev/null +++ b/extras/tests/simavr_based/off_test_issue250_30us/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/off_test_issue250_30us/Makefile b/extras/tests/simavr_based/off_test_issue250_30us/Makefile new file mode 100644 index 00000000..ba9091d3 --- /dev/null +++ b/extras/tests/simavr_based/off_test_issue250_30us/Makefile @@ -0,0 +1,131 @@ +# +# In order to execute the test for one directory use: +# +# make -C test_sd_01b_328p -f ../Makefile.test + +SRC=$(wildcard ../../../src/*) $(wildcard src/*) + +# platformio should contain only one env section. +# This section states the dut name +# atmega168 +# atmega168p +# atmega328 +# atmega328p +# atmega2560_timer1 +# atmega2560_timer3 +# atmega2560_timer4 +# atmega2560_timer5 +# + +DUT=$(shell gawk '/env:/{print(substr($$1,6,length($$1)-6))}' platformio.ini) + +TRACES=-at StepISR=trace@0x25/0x08 # PB3 +TRACES+=-at FillISR=trace@0x25/0x10 # PB4 + +# +ifeq ($(DUT),atmega2560_timer1) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 ATMega2560 +TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 ATMega2560 +TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer3) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x02e/0x08 #OC3A PE3 5 ATMega2560 +TRACES+=-at StepB=trace@0x02e/0x10 #OC3B PE4 2 ATMega2560 +TRACES+=-at StepC=trace@0x02e/0x20 #OC3C PE5 3 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer4) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x102/0x08 #OC4A PH3 6 ATMega2560 +TRACES+=-at StepB=trace@0x102/0x10 #OC4B PH4 7 ATMega2560 +TRACES+=-at StepC=trace@0x102/0x20 #OC4C PH5 8 ATMega2560 +# +else ifeq ($(DUT),atmega2560_timer5) +DEVICE=atmega2560 +TRACES+=-at StepA=trace@0x10b/0x08 #OC5A PL3 46 ATMega2560 +TRACES+=-at StepB=trace@0x10b/0x10 #OC5B PL4 45 ATMega2560 +TRACES+=-at StepC=trace@0x10b/0x20 #OC5C PL5 44 ATMega2560 + +else ifeq ($(DUT),atmega168) +DEVICE=atmega168 +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168 +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168 + +else ifeq ($(DUT),atmega168p) +DEVICE=atmega168p +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 atmega168p +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 atmega168p + +else ifeq ($(DUT),atmega328) +DEVICE=atmega328 +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328 +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328 + +else ifeq ($(DUT),atmega328p) +DEVICE=atmega328p +TRACES+=-at StepA=trace@0x25/0x02 #OC1A PB1 9 ATMega328p +TRACES+=-at StepB=trace@0x25/0x04 #OC1B PB2 10 ATMega328p + +else ifeq ($(DUT),atmega32u4) +DEVICE=atmega32u4 +TRACES+=-at StepA=trace@0x025/0x20 #OC1A PB5 11 +TRACES+=-at StepB=trace@0x025/0x40 #OC1B PB6 12 +#TRACES+=-at StepC=trace@0x025/0x80 #OC1C PB7 13 + +endif + +ifeq ($(DEVICE),atmega2560) +TRACES+=-at DirA=trace@0x2b/0x01 # Pin 21 PD0 +TRACES+=-at DirB=trace@0x2b/0x02 # Pin 20 PD1 +TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 +TRACES+=-at EnableA=trace@0x2b/0x04 # Pin 19 PD2 +TRACES+=-at EnableB=trace@0x2b/0x08 # Pin 18 PD3 +TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 + +else ifeq ($(DEVICE),$(filter $(DEVICE),atmega168 atmega168p atmega328 atmega328p)) +TRACES+=-at DirA=trace@0x2b/0x20 # Pin 5 PD5 +TRACES+=-at DirB=trace@0x2b/0x80 # Pin 7 PD7 +TRACES+=-at EnableA=trace@0x2b/0x40 # Pin 6 PD6 +TRACES+=-at EnableB=trace@0x25/0x01 # Pin 8 PB0 + +else ifeq ($(DUT),atmega32u4) +TRACES+=-at DirA=trace@0x25/0x10 # Pin 26 PB4 +TRACES+=-at DirB=trace@0x25/0x08 # Pin 14 PB3 +#TRACES+=-at DirC=trace@0x10b/0x80 # Pin 42 PL7 +TRACES+=-at EnableA=trace@0x25/0x04 # Pin 16 PB2 +TRACES+=-at EnableB=trace@0x25/0x02 # Pin 15 PB1 +#TRACES+=-at EnableC=trace@0x10b/0x40 # Pin 43 PL6 + +endif + +FIRMWARE=".pio/build/$(DUT)/firmware.elf" + +DIR=$(shell env pwd) + +ifndef SILENCE + SILENCE=0 +endif + +test: .tested + +.tested: result.txt ../judge_pos0.awk + echo DUT=$(DUT) + rm -f .tested + gawk -f ../judge_pos0.awk -v DIR=$(DIR) result.txt + test -f .tested + +result.txt: x.vcd + gawk -v SILENCE=$(SILENCE) -f ../eval.awk x.vcd >/dev/null + +x.vcd: $(SRC) ../run_avr platformio.ini src/Issue250.ino + ~/.platformio/penv/bin/pio run -e $(DUT) || ~/.local/bin/pio run -e $(DUT) || env pio run -e $(DUT) + ../run_avr $(FIRMWARE) -m $(DEVICE) -o x.vcd $(TRACES) + +src/Issue250.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue250/Issue250.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/off_test_issue250_30us/expect.txt b/extras/tests/simavr_based/off_test_issue250_30us/expect.txt new file mode 100644 index 00000000..e69de29b diff --git a/extras/tests/simavr_based/off_test_issue250_30us/platformio.ini b/extras/tests/simavr_based/off_test_issue250_30us/platformio.ini new file mode 100644 index 00000000..4136b312 --- /dev/null +++ b/extras/tests/simavr_based/off_test_issue250_30us/platformio.ini @@ -0,0 +1,32 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[platformio] + +# There should be only one env section for the DUT under test. +# One of +# atmega168p +# atmega328p +# atmega2560_timer1 +# atmega2560_timer3 +# atmega2560_timer4 +# atmega2560_timer5 +# +[common] +# This is the line input to StepperDemo: +#build_flags = -D SIMULATOR -D SIMAVR_TIME_MEASUREMENT -D BLOCK_INTERRUPT_US=30 +build_flags = -D SIMULATOR -D SIMAVR_TIME_MEASUREMENT -D BLOCK_INTERRUPT_US=30 -D LOOPS=100 -D TOGGLE_DIRECTION=false -D USE_MOVETO=false + +[env:atmega328p] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index 1292acd0..79f43eca 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -16,8 +16,10 @@ TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##1)) & ~_BV(COM##T##X##0) // Force compare of Stepper_Toggle appears to be broken in simavr // In other words, use of Stepper_Toggle yields errors in simavr, for which root cause is unclear +#ifdef DISABLE #define Stepper_Toggle(T, X) \ - TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##0)) & ~_BV(COM##T##X##1) + TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##0)) & ~_BV(COM##T##X##1) +#endif #define Stepper_One(T, X) TCCR##T##A |= _BV(COM##T##X##1) | _BV(COM##T##X##0) #define Stepper_Disconnect(T, X) \ TCCR##T##A &= ~(_BV(COM##T##X##1) | _BV(COM##T##X##0)) @@ -155,12 +157,12 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { /* There is a risk, that this new compare time is delayed by one cycle */ \ uint16_t ticks = e->ticks; \ /* Set output to zero, this works in any case. In case of pause: no-op */ \ - Stepper_Zero(T, CHANNEL); \ + Stepper_Zero(T, CHANNEL); \ ForceCompare(T, CHANNEL); \ if (e->steps > 1) { \ - /* perform another step with this queue entry, toggle mode is active */ \ + /* perform another step with this queue entry */ \ e->steps--; \ - Stepper_Toggle(T, CHANNEL); \ + Stepper_One(T, CHANNEL); \ } else { \ /* either pause command or no more steps */ \ if (fas_queue_##CHANNEL._noMoreCommands) { \ @@ -177,7 +179,6 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { fas_queue_##CHANNEL.read_idx = rp; \ if (rp == wp) { \ /* queue is empty, wait this command to complete, then disconnect */ \ - Stepper_Zero(T, CHANNEL); \ fas_queue_##CHANNEL._noMoreCommands = true; \ OCR##T##CHANNEL += ticks; \ exitStepperISR(); \ @@ -189,9 +190,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { Stepper_ToggleDirection(CHANNEL); \ } \ if (e->steps != 0) { \ - Stepper_Toggle(T, CHANNEL); \ - } else { \ - Stepper_Zero(T, CHANNEL); \ + Stepper_One(T, CHANNEL); \ } \ } \ OCR##T##CHANNEL += ticks; \ @@ -238,9 +237,10 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) /* ensure no compare event */ \ SetTimerCompareRelative(T, CHANNEL, 32768); \ /* set output one, if steps to be generated */ \ - Stepper_Zero(T, CHANNEL); \ if (e->steps != 0) { \ - Stepper_Toggle(T, CHANNEL); \ + Stepper_One(T, CHANNEL); \ + } else { \ + Stepper_Zero(T, CHANNEL); \ } \ /* clear interrupt flag */ \ ClearInterruptFlag(T, CHANNEL); \ From 3990aa44e244ea18f566ddad58c6f9a50ea2427b Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Wed, 1 May 2024 01:11:46 +0000 Subject: [PATCH 81/85] final rework for issue #250. all tests passed --- CHANGELOG.md | 4 ++++ extras/tests/simavr_based/test_issue208/expect.txt | 10 +++++----- .../.gitignore | 0 .../Makefile | 0 .../expect.txt | 0 .../platformio.ini | 0 .../simavr_based/test_sd_04_timing_2560/expect.txt | 14 +++++++------- .../simavr_based/test_sd_04_timing_328p/expect.txt | 12 ++++++------ .../test_sd_04_timing_328p_37k/expect.txt | 8 ++++---- 9 files changed, 26 insertions(+), 22 deletions(-) rename extras/tests/simavr_based/{off_test_issue250_30us => test_issue250_30us}/.gitignore (100%) rename extras/tests/simavr_based/{off_test_issue250_30us => test_issue250_30us}/Makefile (100%) rename extras/tests/simavr_based/{off_test_issue250_30us => test_issue250_30us}/expect.txt (100%) rename extras/tests/simavr_based/{off_test_issue250_30us => test_issue250_30us}/platformio.ini (100%) diff --git a/CHANGELOG.md b/CHANGELOG.md index d5af3994..d9408222 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,10 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works +pre-0.30.13:1 +- avr: rework Stepper-ISR routine. It should now be robust against interrupt blockage in regard to steps lost. If interrupt blockage is too long, then 4ms paus could occur between two steps. +- avr: Interrupt blockage of 30us tested and passed + 0.30.12: - esp32: fix deprecation warning for `rmt_memory_rw_rst()` - esp32: add build test for platform espressif v6.6.0 with arduino core (#251) diff --git a/extras/tests/simavr_based/test_issue208/expect.txt b/extras/tests/simavr_based/test_issue208/expect.txt index eac920c6..6cd81dbf 100644 --- a/extras/tests/simavr_based/test_issue208/expect.txt +++ b/extras/tests/simavr_based/test_issue208/expect.txt @@ -2,17 +2,17 @@ DirB: 1*L->H, 0*H->L EnableA: 0*L->H, 0*H->L EnableB: 0*L->H, 0*H->L - StepA: 235907*L->H, 235907*H->L, Max High=10us Total High=927140us + StepA: 235908*L->H, 235908*H->L, Max High=11us Total High=927312us StepB: 2*L->H, 2*H->L, Max High=12us Total High=17us -Position[A]=37463 +Position[A]=37462 Position[B]=2 -Time in FillISR max=768 us, total=1213765 us +Time in FillISR max=767 us, total=1211908 us -Time in StepA max=10 us, total=927140 us +Time in StepA max=11 us, total=927312 us Time in StepB max=12 us, total=17 us -Time in StepISR max=6 us, total=912200 us +Time in StepISR max=6 us, total=897842 us diff --git a/extras/tests/simavr_based/off_test_issue250_30us/.gitignore b/extras/tests/simavr_based/test_issue250_30us/.gitignore similarity index 100% rename from extras/tests/simavr_based/off_test_issue250_30us/.gitignore rename to extras/tests/simavr_based/test_issue250_30us/.gitignore diff --git a/extras/tests/simavr_based/off_test_issue250_30us/Makefile b/extras/tests/simavr_based/test_issue250_30us/Makefile similarity index 100% rename from extras/tests/simavr_based/off_test_issue250_30us/Makefile rename to extras/tests/simavr_based/test_issue250_30us/Makefile diff --git a/extras/tests/simavr_based/off_test_issue250_30us/expect.txt b/extras/tests/simavr_based/test_issue250_30us/expect.txt similarity index 100% rename from extras/tests/simavr_based/off_test_issue250_30us/expect.txt rename to extras/tests/simavr_based/test_issue250_30us/expect.txt diff --git a/extras/tests/simavr_based/off_test_issue250_30us/platformio.ini b/extras/tests/simavr_based/test_issue250_30us/platformio.ini similarity index 100% rename from extras/tests/simavr_based/off_test_issue250_30us/platformio.ini rename to extras/tests/simavr_based/test_issue250_30us/platformio.ini diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 559ecba9..0e01c2fa 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,9 +4,9 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=26us Total High=479601us - StepB: 64000*L->H, 64000*H->L, Max High=30us Total High=368752us - StepC: 64000*L->H, 64000*H->L, Max High=36us Total High=379262us + StepA: 64000*L->H, 64000*H->L, Max High=29us Total High=491914us + StepB: 64000*L->H, 64000*H->L, Max High=31us Total High=363617us + StepC: 64000*L->H, 64000*H->L, Max High=41us Total High=374085us Position[A]=64000 Position[B]=64000 @@ -19,11 +19,11 @@ Time in EnableB max=246080 us, total=246080 us Time in EnableC max=254234 us, total=254234 us -Time in FillISR max=2805 us, total=2111191 us +Time in FillISR max=2697 us, total=2102378 us -Time in StepA max=26 us, total=479601 us +Time in StepA max=29 us, total=491914 us -Time in StepB max=30 us, total=368752 us +Time in StepB max=31 us, total=363617 us -Time in StepC max=36 us, total=379262 us +Time in StepC max=41 us, total=374085 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 573eddea..4bdba4a8 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,8 +2,8 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=4334us - StepB: 1000*L->H, 1000*H->L, Max High=14us Total High=4915us + StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=4311us + StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=4977us Position[A]=1000 Position[B]=1000 @@ -12,11 +12,11 @@ Time in EnableA max=225398 us, total=225398 us Time in EnableB max=238115 us, total=238115 us -Time in FillISR max=2640 us, total=47588 us +Time in FillISR max=2639 us, total=47567 us -Time in StepA max=14 us, total=4334 us +Time in StepA max=14 us, total=4311 us -Time in StepB max=14 us, total=4915 us +Time in StepB max=15 us, total=4977 us -Time in StepISR max=6 us, total=8679 us +Time in StepISR max=6 us, total=8566 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index e6e67ac0..d9d2b02b 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=11us Total High=3969us + StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=3965us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2013 us, total=27494 us +Time in FillISR max=2013 us, total=27460 us -Time in StepA max=11 us, total=3969 us +Time in StepA max=10 us, total=3965 us -Time in StepISR max=5 us, total=4031 us +Time in StepISR max=5 us, total=3975 us From e5a3d887f8573d66596612d4f2273c61f0766fa3 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Wed, 1 May 2024 08:58:59 +0000 Subject: [PATCH 82/85] avr: make `setAbsoluteSpeedLimit()` available --- src/FastAccelStepper.h | 6 ++++-- src/StepperISR.h | 8 +++++--- src/StepperISR_avr.cpp | 7 ------- src/StepperISR_esp32.cpp | 4 ---- src/fas_common.h | 2 +- 5 files changed, 10 insertions(+), 17 deletions(-) diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index b3e93333..72a36736 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -333,8 +333,10 @@ class FastAccelStepper { uint32_t getMaxSpeedInHz(); uint32_t getMaxSpeedInMilliHz(); - // For esp32, the device's maximum allowed speed can be overridden - // This is absolutely untested. Use at your own risk. + // For esp32 and avr, the device's maximum allowed speed can be overridden. + // Allocating a new stepper will override any absolute speed limit. + // This is absolutely untested, no error checking implemented. + // Use at your own risk ! #if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); #endif diff --git a/src/StepperISR.h b/src/StepperISR.h index aabc8ae6..705ae423 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -136,9 +136,6 @@ class StepperQueue { entry[read_idx & QUEUE_LEN_MASK].repeat_entry = 0; } #endif -#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 - void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); -#endif int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start); int32_t getCurrentPosition(); @@ -190,6 +187,11 @@ class StepperQueue { } #endif } + #if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 + void setAbsoluteSpeedLimit(uint16_t ticks) { + max_speed_in_ticks = ticks; + } + #endif void adjustSpeedToStepperCount(uint8_t steppers); static bool isValidStepPin(uint8_t step_pin); static int8_t queueNumForStepPin(uint8_t step_pin); diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index 79f43eca..abcf54a2 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -337,18 +337,11 @@ int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { return -1; } void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { - // using test_sd_04_timing_2560 version 0.25.6 as reference - // manageStepper (fillISR) already needs max 3ms ! - // so 25kHz for three steppers is on the limit - // // commit 9577e9bfd4b9a6cf1ad830901c00c8b129a62aee fails // test_sd_04_timing_2560 as timer 3 reaches 40us. // This includes port set/clear for timer measurement. // So choose 20kHz // - // using test_sd_04_timing_328p version 0.25.6 as reference - // manageStepper (fillISR) already needs max 2.3 ms ! - // // check if Issue_152.ino, the interrupt need 14us. // So 70000 Steps/s is too high. if (steppers == 1) { diff --git a/src/StepperISR_esp32.cpp b/src/StepperISR_esp32.cpp index 02ca1717..9b7b2e02 100644 --- a/src/StepperISR_esp32.cpp +++ b/src/StepperISR_esp32.cpp @@ -125,10 +125,6 @@ void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { max_speed_in_ticks = 80; // This equals 200kHz @ 16MHz } -void StepperQueue::setAbsoluteSpeedLimit(uint16_t ticks) { - max_speed_in_ticks = ticks; -} - void fas_init_engine(FastAccelStepperEngine *engine, uint8_t cpu_core) { #define STACK_SIZE 2000 #define PRIORITY configMAX_PRIORITIES diff --git a/src/fas_common.h b/src/fas_common.h index 0d209599..2a4dfc4f 100644 --- a/src/fas_common.h +++ b/src/fas_common.h @@ -348,7 +348,7 @@ struct queue_end_s { //========================================================================== #elif defined(ARDUINO_ARCH_AVR) #define SUPPORT_AVR -#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 0 +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1 // this is an arduino platform, so include the Arduino.h header file #include From 62c648ceafc7ffc1c6d819e9ac424f5e213e9cc2 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Wed, 1 May 2024 09:14:42 +0000 Subject: [PATCH 83/85] Add configurable forward planning time for filling the stepper queue (#253) --- CHANGELOG.md | 4 +- extras/doc/FastAccelStepper_API.md | 42 +++++++++++++++++-- .../test_sd_04_timing_2560/expect.txt | 18 ++++---- .../test_sd_04_timing_328p/expect.txt | 12 +++--- .../test_sd_04_timing_328p_37k/expect.txt | 6 +-- src/FastAccelStepper.cpp | 6 ++- src/FastAccelStepper.h | 32 ++++++++++++++ 7 files changed, 96 insertions(+), 24 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d9408222..b96e2cac 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,9 +6,11 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.13:1 +pre-0.30.13: - avr: rework Stepper-ISR routine. It should now be robust against interrupt blockage in regard to steps lost. If interrupt blockage is too long, then 4ms paus could occur between two steps. - avr: Interrupt blockage of 30us tested and passed +- Add configurable forward planning time for filling the stepper queue (#253) +- avr: make `setAbsoluteSpeedLimit()` available 0.30.12: - esp32: fix deprecation warning for `rmt_memory_rw_rst()` diff --git a/extras/doc/FastAccelStepper_API.md b/extras/doc/FastAccelStepper_API.md index e5e38c8d..97dfc231 100644 --- a/extras/doc/FastAccelStepper_API.md +++ b/extras/doc/FastAccelStepper_API.md @@ -76,7 +76,8 @@ This call allows to select the respective driver #define DRIVER_MCPWM_PCNT 0 #define DRIVER_RMT 1 #define DRIVER_DONT_CARE 2 - FastAccelStepper* stepperConnectToPin(uint8_t step_pin, uint8_t driver_type = DRIVER_DONT_CARE); + FastAccelStepper* stepperConnectToPin(uint8_t step_pin, + uint8_t driver_type = DRIVER_DONT_CARE); #endif ``` Comments to valid pins: @@ -292,8 +293,10 @@ For the device's maximum allowed speed, the following calls can be used. uint32_t getMaxSpeedInHz(); uint32_t getMaxSpeedInMilliHz(); ``` -For esp32, the device's maximum allowed speed can be overridden -This is absolutely untested. Use at your own risk. +For esp32 and avr, the device's maximum allowed speed can be overridden. +Allocating a new stepper will override any absolute speed limit. +This is absolutely untested, no error checking implemented. +Use at your own risk ! ```cpp #if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); @@ -510,6 +513,39 @@ In keep running mode, the targetPos() is not updated ```cpp int32_t targetPos() { return _rg.targetPosition(); } ``` +The stepper task adds commands to the stepper queue until +either at least two commands are planned, or the commands +cover sufficient time into the future. Default value for that time is 20ms. + +The stepper task is cyclically executed every ~4ms. +Especially for avr, the step interrupts puts a significant load on the uC, +so the cyclical stepper task can even run for 2-3 ms. On top of that, +other interrupts caused by the application could increase the load even further. + +Consequently, the forward planning should fill the queue for ideally two cycles, +this means 8ms. This means, the default 20ms provide a sufficient margin and +even a missed cycle is not an issue. + +The drawback of the 20ms is, that any change in speed/acceleration are added after +those 20ms and for an application, requiring fast reaction times, this may +impact the expected performance. + +Due to this the forward planning time can be adjusted with the following API call +for each stepper individually. + +Attention: +- This is only for advanced users: no error checking is implemented. +- Only change the forward planning time, if the stepper is not running. +- Too small values bear the risk of a stepper running at full speed suddenly stopping + due to lack of commands in the queue. +```cpp + void setForwardPlanningTimeInMs(uint8_t ms) { + _forward_planning_in_ticks = ms; +``` + _forward_planning_in_ticks *= TICKS_PER_S / 1000;ticks per ms +```cpp + } +``` ## Low Level Stepper Queue Management (low level access) If the queue is already running, then the start parameter is obsolote. diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt index 0e01c2fa..9c2057ff 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -4,26 +4,26 @@ EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L EnableC: 2*L->H, 1*H->L - StepA: 64000*L->H, 64000*H->L, Max High=29us Total High=491914us - StepB: 64000*L->H, 64000*H->L, Max High=31us Total High=363617us - StepC: 64000*L->H, 64000*H->L, Max High=41us Total High=374085us + StepA: 64000*L->H, 64000*H->L, Max High=25us Total High=311049us + StepB: 64000*L->H, 64000*H->L, Max High=31us Total High=364986us + StepC: 64000*L->H, 64000*H->L, Max High=38us Total High=530547us Position[A]=64000 Position[B]=64000 Position[C]=64000 -Time in EnableA max=233598 us, total=233598 us +Time in EnableA max=233599 us, total=233599 us Time in EnableB max=246080 us, total=246080 us -Time in EnableC max=254234 us, total=254234 us +Time in EnableC max=254243 us, total=254243 us -Time in FillISR max=2697 us, total=2102378 us +Time in FillISR max=2740 us, total=2116523 us -Time in StepA max=29 us, total=491914 us +Time in StepA max=25 us, total=311049 us -Time in StepB max=31 us, total=363617 us +Time in StepB max=31 us, total=364986 us -Time in StepC max=41 us, total=374085 us +Time in StepC max=38 us, total=530547 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt index 4bdba4a8..f0433a5f 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -2,21 +2,21 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 2*L->H, 1*H->L - StepA: 1000*L->H, 1000*H->L, Max High=14us Total High=4311us - StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=4977us + StepA: 1000*L->H, 1000*H->L, Max High=13us Total High=4231us + StepB: 1000*L->H, 1000*H->L, Max High=15us Total High=5045us Position[A]=1000 Position[B]=1000 Time in EnableA max=225398 us, total=225398 us -Time in EnableB max=238115 us, total=238115 us +Time in EnableB max=238118 us, total=238118 us -Time in FillISR max=2639 us, total=47567 us +Time in FillISR max=2651 us, total=47806 us -Time in StepA max=14 us, total=4311 us +Time in StepA max=13 us, total=4231 us -Time in StepB max=15 us, total=4977 us +Time in StepB max=15 us, total=5045 us Time in StepISR max=6 us, total=8566 us diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt index d9d2b02b..e3fcffa0 100644 --- a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -2,15 +2,15 @@ DirB: 1*L->H, 0*H->L EnableA: 1*L->H, 1*H->L EnableB: 1*L->H, 0*H->L - StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=3965us + StepA: 1000*L->H, 1000*H->L, Max High=10us Total High=3948us StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us Position[A]=1000 Time in EnableA max=225399 us, total=225399 us -Time in FillISR max=2013 us, total=27460 us +Time in FillISR max=2023 us, total=27615 us -Time in StepA max=10 us, total=3965 us +Time in StepA max=10 us, total=3948 us Time in StepISR max=5 us, total=3975 us diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index 6b5aabe2..e432660c 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -377,12 +377,13 @@ void FastAccelStepper::fill_queue() { // preconditions are fulfilled, so create the command(s) NextCommand cmd; - // Plan ahead for max. 20 ms. Currently hard coded + // Plan ahead for max. 20 ms and minimum two commands. + // This is now configurable using _forward_planning_in_ticks. bool delayed_start = !q->isRunning(); bool need_delayed_start = false; uint32_t ticksPrepared = q->ticksInQueue(); while (!isQueueFull() && - ((ticksPrepared < TICKS_PER_S / 50) || q->queueEntries() <= 1) && + ((ticksPrepared < _forward_planning_in_ticks) || q->queueEntries() <= 1) && _rg.isRampGeneratorActive()) { #if (TEST_MEASURE_ISR_SINGLE_FILL == 1) // For run time measurement @@ -508,6 +509,7 @@ void FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num, _dirPin = PIN_UNDEFINED; _enablePinHighActive = PIN_UNDEFINED; _enablePinLowActive = PIN_UNDEFINED; + _forward_planning_in_ticks = TICKS_PER_S/50; _rg.init(); _queue_num = num; diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index 72a36736..eaaa90a1 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -535,6 +535,36 @@ class FastAccelStepper { // In keep running mode, the targetPos() is not updated inline int32_t targetPos() { return _rg.targetPosition(); } + // The stepper task adds commands to the stepper queue until + // either at least two commands are planned, or the commands + // cover sufficient time into the future. Default value for that time is 20ms. + // + // The stepper task is cyclically executed every ~4ms. + // Especially for avr, the step interrupts puts a significant load on the uC, + // so the cyclical stepper task can even run for 2-3 ms. On top of that, + // other interrupts caused by the application could increase the load even further. + // + // Consequently, the forward planning should fill the queue for ideally two cycles, + // this means 8ms. This means, the default 20ms provide a sufficient margin and + // even a missed cycle is not an issue. + // + // The drawback of the 20ms is, that any change in speed/acceleration are added after + // those 20ms and for an application, requiring fast reaction times, this may + // impact the expected performance. + // + // Due to this the forward planning time can be adjusted with the following API call + // for each stepper individually. + // + // Attention: + // - This is only for advanced users: no error checking is implemented. + // - Only change the forward planning time, if the stepper is not running. + // - Too small values bear the risk of a stepper running at full speed suddenly stopping + // due to lack of commands in the queue. + inline void setForwardPlanningTimeInMs(uint8_t ms) { + _forward_planning_in_ticks = ms; + _forward_planning_in_ticks *= TICKS_PER_S / 1000; // ticks per ms + } + // ## Low Level Stepper Queue Management (low level access) // // If the queue is already running, then the start parameter is obsolote. @@ -686,6 +716,8 @@ class FastAccelStepper { uint16_t _off_delay_count; uint16_t _auto_disable_delay_counter; + uint32_t _forward_planning_in_ticks; + #if defined(SUPPORT_ESP32_PULSE_COUNTER) int16_t _attached_pulse_cnt_unit; #endif From 56df459584fd62684109839c1a32a8a64eabf5c2 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Wed, 1 May 2024 09:15:32 +0000 Subject: [PATCH 84/85] run format_code.sh --- src/FastAccelStepper.cpp | 5 +- src/FastAccelStepper.h | 24 ++++---- src/StepperISR.h | 8 +-- src/StepperISR_avr.cpp | 123 ++++++++++++++++++++------------------- 4 files changed, 81 insertions(+), 79 deletions(-) diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index e432660c..a995bb9d 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -383,7 +383,8 @@ void FastAccelStepper::fill_queue() { bool need_delayed_start = false; uint32_t ticksPrepared = q->ticksInQueue(); while (!isQueueFull() && - ((ticksPrepared < _forward_planning_in_ticks) || q->queueEntries() <= 1) && + ((ticksPrepared < _forward_planning_in_ticks) || + q->queueEntries() <= 1) && _rg.isRampGeneratorActive()) { #if (TEST_MEASURE_ISR_SINGLE_FILL == 1) // For run time measurement @@ -509,7 +510,7 @@ void FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num, _dirPin = PIN_UNDEFINED; _enablePinHighActive = PIN_UNDEFINED; _enablePinLowActive = PIN_UNDEFINED; - _forward_planning_in_ticks = TICKS_PER_S/50; + _forward_planning_in_ticks = TICKS_PER_S / 50; _rg.init(); _queue_num = num; diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index eaaa90a1..35637c12 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -542,27 +542,29 @@ class FastAccelStepper { // The stepper task is cyclically executed every ~4ms. // Especially for avr, the step interrupts puts a significant load on the uC, // so the cyclical stepper task can even run for 2-3 ms. On top of that, - // other interrupts caused by the application could increase the load even further. + // other interrupts caused by the application could increase the load even + // further. // - // Consequently, the forward planning should fill the queue for ideally two cycles, - // this means 8ms. This means, the default 20ms provide a sufficient margin and - // even a missed cycle is not an issue. + // Consequently, the forward planning should fill the queue for ideally two + // cycles, this means 8ms. This means, the default 20ms provide a sufficient + // margin and even a missed cycle is not an issue. // - // The drawback of the 20ms is, that any change in speed/acceleration are added after - // those 20ms and for an application, requiring fast reaction times, this may - // impact the expected performance. + // The drawback of the 20ms is, that any change in speed/acceleration are + // added after those 20ms and for an application, requiring fast reaction + // times, this may impact the expected performance. // - // Due to this the forward planning time can be adjusted with the following API call - // for each stepper individually. + // Due to this the forward planning time can be adjusted with the following + // API call for each stepper individually. // // Attention: // - This is only for advanced users: no error checking is implemented. // - Only change the forward planning time, if the stepper is not running. - // - Too small values bear the risk of a stepper running at full speed suddenly stopping + // - Too small values bear the risk of a stepper running at full speed + // suddenly stopping // due to lack of commands in the queue. inline void setForwardPlanningTimeInMs(uint8_t ms) { _forward_planning_in_ticks = ms; - _forward_planning_in_ticks *= TICKS_PER_S / 1000; // ticks per ms + _forward_planning_in_ticks *= TICKS_PER_S / 1000; // ticks per ms } // ## Low Level Stepper Queue Management (low level access) diff --git a/src/StepperISR.h b/src/StepperISR.h index 705ae423..481a390d 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -187,11 +187,9 @@ class StepperQueue { } #endif } - #if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 - void setAbsoluteSpeedLimit(uint16_t ticks) { - max_speed_in_ticks = ticks; - } - #endif +#if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 + void setAbsoluteSpeedLimit(uint16_t ticks) { max_speed_in_ticks = ticks; } +#endif void adjustSpeedToStepperCount(uint8_t steppers); static bool isValidStepPin(uint8_t step_pin); static int8_t queueNumForStepPin(uint8_t step_pin); diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index abcf54a2..2b1b7a16 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -15,10 +15,11 @@ #define Stepper_Zero(T, X) \ TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##1)) & ~_BV(COM##T##X##0) // Force compare of Stepper_Toggle appears to be broken in simavr -// In other words, use of Stepper_Toggle yields errors in simavr, for which root cause is unclear +// In other words, use of Stepper_Toggle yields errors in simavr, for which root +// cause is unclear #ifdef DISABLE #define Stepper_Toggle(T, X) \ - TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##0)) & ~_BV(COM##T##X##1) + TCCR##T##A = (TCCR##T##A | _BV(COM##T##X##0)) & ~_BV(COM##T##X##1) #endif #define Stepper_One(T, X) TCCR##T##A |= _BV(COM##T##X##1) | _BV(COM##T##X##0) #define Stepper_Disconnect(T, X) \ @@ -136,65 +137,65 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { // Remark: Interrupt Flag is automatically cleared on ISR execution // // If reaching here without further commands, then the queue is done -#define AVR_STEPPER_ISR(T, CHANNEL) \ - ISR(TIMER##T##_COMP##CHANNEL##_vect) { \ - enterStepperISR(); \ - uint8_t rp = fas_queue_##CHANNEL.read_idx; \ - uint8_t wp = fas_queue_##CHANNEL.next_write_idx; \ - if (rp == wp) { \ - /* queue is empty => set to disconnect */ \ - /* disable compare interrupt */ \ - DisableCompareInterrupt(T, CHANNEL); \ - Stepper_Disconnect(T, CHANNEL); \ - /* force compare to ensure disconnect */ \ - ForceCompare(T, CHANNEL); \ - fas_queue_##CHANNEL._isRunning = false; \ - fas_queue_##CHANNEL._noMoreCommands = false; \ - exitStepperISR(); \ - return; \ - } \ - struct queue_entry* e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ - /* There is a risk, that this new compare time is delayed by one cycle */ \ - uint16_t ticks = e->ticks; \ - /* Set output to zero, this works in any case. In case of pause: no-op */ \ - Stepper_Zero(T, CHANNEL); \ - ForceCompare(T, CHANNEL); \ - if (e->steps > 1) { \ - /* perform another step with this queue entry */ \ - e->steps--; \ - Stepper_One(T, CHANNEL); \ - } else { \ - /* either pause command or no more steps */ \ - if (fas_queue_##CHANNEL._noMoreCommands) { \ - /* new command received after running out of commands */ \ - /* if this new command requires a step, then this step would be lost */ \ - fas_queue_##CHANNEL._noMoreCommands = false; \ - if (e->steps != 0) { \ - /* New command needs steps, so do it immediately */ \ - ticks = 10; \ - } \ - } \ - else if (TEST_NOT_REPEATING_ENTRY) { \ - rp++; \ - fas_queue_##CHANNEL.read_idx = rp; \ - if (rp == wp) { \ - /* queue is empty, wait this command to complete, then disconnect */ \ - fas_queue_##CHANNEL._noMoreCommands = true; \ - OCR##T##CHANNEL += ticks; \ - exitStepperISR(); \ - return; \ - } \ - e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ - } \ - if (e->toggle_dir) { \ - Stepper_ToggleDirection(CHANNEL); \ - } \ - if (e->steps != 0) { \ - Stepper_One(T, CHANNEL); \ - } \ - } \ - OCR##T##CHANNEL += ticks; \ - exitStepperISR(); \ +#define AVR_STEPPER_ISR(T, CHANNEL) \ + ISR(TIMER##T##_COMP##CHANNEL##_vect) { \ + enterStepperISR(); \ + uint8_t rp = fas_queue_##CHANNEL.read_idx; \ + uint8_t wp = fas_queue_##CHANNEL.next_write_idx; \ + if (rp == wp) { \ + /* queue is empty => set to disconnect */ \ + /* disable compare interrupt */ \ + DisableCompareInterrupt(T, CHANNEL); \ + Stepper_Disconnect(T, CHANNEL); \ + /* force compare to ensure disconnect */ \ + ForceCompare(T, CHANNEL); \ + fas_queue_##CHANNEL._isRunning = false; \ + fas_queue_##CHANNEL._noMoreCommands = false; \ + exitStepperISR(); \ + return; \ + } \ + struct queue_entry* e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ + /* There is a risk, that this new compare time is delayed by one cycle */ \ + uint16_t ticks = e->ticks; \ + /* Set output to zero, this works in any case. In case of pause: no-op */ \ + Stepper_Zero(T, CHANNEL); \ + ForceCompare(T, CHANNEL); \ + if (e->steps > 1) { \ + /* perform another step with this queue entry */ \ + e->steps--; \ + Stepper_One(T, CHANNEL); \ + } else { \ + /* either pause command or no more steps */ \ + if (fas_queue_##CHANNEL._noMoreCommands) { \ + /* new command received after running out of commands */ \ + /* if this new command requires a step, then this step would be lost \ + */ \ + fas_queue_##CHANNEL._noMoreCommands = false; \ + if (e->steps != 0) { \ + /* New command needs steps, so do it immediately */ \ + ticks = 10; \ + } \ + } else if (TEST_NOT_REPEATING_ENTRY) { \ + rp++; \ + fas_queue_##CHANNEL.read_idx = rp; \ + if (rp == wp) { \ + /* queue is empty, wait this command to complete, then disconnect */ \ + fas_queue_##CHANNEL._noMoreCommands = true; \ + OCR##T##CHANNEL += ticks; \ + exitStepperISR(); \ + return; \ + } \ + e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; \ + } \ + if (e->toggle_dir) { \ + Stepper_ToggleDirection(CHANNEL); \ + } \ + if (e->steps != 0) { \ + Stepper_One(T, CHANNEL); \ + } \ + } \ + OCR##T##CHANNEL += ticks; \ + exitStepperISR(); \ } #define AVR_STEPPER_ISR_GEN(T, CHANNEL) AVR_STEPPER_ISR(T, CHANNEL) From 0539e9b4847a77601dd000627f8d3460c6d675e0 Mon Sep 17 00:00:00 2001 From: gin66 <5549373+gin66@users.noreply.github.com> Date: Thu, 9 May 2024 17:43:14 +0000 Subject: [PATCH 85/85] bump version to 0.30.13 --- CHANGELOG.md | 2 +- library.properties | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b96e2cac..28d83b41 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +6,7 @@ TODO: - rename RampConstAcceleration to e.g. RampControl - merge the two esp32 rmt drivers as soon as esp32c3 works -pre-0.30.13: +0.30.13: - avr: rework Stepper-ISR routine. It should now be robust against interrupt blockage in regard to steps lost. If interrupt blockage is too long, then 4ms paus could occur between two steps. - avr: Interrupt blockage of 30us tested and passed - Add configurable forward planning time for filling the stepper queue (#253) diff --git a/library.properties b/library.properties index 6361e7db..0dc0fbb7 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=FastAccelStepper -version=0.30.12 +version=0.30.13 1icense=MIT author=Jochen Kiemes maintainer=Jochen Kiemes