From 1992e632d38562e4b1fc88c923e6a258ca4adfd8 Mon Sep 17 00:00:00 2001 From: Beslan Date: Sun, 22 Dec 2024 05:22:46 +0300 Subject: [PATCH] first --- .gitignore | 38 + CHANGELOG.md | 667 +++++++++++++ CMakeLists.txt | 6 + LICENSE | 21 + README.md | 512 ++++++++++ extras/ci/platformio.ini | 567 +++++++++++ extras/doc/FastAccelStepper_API.md | 722 ++++++++++++++ extras/doc/application.wxm | 44 + extras/doc/arch_esp32.txt | 62 ++ extras/doc/generalization.wxm | 338 +++++++ extras/doc/ramp.md | 125 +++ extras/doc/ramp_cubic_quadratic.md | 63 ++ extras/gen_pmf_const/Makefile | 17 + extras/gen_pmf_const/README.md | 4 + extras/gen_pmf_const/main.cpp | 86 ++ .../UsageExample/UsageExample.cpp | 78 ++ extras/scripts/build-idf-platformio.sh | 39 + extras/scripts/build-pio-dirs.sh | 59 ++ extras/scripts/build-platformio.sh | 43 + extras/scripts/format_code.sh | 11 + extras/scripts/header2markdown.sh | 50 + extras/tests/esp32_hw_based/Makefile | 29 + .../tests/esp32_hw_based/judge_pcnt_sync.awk | 51 + extras/tests/esp32_hw_based/seq_01a.sh | 34 + extras/tests/esp32_hw_based/seq_01b.sh | 34 + extras/tests/esp32_hw_based/seq_01c.sh | 34 + extras/tests/esp32_hw_based/seq_02.sh | 32 + extras/tests/esp32_hw_based/seq_03.sh | 43 + extras/tests/esp32_hw_based/seq_04.sh | 34 + extras/tests/esp32_hw_based/seq_05.sh | 34 + extras/tests/esp32_hw_based/seq_06.sh | 34 + extras/tests/esp32_hw_based/seq_07a.sh | 34 + extras/tests/esp32_hw_based/seq_07b.sh | 34 + extras/tests/esp32_hw_based/seq_07c.sh | 34 + extras/tests/esp32_hw_based/seq_xxx.sh.off | 33 + extras/tests/esp32_hw_based/test_all.sh | 13 + extras/tests/pc_based/Makefile | 58 ++ extras/tests/pc_based/PMF_test.ino | 59 ++ extras/tests/pc_based/README.txt | 46 + extras/tests/pc_based/RampChecker.h | 142 +++ extras/tests/pc_based/off_test_15.cpp | 124 +++ extras/tests/pc_based/pmf_test.cpp | 29 + extras/tests/pc_based/rmc_test.cpp | 94 ++ extras/tests/pc_based/stubs.h | 41 + extras/tests/pc_based/test_01.cpp | 118 +++ extras/tests/pc_based/test_02.cpp | 307 ++++++ extras/tests/pc_based/test_03.h | 438 +++++++++ extras/tests/pc_based/test_04.cpp | 169 ++++ extras/tests/pc_based/test_05.cpp | 160 ++++ extras/tests/pc_based/test_06.cpp | 189 ++++ extras/tests/pc_based/test_07.cpp | 132 +++ extras/tests/pc_based/test_08.cpp | 103 ++ extras/tests/pc_based/test_09.cpp | 99 ++ extras/tests/pc_based/test_10.cpp | 107 +++ extras/tests/pc_based/test_11.cpp | 108 +++ extras/tests/pc_based/test_12.cpp | 103 ++ extras/tests/pc_based/test_13.cpp | 113 +++ extras/tests/pc_based/test_14.cpp | 106 ++ extras/tests/pc_based/test_15.cpp | 119 +++ extras/tests/simavr_based/.gitignore | 2 + extras/tests/simavr_based/Makefile | 62 ++ extras/tests/simavr_based/Makefile.test | 127 +++ extras/tests/simavr_based/Raw/platformio.ini | 31 + extras/tests/simavr_based/eval.awk | 128 +++ extras/tests/simavr_based/judge.awk | 38 + extras/tests/simavr_based/judge_pos0.awk | 22 + .../off_test_seq_03/platformio.ini | 26 + .../off_test_seq_04/platformio.ini | 26 + .../off_test_seq_05/platformio.ini | 26 + .../simavr_based/off_test_timing/expect.txt | 0 .../off_test_timing/platformio.ini | 30 + .../simavr_based/off_test_timing/src/main.ino | 40 + .../simavr_based/test_externalCall/.gitignore | 1 + .../simavr_based/test_externalCall/Makefile | 131 +++ .../simavr_based/test_externalCall/expect.txt | 22 + .../test_externalCall/platformio.ini | 31 + .../simavr_based/test_issue150/.gitignore | 1 + .../tests/simavr_based/test_issue150/Makefile | 132 +++ .../simavr_based/test_issue150/expect.txt | 16 + .../simavr_based/test_issue150/platformio.ini | 31 + .../simavr_based/test_issue151/.gitignore | 1 + .../tests/simavr_based/test_issue151/Makefile | 132 +++ .../simavr_based/test_issue151/expect.txt | 16 + .../simavr_based/test_issue151/platformio.ini | 31 + .../simavr_based/test_issue152/.gitignore | 1 + .../tests/simavr_based/test_issue152/Makefile | 132 +++ .../simavr_based/test_issue152/expect.txt | 20 + .../simavr_based/test_issue152/platformio.ini | 31 + .../simavr_based/test_issue172/.gitignore | 1 + .../tests/simavr_based/test_issue172/Makefile | 132 +++ .../simavr_based/test_issue172/expect.txt | 20 + .../simavr_based/test_issue172/platformio.ini | 31 + .../simavr_based/test_issue173/.gitignore | 1 + .../tests/simavr_based/test_issue173/Makefile | 132 +++ .../simavr_based/test_issue173/expect.txt | 20 + .../simavr_based/test_issue173/platformio.ini | 31 + .../simavr_based/test_issue208/.gitignore | 1 + .../tests/simavr_based/test_issue208/Makefile | 132 +++ .../simavr_based/test_issue208/expect.txt | 18 + .../simavr_based/test_issue208/platformio.ini | 31 + .../simavr_based/test_issue250/.gitignore | 1 + .../tests/simavr_based/test_issue250/Makefile | 131 +++ .../simavr_based/test_issue250/expect.txt | 0 .../simavr_based/test_issue250/platformio.ini | 31 + .../test_issue250_30us/.gitignore | 1 + .../simavr_based/test_issue250_30us/Makefile | 131 +++ .../test_issue250_30us/expect.txt | 0 .../test_issue250_30us/platformio.ini | 32 + .../simavr_based/test_issue280/.gitignore | 1 + .../tests/simavr_based/test_issue280/Makefile | 131 +++ .../simavr_based/test_issue280/expect.txt | 16 + .../simavr_based/test_issue280/platformio.ini | 31 + extras/tests/simavr_based/test_pmf/.gitignore | 1 + extras/tests/simavr_based/test_pmf/Makefile | 135 +++ extras/tests/simavr_based/test_pmf/expect.txt | 10 + .../simavr_based/test_pmf/platformio.ini | 31 + .../test_sd_01a_2560t1/expect.txt | 15 + .../test_sd_01a_2560t1/platformio.ini | 32 + .../test_sd_01a_2560t3/expect.txt | 15 + .../test_sd_01a_2560t3/platformio.ini | 32 + .../test_sd_01a_2560t4/expect.txt | 15 + .../test_sd_01a_2560t4/platformio.ini | 32 + .../test_sd_01a_2560t5/expect.txt | 15 + .../test_sd_01a_2560t5/platformio.ini | 32 + .../simavr_based/test_sd_01a_328p/expect.txt | 12 + .../test_sd_01a_328p/platformio.ini | 31 + .../test_sd_01a_leonardo/expect.txt | 12 + .../test_sd_01a_leonardo/platformio.ini | 32 + .../test_sd_01b_2560t1/expect.txt | 15 + .../test_sd_01b_2560t1/platformio.ini | 32 + .../test_sd_01b_2560t3/expect.txt | 15 + .../test_sd_01b_2560t3/platformio.ini | 32 + .../test_sd_01b_2560t4/expect.txt | 15 + .../test_sd_01b_2560t4/platformio.ini | 32 + .../test_sd_01b_2560t5/expect.txt | 15 + .../test_sd_01b_2560t5/platformio.ini | 32 + .../simavr_based/test_sd_01b_328p/expect.txt | 12 + .../test_sd_01b_328p/platformio.ini | 31 + .../test_sd_01c_2560t1/expect.txt | 15 + .../test_sd_01c_2560t1/platformio.ini | 32 + .../test_sd_01c_2560t3/expect.txt | 15 + .../test_sd_01c_2560t3/platformio.ini | 32 + .../test_sd_01c_2560t4/expect.txt | 15 + .../test_sd_01c_2560t4/platformio.ini | 32 + .../test_sd_01c_2560t5/expect.txt | 15 + .../test_sd_01c_2560t5/platformio.ini | 32 + .../simavr_based/test_sd_02_328p/expect.txt | 12 + .../test_sd_02_328p/platformio.ini | 31 + .../simavr_based/test_sd_03_328p/expect.txt | 12 + .../test_sd_03_328p/platformio.ini | 31 + .../test_sd_04_timing_2560/expect.txt | 29 + .../test_sd_04_timing_2560/platformio.ini | 32 + .../test_sd_04_timing_328p/expect.txt | 22 + .../test_sd_04_timing_328p/platformio.ini | 31 + .../test_sd_04_timing_328p_37k/expect.txt | 16 + .../test_sd_04_timing_328p_37k/platformio.ini | 31 + .../simavr_based/test_sd_05_328p/expect.txt | 12 + .../test_sd_05_328p/platformio.ini | 31 + .../simavr_based/test_sd_06_328p/expect.txt | 12 + .../test_sd_06_328p/platformio.ini | 31 + .../simavr_based/test_sd_07_328p/expect.txt | 12 + .../test_sd_07_328p/platformio.ini | 31 + .../simavr_based/test_sd_08_328p/expect.txt | 12 + .../test_sd_08_328p/platformio.ini | 31 + .../simavr_based/test_sd_09_328p/expect.txt | 12 + .../test_sd_09_328p/platformio.ini | 31 + .../simavr_based/test_sd_10_328p/expect.txt | 12 + .../test_sd_10_328p/platformio.ini | 31 + .../simavr_based/test_sd_11_328p/expect.txt | 12 + .../test_sd_11_328p/platformio.ini | 31 + .../simavr_based/test_sd_12_328p/expect.txt | 12 + .../test_sd_12_328p/platformio.ini | 31 + .../simavr_based/test_sd_13_328p/expect.txt | 12 + .../test_sd_13_328p/platformio.ini | 31 + .../simavr_based/test_sd_14_328p/expect.txt | 12 + .../test_sd_14_328p/platformio.ini | 31 + .../simavr_based/test_sd_15_328p/expect.txt | 12 + .../test_sd_15_328p/platformio.ini | 40 + .../simavr_based/test_sd_16_328p/expect.txt | 12 + .../test_sd_16_328p/platformio.ini | 34 + .../simavr_based/test_sd_17_328p/expect.txt | 12 + .../test_sd_17_328p/platformio.ini | 34 + .../simavr_based/test_sd_18_328p/expect.txt | 12 + .../test_sd_18_328p/platformio.ini | 35 + .../simavr_based/test_sd_19_328p/expect.txt | 12 + .../test_sd_19_328p/platformio.ini | 35 + .../test_seq_sd_01_328p/expect.txt | 12 + .../test_seq_sd_01_328p/platformio.ini | 31 + .../test_seq_sd_02_328p/expect.txt | 14 + .../test_seq_sd_02_328p/platformio.ini | 19 + .../test_seq_sd_06_328p/expect.txt | 12 + .../test_seq_sd_06_328p/platformio.ini | 31 + .../test_seq_sd_07_328p/expect.txt | 12 + .../test_seq_sd_07_328p/platformio.ini | 31 + .../test_seq_sd_11_328p/expect.txt | 12 + .../test_seq_sd_11_328p/platformio.ini | 32 + keywords.txt | 24 + shell.nix | 4 + src/AVRStepperPins.h | 80 ++ src/FastAccelStepper.cpp | 906 ++++++++++++++++++ src/FastAccelStepper.h | 770 +++++++++++++++ src/FastAccelStepper_idf4_esp32_pcnt.cpp | 95 ++ src/FastAccelStepper_idf5_esp32_pcnt.cpp | 137 +++ src/PoorManFloat.cpp | 302 ++++++ src/PoorManFloat.h | 45 + src/PoorManFloatConst.h | 73 ++ src/RampCalculator.cpp | 181 ++++ src/RampCalculator.h | 235 +++++ src/RampConstAcceleration.cpp | 529 ++++++++++ src/RampConstAcceleration.h | 79 ++ src/RampGenerator.cpp | 274 ++++++ src/RampGenerator.h | 118 +++ src/StepperISR.cpp | 349 +++++++ src/StepperISR.h | 202 ++++ src/StepperISR_avr.cpp | 359 +++++++ src/StepperISR_due.cpp | 719 ++++++++++++++ src/StepperISR_esp32.cpp | 142 +++ src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp | 574 +++++++++++ src/StepperISR_idf4_esp32_rmt.cpp | 511 ++++++++++ src/StepperISR_idf4_esp32c3_rmt.cpp | 652 +++++++++++++ src/StepperISR_idf4_esp32s3_rmt.cpp | 652 +++++++++++++ src/StepperISR_idf5_esp32_rmt.cpp | 350 +++++++ src/fas_arch/arduino_avr.h | 83 ++ src/fas_arch/arduino_esp32.h | 13 + src/fas_arch/arduino_sam.h | 34 + src/fas_arch/common.h | 105 ++ src/fas_arch/common_esp32.h | 60 ++ src/fas_arch/common_esp32_idf4.h | 104 ++ src/fas_arch/common_esp32_idf5.h | 152 +++ src/fas_arch/espidf_esp32.h | 25 + src/fas_arch/test_pc.h | 42 + src/fas_arch/test_probe.h | 56 ++ 232 files changed, 20394 insertions(+) create mode 100644 .gitignore create mode 100644 CHANGELOG.md create mode 100644 CMakeLists.txt create mode 100644 LICENSE create mode 100644 README.md create mode 100644 extras/ci/platformio.ini create mode 100644 extras/doc/FastAccelStepper_API.md create mode 100644 extras/doc/application.wxm create mode 100644 extras/doc/arch_esp32.txt create mode 100644 extras/doc/generalization.wxm create mode 100644 extras/doc/ramp.md create mode 100644 extras/doc/ramp_cubic_quadratic.md create mode 100644 extras/gen_pmf_const/Makefile create mode 100644 extras/gen_pmf_const/README.md create mode 100644 extras/gen_pmf_const/main.cpp create mode 100644 extras/idf_examples/UsageExample/UsageExample.cpp create mode 100755 extras/scripts/build-idf-platformio.sh create mode 100755 extras/scripts/build-pio-dirs.sh create mode 100755 extras/scripts/build-platformio.sh create mode 100755 extras/scripts/format_code.sh create mode 100755 extras/scripts/header2markdown.sh create mode 100644 extras/tests/esp32_hw_based/Makefile create mode 100644 extras/tests/esp32_hw_based/judge_pcnt_sync.awk create mode 100755 extras/tests/esp32_hw_based/seq_01a.sh create mode 100755 extras/tests/esp32_hw_based/seq_01b.sh create mode 100755 extras/tests/esp32_hw_based/seq_01c.sh create mode 100755 extras/tests/esp32_hw_based/seq_02.sh create mode 100755 extras/tests/esp32_hw_based/seq_03.sh create mode 100755 extras/tests/esp32_hw_based/seq_04.sh create mode 100755 extras/tests/esp32_hw_based/seq_05.sh create mode 100755 extras/tests/esp32_hw_based/seq_06.sh create mode 100755 extras/tests/esp32_hw_based/seq_07a.sh create mode 100755 extras/tests/esp32_hw_based/seq_07b.sh create mode 100755 extras/tests/esp32_hw_based/seq_07c.sh create mode 100755 extras/tests/esp32_hw_based/seq_xxx.sh.off create mode 100644 extras/tests/esp32_hw_based/test_all.sh create mode 100644 extras/tests/pc_based/Makefile create mode 100644 extras/tests/pc_based/PMF_test.ino create mode 100644 extras/tests/pc_based/README.txt create mode 100644 extras/tests/pc_based/RampChecker.h create mode 100644 extras/tests/pc_based/off_test_15.cpp create mode 100644 extras/tests/pc_based/pmf_test.cpp create mode 100644 extras/tests/pc_based/rmc_test.cpp create mode 100644 extras/tests/pc_based/stubs.h create mode 100644 extras/tests/pc_based/test_01.cpp create mode 100644 extras/tests/pc_based/test_02.cpp create mode 100644 extras/tests/pc_based/test_03.h create mode 100644 extras/tests/pc_based/test_04.cpp create mode 100644 extras/tests/pc_based/test_05.cpp create mode 100644 extras/tests/pc_based/test_06.cpp create mode 100644 extras/tests/pc_based/test_07.cpp create mode 100644 extras/tests/pc_based/test_08.cpp create mode 100644 extras/tests/pc_based/test_09.cpp create mode 100644 extras/tests/pc_based/test_10.cpp create mode 100644 extras/tests/pc_based/test_11.cpp create mode 100644 extras/tests/pc_based/test_12.cpp create mode 100644 extras/tests/pc_based/test_13.cpp create mode 100644 extras/tests/pc_based/test_14.cpp create mode 100644 extras/tests/pc_based/test_15.cpp create mode 100644 extras/tests/simavr_based/.gitignore create mode 100644 extras/tests/simavr_based/Makefile create mode 100644 extras/tests/simavr_based/Makefile.test create mode 100644 extras/tests/simavr_based/Raw/platformio.ini create mode 100644 extras/tests/simavr_based/eval.awk create mode 100644 extras/tests/simavr_based/judge.awk create mode 100644 extras/tests/simavr_based/judge_pos0.awk create mode 100644 extras/tests/simavr_based/off_test_seq_03/platformio.ini create mode 100644 extras/tests/simavr_based/off_test_seq_04/platformio.ini create mode 100644 extras/tests/simavr_based/off_test_seq_05/platformio.ini create mode 100644 extras/tests/simavr_based/off_test_timing/expect.txt create mode 100644 extras/tests/simavr_based/off_test_timing/platformio.ini create mode 100644 extras/tests/simavr_based/off_test_timing/src/main.ino create mode 100644 extras/tests/simavr_based/test_externalCall/.gitignore create mode 100644 extras/tests/simavr_based/test_externalCall/Makefile create mode 100644 extras/tests/simavr_based/test_externalCall/expect.txt create mode 100644 extras/tests/simavr_based/test_externalCall/platformio.ini create mode 100644 extras/tests/simavr_based/test_issue150/.gitignore create mode 100644 extras/tests/simavr_based/test_issue150/Makefile create mode 100644 extras/tests/simavr_based/test_issue150/expect.txt create mode 100644 extras/tests/simavr_based/test_issue150/platformio.ini create mode 100644 extras/tests/simavr_based/test_issue151/.gitignore create mode 100644 extras/tests/simavr_based/test_issue151/Makefile create mode 100644 extras/tests/simavr_based/test_issue151/expect.txt create mode 100644 extras/tests/simavr_based/test_issue151/platformio.ini create mode 100644 extras/tests/simavr_based/test_issue152/.gitignore create mode 100644 extras/tests/simavr_based/test_issue152/Makefile create mode 100644 extras/tests/simavr_based/test_issue152/expect.txt create mode 100644 extras/tests/simavr_based/test_issue152/platformio.ini create mode 100644 extras/tests/simavr_based/test_issue172/.gitignore create mode 100644 extras/tests/simavr_based/test_issue172/Makefile create mode 100644 extras/tests/simavr_based/test_issue172/expect.txt create mode 100644 extras/tests/simavr_based/test_issue172/platformio.ini create mode 100644 extras/tests/simavr_based/test_issue173/.gitignore create mode 100644 extras/tests/simavr_based/test_issue173/Makefile create mode 100644 extras/tests/simavr_based/test_issue173/expect.txt create mode 100644 extras/tests/simavr_based/test_issue173/platformio.ini 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 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 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 create mode 100644 extras/tests/simavr_based/test_issue280/.gitignore create mode 100644 extras/tests/simavr_based/test_issue280/Makefile create mode 100644 extras/tests/simavr_based/test_issue280/expect.txt create mode 100644 extras/tests/simavr_based/test_issue280/platformio.ini create mode 100644 extras/tests/simavr_based/test_pmf/.gitignore create mode 100644 extras/tests/simavr_based/test_pmf/Makefile create mode 100644 extras/tests/simavr_based/test_pmf/expect.txt create mode 100644 extras/tests/simavr_based/test_pmf/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01a_2560t1/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01a_2560t1/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01a_2560t3/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01a_2560t3/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01a_2560t4/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01a_2560t4/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01a_2560t5/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01a_2560t5/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01a_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01a_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01a_leonardo/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01a_leonardo/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01b_2560t1/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01b_2560t1/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01b_2560t3/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01b_2560t3/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01b_2560t4/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01b_2560t4/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01b_2560t5/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01b_2560t5/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01b_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01b_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01c_2560t1/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01c_2560t1/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01c_2560t3/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01c_2560t3/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01c_2560t4/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01c_2560t4/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_01c_2560t5/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_01c_2560t5/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_02_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_02_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_03_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_03_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_04_timing_2560/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_04_timing_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_04_timing_328p_37k/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_05_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_05_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_06_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_06_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_07_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_07_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_08_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_08_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_09_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_09_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_10_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_10_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_11_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_11_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_12_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_12_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_13_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_13_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_14_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_14_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_15_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_15_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_16_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_16_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_17_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_17_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_18_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_18_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_sd_19_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_sd_19_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_seq_sd_01_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_seq_sd_01_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_seq_sd_02_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_seq_sd_02_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_seq_sd_06_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_seq_sd_06_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_seq_sd_07_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_seq_sd_07_328p/platformio.ini create mode 100644 extras/tests/simavr_based/test_seq_sd_11_328p/expect.txt create mode 100644 extras/tests/simavr_based/test_seq_sd_11_328p/platformio.ini create mode 100644 keywords.txt create mode 100644 shell.nix create mode 100644 src/AVRStepperPins.h create mode 100644 src/FastAccelStepper.cpp create mode 100644 src/FastAccelStepper.h create mode 100644 src/FastAccelStepper_idf4_esp32_pcnt.cpp create mode 100644 src/FastAccelStepper_idf5_esp32_pcnt.cpp create mode 100644 src/PoorManFloat.cpp create mode 100644 src/PoorManFloat.h create mode 100644 src/PoorManFloatConst.h create mode 100644 src/RampCalculator.cpp create mode 100644 src/RampCalculator.h create mode 100644 src/RampConstAcceleration.cpp create mode 100644 src/RampConstAcceleration.h create mode 100644 src/RampGenerator.cpp create mode 100644 src/RampGenerator.h create mode 100644 src/StepperISR.cpp create mode 100644 src/StepperISR.h create mode 100644 src/StepperISR_avr.cpp create mode 100644 src/StepperISR_due.cpp create mode 100644 src/StepperISR_esp32.cpp create mode 100644 src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp create mode 100644 src/StepperISR_idf4_esp32_rmt.cpp create mode 100644 src/StepperISR_idf4_esp32c3_rmt.cpp create mode 100644 src/StepperISR_idf4_esp32s3_rmt.cpp create mode 100644 src/StepperISR_idf5_esp32_rmt.cpp create mode 100644 src/fas_arch/arduino_avr.h create mode 100644 src/fas_arch/arduino_esp32.h create mode 100644 src/fas_arch/arduino_sam.h create mode 100644 src/fas_arch/common.h create mode 100644 src/fas_arch/common_esp32.h create mode 100644 src/fas_arch/common_esp32_idf4.h create mode 100644 src/fas_arch/common_esp32_idf5.h create mode 100644 src/fas_arch/espidf_esp32.h create mode 100644 src/fas_arch/test_pc.h create mode 100644 src/fas_arch/test_probe.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..54afbaa --- /dev/null +++ b/.gitignore @@ -0,0 +1,38 @@ +*.swp +*.o +*.py +.pio +.dir +.tested +run_avr +pio_dirs +pio_espidf +PCB/ +examples/*/*.h +examples/*/*.cpp +!examples/*/test_*.cpp +extras/tests/pc_based/test_[0-9][0-9] +extras/tests/pc_based/FastAccelStepper.cpp +extras/tests/pc_based/FastAccelStepper.h +extras/tests/pc_based/PoorManFloat.cpp +extras/tests/pc_based/PoorManFloat.h +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/fas_common.h +extras/tests/pc_based/*.gnuplot +extras/tests/pc_based/*_test +extras/tests/pc_based/test.log +extras/tests/pc_based/*.png +extras/tests/simavr_based/*/result.txt +extras/tests/simavr_based/*/x.vcd +extras/tests/simavr_based/test*sd*/src +extras/tests/simavr_based/simavr +extras/tests/simavr_based/test_sd*/Makefile +extras/tests/simavr_based/test_seq*/Makefile +extras/tests/esp32_hw_based/*.log +library.properties +examples +extras/gen_pmf_const/PoorManFloat.cpp +extras/gen_pmf_const/main diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..2c2004c --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,667 @@ +TODO: +- Different behavior avr vs pc-based testgis to be analyzed +- #include-file structure needs to be cleaned up +- rename PoorManFloat to e.g. Log2Representation +- rename RampConstAcceleration to e.g. RampControl +- for esp-idf 5 make use of espressif resource management of rmt channels + +0.31.2: +- Move constants out of PoorManFloat.h and autogenerate these +- Fix pmfl constant used by SAM Due aka 21MHz +- Update doc in PoorManFloat.cpp + +0.31.1: +- Fix for issue #280: stopMove() is interrupted if followed by update of speed/acceleration +- Related to issue #280: `isStopping()` did not work as expected and this should be fixed, too. + +0.31.0: +- Fix missing parenthesis in preprocessor macro (#271) +- Position parameter for `forceStopAndNewPosition()` changed from `uint32_t` to `int32_t` (#268) +- Add `stepsToStop()` to predict motor stop position +- compatibility with esp-idf 5.3.0 (not working with 5.0.x to 5.2.x) + `pio run -e esp32_idf_V6_8_1` in `pio_espidf/UsageExample` works. +- Drop support for esp-idf 3.x.y versions +- Add github build tests for esp32c6 with espidf5 +- StepperDemo can be compiled with esp-idf4/5 without arduino +- idf5 version yields mismatch with pulse counter. not ready for use +- esp32: dirPin toggle in ISR should not cause cache error due to use of `gpio_ll` routines + +0.30.15: +- Fix missing initialization in `getCurrentSpeedInTicks()` (#262) + +0.30.14: +- Fix typo in library.properties. No functional changes + +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()` +- 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. + +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 + of uncontrolled write to I/O-region +- rename `common.h` to `fas_common.h` as proposed in #220 + +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 + +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. + +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 +- 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 + +0.30.3: +- esp32s2: Add an explicit start for rmt module, which is for esp32 not needed + +0.30.2: +- Fix compile error for esp32 arduino V3.4.0 + +0.30.1: +- Fix compile error for esp32s2/s3 in rmt module + +0.30.0: +- fix issue #172: `move()` during ramping down of `stopMove()` has used old target position as reference +- fix issue #173: After `forceStopWithNewPosition()` next `move()` was not executed properly +- fix issue #173: After `forceStop()` target position was not updated +- Fix: With high acceleration and jump start value, the speed could be too high for the motor. Now is limited +- hot fix for issue #174: esp32-rmt module has not changed direction under high load +- esp32s2: Enable pulse counter support +- remove ramp state `RAMP_STATE_DECELERATE_TO_STOP` +- Fix for command errors at very high acceleration as detected by issue #174 + +0.29.2: +- add `stepperConnectToPin(pin, driver_type)` to allow the module selection +- fix issue #170 and use consequently `fas_max()` instead of `max()` +- add `setJumpStart(jump_step)` to start the ramp with higher speed +- StepperDemo: Add j command to set steps for jump start + +0.29.1: +- improve rounding for log2/pow2 conversion +- avr: reduce max speed for single stepper use down to 50kSteps/s from 70kSteps/s +- implement linear acceleration from/to standstill - configurable by setLinearAcceleration() +- StepperDemo: Add J command to set linear acceleration + +0.29.0: +- replace former PoorManFloat implementation `ump_float` being exponent+mantissa with `pmfl_logarithmic` as log2 representation +- avr: reduce max speed for 3 steppers to 20kSteps/s from 25kSteps/s +- The tests `simavr_based/test_sd_12_328p` and`simavr_based/test_sd_14_328p` yield significant different results than before. + Current assumption is, that the new behavior is correct in regard to acceleration/deceleration. + +0.28.4: +- include esp32s3 port + +0.28.3: +- reduce AVR max single stepper speed to 70kSteps/s due to issue [Issue #152](https://github.com/gin66/FastAccelStepper/issues/152) + +0.28.2: +- fix for issue [Issue #150](https://github.com/gin66/FastAccelStepper/issues/150) for low speed not reducing below startup speed + +0.28.1: +- try to fix [Issue #142](https://github.com/gin66/FastAccelStepper/issues/142) for esp32 mcpwm/pcnt and format code + +0.28.0: +- esp32: fix for compile issue on arduino 1.8.19 as reported by [Issue #127](https://github.com/gin66/FastAccelStepper/issues/127) +- remove deprecated isMotorRunning() +- external callback for pins are now defined on the engine level. This allows generalization for enable and direction pins for esp32 and avr (not atmega32u4). This will break existing code, which has used external enable pin. + +0.27.5: +- esp32: fix for compile issue on arduino 1.8.19 as reported by [Issue #117](https://github.com/gin66/FastAccelStepper/issues/117) + +0.27.4: +- esp32s2: fix not moving stepper after forcestop as reported by [Issue #101](https://github.com/gin66/FastAccelStepper/issues/101) + +0.27.3: +- esp32/esp32s2: enable all 8/4 rmt backed steppers + +0.27.2: +- esp32/esp32s2: fix compilation error of 0.27.1 + +0.27.1: +- esp32/esp32s2: disable test probes + +0.27.0: +- esp32: StepperDemo make p command work again +- esp32: check step pin is a valid gpio output pin +- esp32: Support rmt device with two steppers. so in total 8 steppers for now +- esp32s2: experimental support +- new return code for `addQueueEntry()`: `AQE_DEVICE_NOT_READY` +- `move()` and `moveTo()` expect an optional parameter to opt for blocking operation +- add more `ìnline` in FastAccelStepper.h for short routines to be inlined +- StepperDemo: fix output of usage config + +0.26.1: +- after forceStop, the first command was lost + +0.26.0: +- avr: Max stepper speed depending on number of assigned steppers: + 1 stepper => ~76 kHz + 2 steppers => ~37 kHz + 3 steppers => ~25 kHz +- API: add getMaxSpeedIn...() functions +- Remove `MIN_DELTA_TICKS` constant + +0.25.7: +- atmega32u4: Disable direct port access function in StepperDemo to free up program memory + +0.25.6: +- Internal refactoring: separate RampGenerator and Constant Acceleration Ramp Calculation +- StepperDemo extended: + - h: Set the speed in Steps/1000s +- Fix an issue, which has appeared on simavr with overshooting ramp end (test seq 06). + Actually only the overshoot is avoided, the different behavior avr vs pc-based tests to be analyzed + +0.25.5: +- esp32: use busy wait in forwardStep()/backwardStep() instead of yield() as proposed by [Issue #115](https://github.com/gin55/FastAccelStepper/issues/115) +- remove blockingWait for forceStop() in move/moveTo/... as this is actually not required. + +0.25.4: +- Add new function forceStop() as solution for [Issue #116](https://github.com/gin66/FastAccelStepper/issues/116) +- esp32: use yield() instead of vTaskDelay(1) as proposed by [Issue #115](https://github.com/gin55/FastAccelStepper/issues/115) + +0.25.3: +- esp32: fix [Issue #113](https://github.com/gin66/FastAccelStepper/issues/113): + Stepper can fail to start as regression introduced in 0.25.0 and present up to 0.25.2 +- sam: fix [Issue #111](https://github.com/gin66/FastAccelStepper/issues/111): + Stepper can fail to start as regression introduced in 0.25.0 and present up to 0.25.2 + +0.25.2: +- Fix sketch name of RawOneTurn example +- Fix overshooting due to clipping error +- Minor reverse sqrt-table rework + +0.25.1: +- expose internal functions for retrieving queue status as requested by [Issue #112](https://github.com/gin66/FastAccelStepper/issues/112): + ticksInQueue() + hasTicksInQueue() + queueEntries() +- SAM: StepperDemo output function was not defined for this architecture +- Fix for compilation error [Issue #113](https://github.com/gin66/FastAccelStepper/issues/113) + eventually name clash for min()/max()-definition. Renamed own version to fas_min/fas_max. + +0.25.0: +- esp32 changes in response to [Issue #106](https://github.com/gin66/FastAccelStepper/issues/106): + - Select CPU core to run the StepperTask on by using init(cpu) + - Trigger the watchdog in the StepperTask + - Ensure that vDelayTask is not called with zero value, + which could be the case for portTICK_PERIOD_MS > 5 +- SAM Due: fix issue that call to isRunning() after forceStopAndNewPosition() has returned true + [Issue #111](https://github.com/gin66/FastAccelStepper/issues/111). +- Rework/simplify internal implementation of queue start for the different architectures. +- ensure forceStopAndNewPosition() is really safe to be called from interrupt +- Refactor internal code for less microcontroller dependency +- isMotorRunning() declared as deprecated and add isQueueRunning() as replacement + +0.24.2: +- arduino library manager has not taken up the last minute change + => bump version witout change + +0.24.1: +- Add support for atmega32u4, leonardo +- Reorganized StepperDemo to fit in smaller device like atmega32u4 +- support for espidf4.4 for arduino +- experimental (broken ?) support for espidf4.4 + +0.24.0: +- Implement configurable delay after direction pin change and first step in new direction + [Issue #83](https://github.com/gin66/FastAccelStepper/issues/83). + The value is set by the third parameter of setDirectionPin(). +- Add table of timing values in FastAccelStepper.h for the architectures +- StepperDemo extended: + - add config mode (press c) for direction pin configuration +- Support changed mpcwpm-definitions in esp-idf v4.4 +- Fix warning identified in this [Issue #98](https://github.com/gin66/FastAccelStepper/issues/98) for `fas_abs()` usage + + +0.23.5: +- Add missing initialization of dir pin to be `PIN_UNDEFINED`. + With two or more steppers without dir pin, the steppers have run only sequential and not in parallel. + +0.23.4: +- Add support for SAM Due (https://github.com/gin66/FastAccelStepper/pull/82) + +0.23.3: +- Add CMakeLists.txt for ESP-IDF (https://github.com/gin66/FastAccelStepper/pull/81) +- avr: use reentrant version for disable/enable interrupts [Issue #75](https://github.com/gin66/FastAccelStepper/issues/75) + +0.23.2: +- StepperDemo extended: + - add test sequence 11 for issue #68 +- Fix issue #68: getCurrentPosition() could be off by one command's step amount + +0.23.1: +- get actual speed from queue if current and next command has at least one step +- improve accuracy for setSpeedInHz() and setSpeedInMilliHz() and use rounding in addition (issue #56) +- Fix issue #67: If the speed changes are below the minimum speed after one step, then the speed will not change + +0.23.0: +- getRampState(): Add two flags for current direction +- add function: getCurrentAcceleration() +- `setAcceleration(uint32_t)` changed to `setAcceleration(int32_t)`. Only positive values allowed. + This way getCurrentAcceleration() can return negative values without range problems +- add function: getCurrentSpeedInUs(), getCurrentSpeedInMilliHz(), getSpeedInMilliHz() +- restructure tests +- StepperDemo extended: + - esp32 only: Add command reset, which causes a watchdog reset + - auto switch between speed in milliSteps/s and us/step depending on command H or V + +0.22.2: +- esp32: getCurrentPosition() does take current command's pulse count into consideration +- fix issue #57: counter clear needs a strobe on a bit and not only being set + + +0.22.1: +- disableOutputs() will return false, if called on a running motor with + autoEnable set to true +- esp32: replace `pcnt_counter_clear()` by preprocessor-directive in relation to issue #55 +- esp32: put const table into RAM due to issue #55 + +0.22.0: +- Internal code clean up/polishing +- replace getPeriodAfterCommandsCompleted() by + getPeriodInUsAfterCommandsCompleted() and getPeriodInTicksAfterCommandsCompleted() +- StepperDemo outputs for a running motor the period in us and in ticks + +0.21.4: +- PoorManFloat changed from 8bit to 9bit mantissa (implicit msb=1) + => speed steps with acceleration=1 hardly noticeable +- PoorManFloat new functions for reciprocal calculations + +0.21.3: +- Unidirectional mode: moveByAcceleration() with negative values has used previous acceleration till stop + +0.21.2: +- Two new API functions: setSpeedInTicks() and getSpeedInTicks() +- setSpeedInHz() and setSpeedInMilliHz() uses higher resolution than us +- esp32: the spare pulse counter attached to a stepper can now be configured and cleared. +- StepperDemo extended: + - esp32 only: Add command p,, to configure limits for attached pulse counter + - esp32 only: Add command pc to clear an attachedPulseCounter + - H: Set the speed in Steps/s +- Revert change in 0.21.0 for better resolution of calculate ramp speed => new more tests +- Fix bugs in clipping code, which has caused speed jumps +- Ensure no illegal command in coasting after acceleration change + +0.21.1: +- RampGenerator uses now the delayed start feature of the queue +- Fix invalid command generated due to upm calculation tolerance at higher speed + +0.21.0: +- StepperDemo extended with test sequence 10 for issue #44 +- avr: Avoid ClearInterruptFlag() in ISR, which is needed by simavr due to implementation bug +- eliminate need for FOC workaround by updated simavr +- Better resolution of calculate ramp speed to reduce steps of constant speed while accelerating or decelerating. + Cost is slower execution of ramp generation. +- Fix a minor bug, that while deleration to a slower set speed, the deceleration could overshoot, + which requires short acceleration +- StepperDemo extended: + - command u to put selected stepper into unidirectional mode (need reset to restore) +- Fix for issue #47: Restart in unidirectional mode and position count down +- addQueueEntry returns `AQE_ERROR_NO_DIR_PIN_TO_TOGGLE`, if command defines count down and direction pin is undefined + +0.20.2: +- Fix for issue #45: if enablePinHighActive has been set to `PIN_UNDEFINED`, + the externalEnableCall was cleared even if enablePinLowActive still in use. +- Bugfix for issue #46: Avoid creation of invalid command in ramp generator while decelerating + +0.20.1: +- few minor speed ups +- StepperDemo extended: + - avr only: command 'r' toggles erroneous digitalRead() to stepperpin + - command 'e' toggles interrupt block of ~100us + +0.20.0: +- rename setSpeed to setSpeedInUs +- add setSpeedInHz() and setSpeedInMilliHz() +- RampGenerator code reworked and simplified + +0.19.0: +- avoid overflow in setSpeed for super slow speed +- setSpeed and setAcceleration return status code +- moveByAcceleration(): check if direction pin is defined for reverse +- add getter functions: getSpeedInUS(), getAcceleration() +- StepperDemo extended: + - move/moveTo/moveByAcceleration => verbose error code + - setSpeed/setAcceleration => report invalid data error + - motor info with speed/acceleration + - less verbose: if any motor is running, only info for running motor +- fix issue #43: Issue on moveTo() to exactly same target after a completion of stopMove() + + +0.18.13: +- Add fix for issue #41: High acceleration and high speed + +0.18.12: +- Issue #40 fixed: + - moveTo() has started ramp generator, even if on target position + - avoid recalculation of ramp on setAcceleration, if unchanged value + +0.18.11: +- Remove obsolete `queue_end` variables ticks and `ticks_from_last_step` +- Fix for issue #33: pulse counter needed to be cleared at motor start + +0.18.10: +- Reapply modified fix, which was reverted in 0.18.9 + +0.18.9: +- Fixed esp32 high speed step loss: partial reverted this fix for now + +0.18.8: +- New API-function: isStopping() +- Fixed esp32 high speed step loss + +0.18.7 +- Fix compile error on esp32 + +0.18.6 +- Running forward/backward could start with previous speed in + opposite direction due to two missing initialization, which are + present for move/moveTo + +0.18.5 +- replace 16bit division with `upm_float` division +- ramp generator packs per command steps for 2 ms or more. + Before this was 1ms. This change makes huge difference on Atmega2560 + +0.18.4: +- avr: Step ISR code optimization e.g. get rid of digitalRead/Write +- esp32: get rid of digitalRead/Write, too +- replave 32bit division with 16bit => much better timing on avr + +0.18.3: +- esp32: extend API to attach free pulse counter for debugging +- StepperDemo extended for esp32: + - p: Attach pulse counter n to the selected stepper + +0.18.2: +- extend addQueueEntry() with a start flag. This allows to first fill the queue + and then start the queue. If several motor are started one after the other + (with interrupts disabled), a nearly synchronous start is possible. + Which is required for coordinated axis movement +- make use of this in enqueueing commands from ramp generator + +0.18.1: +- `moveByAcceleration()` returns `int8_t` result code +- `setDelayToEnable()` returns an `int8_t` instead of int + +0.18.0: +- StepperDemo extended: + - w: Option to wait in input processing for some ms + - test sequence 07 added, which fails on esp32 + - avr: free RAM by usage of `output_msg()` + - tests can return failure, which is displayed at test end +- Rework RampGenerator: pauses are now after the step and not before. This removes + some irregularity between steps. +- Rework esp32 interrupt code. +- Fix issue #29 and a slow start of a ramp (identified by #29) +- RampGenerator: Support coasting at lower speed for a couple of steps + to limit interrupt rate for esp32 (or even rejected commands) +- More tests +- esp32: forceStopAndNewPosition() has not emptied the queue +- fixed: In reversing at max speed an abrupt stop could be initiated +- update FastAccelStepper.h: stopMove() does not make new acceleration value valid +- protect non application fields, which are for test purposes + +0.17.1: +- esp32: Fixed one/two spurious step pulses after reset mainly (issue #29) +- avr: reworked ISR for more reliable operation (correct steps). (issue #31) + Remaining risk (primarily three steppers in parallel in 2560) can cause + an CPU overload and stepper stopping/not running smooth +- Fix issue #32 +- Remove interrupts/noInterrupts protection for shared auto enable function. + => Thus there is a small time window during auto disable, that a new command issued + may loose steps. This will be addressed in a subsequent release + +0.17.0: +- avr: stepper definition support via AVRStepperPins.h +- 2560: all timers and channels are checked with one test +- addQueueEntry(): remove test for steps >= 128 +- new function moveByAcceleration() to control stepper speed by positive and + negative acceleration values. +- stopMove() will not run to stop, if any move command is called afterwards. + The error code `MOVE_ERR_STOP_ONGOING` has been removed. +- StepperDemo extended: + - a: Allows to control the stepper speed by positive/negative acceleration values + +0.16.8: +- Fix for issue #30: for dirPin undefined, digitalWrite/pinMode was called + +0.16.7: +- Avoid unnecessary direction pin toggle on direction change after motor stop +- Fix `MAX_STEPPER` for atmega2560 +- Extend simavr tests for atmega2560 and one test for each timer for both varians + +0.16.6: +- bugfix in setEnablePin(): if called with `PIN_UNDEFINED`, that value was used with pinMode and digitalWrite() +- Use simavr to perform regression test for avr on HW-simulation +- Reworked the avr ISR code to avoid steps being lost, if the command queue is running out of commands and a new command comes in with steps to be generated +- Extended the planning ahead time to 20ms.... possibly the load for avr is bit high !? + +0.16.5: +- esp32: forwardStep()/backwardStep() works again +- Fix for issue #25: RampGenerator was irritated, if interrupt has processed command too fast. +- Rework definition of isRunning() for esp32 to make it more like avr + +0.16.4: +- Example code for platformio (built with ci/build-platformio.sh) references the library code + via platformio.ini option and not via symbolic links. +- StepperDemo test mode extended: + - x: Allows to exit test mode + - I: Allows to toggle motor info while test sequence is running + - New test sequence 06 due to issue #24 +- Fix for issue #24: unplanned stop due to speed changes during ramp down + +0.16.3: +- Add another test case for speed reduction, while running (see issue #23) + This has identified a bug in the ramp generator impacting esp32/avr. This bug is now fixed. + +0.16.2: +- Add comments to example RawAccessWithPause (renamed from RawAccessWithDelay) +- addQueueEntry() expects now a const pointer instead of just a pointer +- Rework RawAccess example and test on hw + +0.16.1: +- Usage example in readme has not worked due to a bug in the auto enable/disable variable setting. + => Added the UsageExample under examples + +0.16.0: +- disableOutputs() returns boolean success value +- The external enable output routine will be called regularly in case of disable, too +- esp32: fixed loss of pauses in command queue due to pcnt/mcpwm timer interaction. + This caused V4100 being faster than V4000, even so it should be slower +- ramp generator: fix for high accelerations to have stop command effective + +0.15.2: +- Assume for now, that atmega328 and ATmega2560 uses always same pins for OC1A and OC1B + +0.15.1: +- Remove the check for specific avr board. This assumes, that all avr boards + uses Pin 9 and Pin 10 for OC1A and OC1B respectively + +0.15.0: +- For the commands in the queue, the minimum time in ticks to execute + a command is limited to 10 * `MIN_DELTA_TICKS`. For esp32 this relates to + the time between interrupts of one channel +- `AQE_ERROR_TICKS_TOO_HIGH` removed, because it is already limited by the data type `uint16_t` +- esp32: Change from mcpwm up count mode to up-down count mode +- avr: adjust implementation to esp32: pulse at start of ticks period of a command +- merge pull request from ixil see (https://github.com/gin66/FastAccelStepper/pull/19) + +0.14.0: +- Direction pins can be shared by several steppers +- enableOutputs() returns bool to indicate, output was enabled +- Possibility to supply external enable output control +- AddQueueEntry() return values changed: >0 => retry again, <0 => error +- ATmega2560: Allow to change the used timer module with preprocesser variable `FAS_TIMER_MODULE` + +0.13.4: +- Automated github test identified a compile error introduced in 0.13.3 + +0.13.3: +- esp32: Cyclic rate increased from 10ms to 4ms. + With a planning ahead time of 10ms, there was the risk + of running out of commands as identified in issue #18 + +0.13.2: +- StepperDemo: Compact output for stopped motor +- Fixed a bug, where stepper 4-6 misconfigured stepper 1-3 pcnt. + => stepper 4-6 had erroneous behavior for speed <500us + => stepper 1-3 could in trouble, if corresponding stepper 4-6 was running + +0.13.1: +- try a mechanism to include application defined config file +- StepperDemo: for avr move messages into program code to reduce RAM usage + +0.13.0: +- Support ATmega2560 for three steppers linked to timer 4 (currently hardcoded) + +0.12.2: +- StepperDemo modification: + - Enable direct drive feature in StepperDemo for esp32 + - While direct driving, check if the signals can be applied + - Add test mode (enter with t). Here can select stepper, a test sequence and run it + - In total four test sequences implemented until now +- Add detachFromPin() and reattachToPin() to the API. Shouldn't be used from an application. + +0.12.1: +- implement runForward()/runBackward() +- avr: fix interrupt for direction change +- StepperDemo modification: + - r: Call ESP.restart() to check for issue #6 + +0.12.0: +- reduce data type for command queue entries' ticks value from `uint32_t` to `uint16_t` + => remove `ABS_MAX_AQE_TICKS` +- each command in queue can now emit up to 255 steps +- StepperDemo modification: + - r: Call ESP.restart() to check for issue #6 + - Disable direct drive for esp32 +- AutoEnable-Pin can be shared by steppers +- avr: fix interrupt for direction change + +0.11.3: +- `ABSOLUTE_MAX_AQE_TICKS` is now 65535 + +0.11.2: +- auto enable on delay implemented by filling the queue with pause + This allows approx. 60/120 ms delay for avr/esp32 +- esp32: two motors in parallel could lead to uncontrolled running steppers. + Reason was the wrong registration of the shared interrupt service routine + +0.11.1: +- AVR works again +- Limit auto enable on delay to approx. 16ms due to further bugs + +0.11.0: +- BROKEN ON AVR +- Slowest speed is `TICKS_PER_S`/0xffffffff, which is ~268s between steps +- `ABS_MAX_TICKS` renamed to `ABS_MAX_AQE_TICKS`. Only applicable to raw commands +- Done: Extend command queue entry to perform delay only without step (steps=0) to reduce the 1.0 steps/s + +0.10.0: +- setSpeed() silently imposes lower limit for period +- esp32: step pulse length is for high speed with 50% duty cycle and for low speed fixed at 2ms +- addQueueEntry() receives a `stepper_command_s` struct +- esp32: Task priority of ramp generator task has been set to max Priority. +- StepperDemo extended: + - Q: Quiet the usage info, which takes time to be transmitted. + Try this NEMA-17 without load: + M1 A1000000 V20 P1000 W P0 W P500 W P-500 W P0 + +0.9.5: +- Fix sudden CPU reset on high interrupt load for avr variant. Issue #12 + +0.9.4: +- Fix possible race condition in `check_for_auto_disable()` +- StepperDemo extended: + - blocking wait for stepper stop by press W (dangerous: can deadlock) + +0.9.3: +- Fix auto on delay: Delay way always replied, even if the output is still enabled + +0.9.2: +- Implement new function applySpeedAcceleration() +- StepperDemo extended: + - trigger applySpeedAcceleration by press U +- fix possible bug in move/moveTo while keepRunning is set + +0.9.1: +- reduce interrupt load on esp32 + +0.9.0: +- implement forceStopAndNewPosition() +- StepperDemo extended: + - trigger forceStopAndNewPosition by press X + - set position with press @ + - keep motor running with press K +- addQueueEntry() returns now an `int8_t` instead of an int +- move and moveTo goes to the closest position (+/-2147483647). + This means continues move(1000) will let the stepper turn in same direction, + while the position wraps around: 0,1,...,2147483647,-2147483648,-2147483647,...,-1,0,1,... +- add keepRunning() to let the motor continuously run in same direction. +- rename isrSpeedControlEnabled() to isRampGeneratorActive() + +0.8.3: +- AVR: timer compare interrupts are only enabled, if stepper is running. +- AVR: on arrival of a command, the queue is started with few µs delay +- Implement backwardStep() and forwardStep() +- Bug solved: Speed changes at very low speed with high acceleration values are not always performed + Actually speed with period times > 268436µs has not worked at all before. + +0.8.2: +- Solved issue: Queue is filled too much, which cause slow response to speed/acceleration changes + => Queue is filled to max ~10ms into the future. + +0.8.1: +- Fix issue #8: Long step times are less accurate than short ones + => All time delta between steps are cycle accurate +- Add getPeriodAfterCommandsCompleted() to API +- Fix bug due to AutoEnable at ramp start (can find at low speeds) +- StepperDemo outputs: `F_CPU`/`TICKS_PER_S` and stepper period at queue end + +0.8.0: +- Change direction with running motor is possible !!! +- stopMove() can be called from interrupt routine +- Refactor ramp generation code into RampGenerator.h/cpp +- StepperDemo: ramp state is written as plain text +- Mention platformio in README + +0.7.1: +- StepperDemo extended with commands to + return status code from move/moveTo + toggle motor info (I) to suppress info while steppers are running + output usage (?) + output motor info with usage + test direct drive of stepper by port manipulation bypassing the library (T) +- move/moveTo return error codes + +0.7.0: Changes towards 0.6.15 +- Fix possible floating point exception (divide by zero), which could happen rarely in `isr_single_fill_queue` +- Remove deprecated functions: + addQueueStepperStop() + isStopped() +- internal: remove obsolete `_stepper_num variable` diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..83f0c13 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,6 @@ +idf_component_register( + SRC_DIRS "src" + INCLUDE_DIRS "src" + REQUIRES arduino + PRIV_REQUIRES driver soc +) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..a0c294f --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2020 J.Kiemes + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..d55df85 --- /dev/null +++ b/README.md @@ -0,0 +1,512 @@ +# 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 + +[![arduino-library-badge](https://www.ardu-badge.com/badge/FastAccelStepper.svg?)](https://www.ardu-badge.com/FastAccelStepper) + + +# FastAccelStepper +![GitHub tag](https://img.shields.io/github/v/tag/gin66/FastAccelStepper.svg?sort=semver&no_cache_0.28.1) +[![PlatformIO Registry](https://badges.registry.platformio.org/packages/gin66/library/FastAccelStepper.svg)](https://registry.platformio.org/libraries/gin66/FastAccelStepper) +[![arduino-library-badge](https://www.ardu-badge.com/badge/FastAccelStepper.svg?)](https://www.ardu-badge.com/FastAccelStepper) + +![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) + +## Matrix build for arduino using platformio (esp32, esp32c3, atmega328,...) +[![Build examples](https://github.com/gin66/FastAccelStepper/actions/workflows/build_arduino_examples_matrix.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_arduino_examples_matrix.yml) + +## Matrix build for espidf using platformio +[![Build espidf](https://github.com/gin66/FastAccelStepper/actions/workflows/build_idf_examples_matrix.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_idf_examples_matrix.yml) + +## Build for esp32 with tasmota +[![`V2_0_15`](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_tasmota_2_0_15.yml/badge.svg)](https://github.com/gin66/FastAccelStepper/actions/workflows/build_examples_esp32_tasmota_2_0_15.yml) + +## Arduino and esp32 + +Arduino core v3.0.x are using esp-idf v5.0 up to v5.1 and FastAccelStepper will fail to compile. + +Arduino core 3.1.0 will support ESP-IDF V5.3.0 (based on RC1) + +## 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, esp32c6 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 + - avr atmega168/328/p: only Pin 9 and 10. + - avr atmega32u4: only Pin 9, 10 and 11. + - avr atmega2560: only Pin 6, 7 and 8. + On platformio, this can be changed to other triples: 11/12/13 Timer 1, 5/2/3 Timer 3 or 46/45/44 Timer 5 with FAS_TIMER_MODULE setting. + - esp32: This can be any output capable port pin. + - atmel sam due: This can be one of each group of pins: 34/67/74/35, 17/36/72/37/42, 40/64/69/41, 9, 8/44, 7/45, 6 + - Step should be done on transition Low to High. High time will be only a few us. + On esp32 the high time is for slow speed fixed to ~2ms and high speed to 50% duty cycle +* Direction Signal (optional) + - This can be any output capable port pin. + - Position counting up on direction pin high or low, as per optional parameter to setDirectionPin(). Default is high. + - With external callback on esp32 derivates, even shift register outputs can be used +* Enable Signal (optional) + - This can be any output capable port pin. + - Stepper will be enabled on pin high or low, as per optional parameter to setEnablePin(). Default is low. + - With external callback, even shift register outputs can be used + +FastAccelStepper offers the following features: +* 1-pin operation for e.g. peristaltic pump => only positive move +* 2-pin operation for e.g. axis control +* 3-pin operation to reduce power dissipation of driver/stepper +* Lower limit of 260s per step @ 16MHz aka one step every four minute (esp32/avr), 198s for sam due +* fully interrupt/task driven - no periodic function to be called from application loop +* supports acceleration and deceleration with per stepper max speed/acceleration +* Allows the motor to continuously run in the current direction until stopMove() is called. +* speed/acceleration can be varied while stepper is running (call to functions move or moveTo is needed in order to apply the new values) +* Constant acceleration control: In this mode the motor can be controled by acceleration values and with acceleration=0 will keep current speed +* Linear acceleration increase from/to standstill using cubic speed function - configurable by `setLinearAcceleration()` +* Jump start from standstill - configurable by `setJumpStart()` +* Auto enable mode: stepper motor is enabled before movement and disabled afterwards with configurable delays +* Enable pins can be shared between motors +* Direction pins can be shared between motors +* Configurable delay between direction change and following step +* External callback function can be used to drive the enable pins (e.g. connected to shift register) and, only esp32 derivates: the direction pins +* No float calculation (poor man float: use log2 representation in range -64..64 with 16bit integer representation and 1/512th resolution) +* 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 of Moves +* 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 +* For move() the delta is added to the latest desired end position +* The stepper tries to reach the given desired end position as fast as possible with adherence to acceleration/deceleration +* If the stepper is e.g. running towards position 1000 and moveTo(0) is called at position 500, then the stepper will + 1. decelerate, which means it will overshoot position 500 + 2. stop and accelerate towards 0 + 3. eventually coast for a while and then decelerate + 4. stop +* The stepper position is a 32bit integer variable, which wraps around for continuous movement. + Example: + - Assume counting up turns stepper clockwise, and counting down, anti-clockwise. + - Current position is -2.000.000.000, move to 2.000.000.000. + - Apparently the position has to count up, and count should run clockwise. + - Implementation is done via difference of 32bit signed numbers, which can overflow (being legit). + - The calculation is then: + 2.000.000.000 - (-2.000.000.000) = 4.000.000.000 + - But 4.000.000.000 interpreted as signed 32bit is -294.967.296 => count down, turn anti-clockwise + Means the position will count: +``` + -2.000.000.000 + -2.000.000.001 + -2.000.000.002 + : + -2.147.483.647 + -2.147.483.648 + 2.147.483.647 + 2.147.483.646 + 2.147.483.645 + : + 2.000.000.000 +``` +Comments to pin sharing: +* Enable pin sharing: the common pin will be enabled for as long as one motor is running + delay off. + Every motor will adhere to its auto enable delay, even if other motors already have enabled the pin. +* Direction pin sharing: The direction pin will be exclusively driven by one motor. If one motor is operating, another motor will wait until the direction pin comes available + +### AVR ATMega 168/168P/328/328P + +* allows up to 50000 generated steps per second for single stepper operation, 37000 for dual stepper +* supports up to two stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Uses `F_CPU` Macro for the relation tick value to time, so it should now not be limited to 16 MHz CPU frequency (untested) +* Steppers' command queue depth: 16 + +### AVR ATMega 32u4 + +* allows up to 50000 generated steps per second for single stepper operation, 37000 for dual stepper and 20000 for three steppers +* supports up to three stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Uses `F_CPU` Macro for the relation tick value to time, so it should now not be limited to 16 MHz CPU frequency (untested) +* Steppers' command queue depth: 16 + +### AVR ATMega 2560 + +* allows up to 50000 generated steps per second for single stepper operation, 37000 for dual stepper and 20000 for three steppers +* supports up to three stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Uses `F_CPU` Macro for the relation tick value to time, so it should now not be limited to 16 MHz CPU frequency (untested) +* Steppers' command queue depth: 16 +* This device has four 16 bit timers, so extension up to 12 steppers should be possible (not implemented) + +### ESP32 + +#### ESP-IDF version 4.x.y: +* allows up to 200000 generated steps per second +* supports up to 14 stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Steppers' command queue depth: 32 + +#### ESP-IDF version >=5.3.0: +* allows up to 200000 generated steps per second +* supports up to 8 stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Steppers' command queue depth: 32 + +### ESP32S2 + +* reported to work +* allows up to 200000 generated steps per second ? +* supports up to four stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Steppers' command queue depth: 32 + +### ESP32S3 + +#### ESP-IDF version 4.x.y: +* allows up to 200000 generated steps per second ? +* supports up to eight stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Steppers' command queue depth: 32 + +#### ESP-IDF version >=5.3.0: +* allows up to 200000 generated steps per second ? +* 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 + +### ESP32C6 + +* only from esp-idf >=v5.3.0 +* allows up to 200000 generated steps per second ? +* supports up to four stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Steppers' command queue depth: 32 +* untested + +### Atmel SAM Due + +* allows up to 50000 generated steps per second +* supports up to six stepper motors using Step/Direction/Enable Control (Direction and Enable is optional) +* Steppers' command queue depth: 32 + +Tested with max two stepper motors with 50 kHz step rate by clazarowitz + +## Usage + +The library is in use with A4988, but other driver ICs should work, too. + +For the API definition please consult the header file [FastAccelStepper.h](https://github.com/gin66/FastAccelStepper/blob/master/src/FastAccelStepper.h). Or a generated [markdown file](https://github.com/gin66/FastAccelStepper/blob/master/extras/doc/FastAccelStepper_API.md) + +Please check the examples for application and how to use the low level interface. +Some info is [Issue #86](https://github.com/gin66/FastAccelStepper/issues/86). + +The module defines the global variable `fas_queue`. Do not use or redefine this variable. + +Using the high level interface with ramp up/down as in [UsageExample.ino](https://github.com/gin66/FastAccelStepper/blob/master/examples/UsageExample/UsageExample.ino). + +``` +#include "FastAccelStepper.h" +#include "AVRStepperPins.h" // Only required for AVR controllers + +#define dirPinStepper 5 +#define enablePinStepper 6 +#define stepPinStepper 9 + +// If using an AVR device use the definitons provided in AVRStepperPins +// stepPinStepper1A +// +// or even shorter (for 2560 the correct pin on the chosen timer is selected): +// stepPinStepperA + +FastAccelStepperEngine engine = FastAccelStepperEngine(); +FastAccelStepper *stepper = NULL; + +void setup() { + engine.init(); + stepper = engine.stepperConnectToPin(stepPinStepper); + if (stepper) { + stepper->setDirectionPin(dirPinStepper); + stepper->setEnablePin(enablePinStepper); + stepper->setAutoEnable(true); + + stepper->setSpeedInHz(500); // 500 steps/s + stepper->setAcceleration(100); // 100 steps/s² + stepper->move(1000); + } +} + +void loop() { +} +``` + +Few comments to auto enable/disable: + +* If the motor is operated with micro stepping, then the disable/enable will cause the stepper to jump to/from the closest full step position. +* Some drivers need time to e.g. stabilize voltages until stepping should start. For this the start on delay has been added. See [issue #5](https://github.com/gin66/FastAccelStepper/issues/5). +* The turn off delay is realized in the cyclic task for esp32 or cyclic interrupt for avr. The esp32 task uses 4ms delay, while the avr repeats every ~4 ms at 16 MHz and atmel sam due every 2ms at 21MHz. Thus the turn off delay is a multiple (n>=2) of those period times and actual turning off takes place approx [(n-1)..n] * 4 ms resp. 2ms after the last step. +* The turn on delay is minimal `MIN_CMD_TICKS`. +* More than one stepper can be connected to one auto enable pin. Behaviour is like this: + 1. If stepper #1 needs enable, then it will enable it with its defined on delay time. + 2. If stepper #2, which is connected to same enable pin, starts after stepper one, then it still will wait its defined on delay time and set the enable pin, again (no-op). + The stepper #2 is not aware, that another stepper (stepper #1) has enabled the outputs already. + 3. If e.g. stepper #1 stops, then stepper #1's delay off counter is started. + 4. When stepper #1's counter is finished, then the FastAccelStepperEngine will ask all steppers, if they agree to stepper #1's disable request. + If stepper #2 is still running, then stepper #2 will not agree and the output will stay enabled. + 5. When stepper #2 stops, then stepper #2's delay off counter is started. + 6. When stepper #2's counter is finished, then the FastAccelStepperEngine will ask all steppers, if they agree to stepper #2's disable request. + Stepper #1 agrees, because it is not running. So the engine will call Stepper #2's _AND_ Stepper #1's disableOutputs(). + + The library does not consider the case, that Low/High Active enable may be mixed. + This means stepper #1 uses the enable pin as High Active and stepper #2 the same pin as Low Active. + => This situation will not be identified and will lead to unexpected behaviour + +## Behind the curtains + +### AVR ATmega168/328 and Atmega32u4 + +The timer 1 is used with prescaler 1. With the arduino nano running at 16 MHz, timer overflow interrupts are generated every ~4 ms. This timer overflow interrupt is used for adjusting the speed. + +The timer compare unit toggles the step pin from Low to High precisely. The transition High to Low is done in the timer compare interrupt routine, thus the High state is only few us. + +After stepper movement is completed, the timer compare unit is disconnected from the step pin. Thus the application could change the state freely, while the stepper is not controlled by this library. + +Measurement of the acceleration/deacceleration aka timer overflow interrupt yields: one calculation round needs around 300us. Thus it can keep up with the chosen 10 ms planning ahead time. + +### AVR ATmega2560 + +Similar to ATmega328, but instead of timer 1, timer 4 is used. + +For users of platformio, the used timer can be changed to either 1, 3, 4 or 5. For e.g. timer module 3 add to platformio.ini under `build_flags`: +``` +build_flags = -DFAS_TIMER_MODULE=3 +``` + +or better: +``` +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=3 +``` + +For arduino users, the same can be done by defining the flag *before* including the `FastAccelStepperEngine.h` header (as per info ixil), but apparently to [issue #50](https://github.com/gin66/FastAccelStepper/issues/50), this approach does not work for everyone: +e.g. +``` +sketch.ino +---------- +#include +#define FAS_TIMER_MODULE 3 +#include +/* ... */ +``` + +### ESP32 + +#### ESP-IDF version 4.x.y: +This stepper driver uses mcpwm modules of the esp32: for the first three stepper motors mcpwm0, and mcpwm1 for the steppers four to six. In addition, the pulse counter module is used starting from `unit_0` to `unit_5`. This driver uses the `pcnt_isr_service`, so unallocated modules can still be used by the application. The mcpwm modules' outputs are fed into the pulse counter by direct gpio-matrix-modification. + +For the other stepper motors, the rmt module comes into use. + +#### ESP-IDF version >=5.3.0: + +Only rmt module is supported. + +#### All +A note to `MIN_CMD_TICKS` using mcpwm/pcnt: The current implementation uses one interrupt per command in the command queue. This is much less interrupt rate than for avr. Nevertheless at 200kSteps/s the switch from one command to the next one should be ideally serviced before the next step. This means within 5us. As this cannot be guaranteed, the driver remedies an overrun (at least by design) to deduct the overrun pulses from the next command. The overrun pulses will then be run at the former command's tick rate. For real life stepper application, this should be ok. To be considered for raw access: Do not run many steps at high rate e.g. 200kSteps/s followed by a pause. + +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 | + +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. + +As of now, allocation of steppers on esp32 are: first all 6 mcpwm/pcnt drivers and then the 8 rmt drivers. In future this may be under application control. Starting with 0.29.2, the module can be directly selected on call of `stepperConnectToPin()`. So the allocation gets more flexible. + +One specific note for the rmt: If a direction pin toggle is needed directly after a command with steps, then the driver will add before that direction pin toggle another pause of `MIN_CMD_TICKS` ticks. + +### ESP32S2 + +This stepper driver uses rmt module. + +### ESP32S3 + +The ESP32S3's rmt module is similar to esp32c3 with 4 instead of 2 channels and with different register names. + +#### ESP-IDF version 4.x.y: +This stepper driver uses mcpwm/pcnt + rmt modules. Can drive up to 8 motors. Tested with 6 motors (not by me). +#### ESP-IDF version >=5.3.0: +This stepper driver uses rmt modules. Can drive up to 4 motors. + +### 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. + +### Atmel SAM Due + +This is supported by clazarowitz + +### ALL + +The used formula is just s = 1/2 * a * t² = v² / (2 a) with s = steps, a = acceleration, v = speed and t = time. In order to determine the speed for a given step, the calculation is v = sqrt(2 * a * s). The performed square root is an 8 bit table lookup using log2/pow2. Sufficient exact for this purpose. + +For the linear acceleration from/to standstill the used formula is s = 1/2 * j * t³. The variable j is calculated from acceleration and steps of linear acceleration, which is set by `setLinearAcceleration()`. + +The compare interrupt routines use 16bit tick counters, which translates to approx. 4ms. For longer time between pulses, pauses without step output can be added. With this approach the ramp generation supports up to one step per 268s. + +The low level command queue for each stepper allows direct speed control - when high level ramp generation is not operating. This allows precise control of the stepper, if the code, generating the commands, can cope with the stepper speed (beware of any Serial.print in your hot path). + +The chosen approach has few limitations for esp32. With acceleration = 1 step/s², the maximum speed is approx. 92 kStep/s. The max. supported speed for esp32 will be reachable only with acceleration >= 5 step/s². + +## Usage for multi-axis applications + +For coordinated movement of two or more axis, the current ramp generation will not provide good results. The planning of steps needs to take into consideration max.speed/acceleration of all steppers and eventually the net speed/acceleration of the resulting movement together with its restrictions. Nice example of multi-axis forward planning can be found within the [marlin-project](https://github.com/MarlinFirmware/Marlin/tree/2.0.x/Marlin/src/module). If this kind of multi-dimensional planning is used, then FastAccelStepper is a good solution to execute the raw commands (without ramp generation) with near-synchronous start of involved steppers. With the tick-exact execution of commands, the synchronization will not be lost as long as the command queues are not running out of commands. And for esp32, second requirement is, that the interrupts can be serviced on time (no pulses issued with previous command's pulse period time) + +To keep up the synchronization of two steppers please keep in mind: +* The stepper queue will on initial start, if configured, add pauses to the command queue to implement enable delay. + => Perhaps best to not use enable on delay +* If the stepper is configured for delays for direction change, then one pause is added to the command queue for each direction change together with a step. + => Execute direction change together with a pause or do not configure direction change delay + +Note for esp32 rmt driver: +- Due to the inner implementation, there has been the need to introduce pauses e.g. before a direction change. So the tick-exact execution of commands cannot be assumed, if during command generation pauses before/after dir changes are not generated by the application. + +## TODO + +See [project](https://github.com/gin66/FastAccelStepper/projects/1) + +## Arduino + +[Arduino library manager log](https://downloads.arduino.cc/libraries/logs/github.com/gin66/FastAccelStepper/) + +## PLATFORMIO + +[Library on platformio](https://registry.platformio.org/libraries/gin66/FastAccelStepper) + +If you prefer platformio and you are running Linux, then platformio version of the examples are created by executing + +``` +ci/build-platformio.sh +``` + +This will create a directory pio_dirs, which contains all examples. Can be executed by e.g. + +``` +cd pio_dirs/StepperDemo +pio run -e avr --target upload --upload-port /dev/ttyUSB0 +``` + +## ESP-IDF + +A `CMakeLists.txt` is provided to use FastAccelStepper as an ESP-IDF component. Clone it into the `components/` directory in the root of your project and build as usual. You must have Arduino available as a component. [See this](https://docs.espressif.com/projects/arduino-esp32/en/latest/esp-idf_component.html) for instructions on how to set that up. Tested as ESP-IDF component on PlatformIO Espressif32 Platform v3.3.2. + +For any questions/support please contact [gagank1](https://github.com/gagank1), as I do not use esp-idf + +## TEST STRATEGY + +The library is tested with different kind of tests: +* PC only (sub folder `./tests/pc_based`) + + These tests focussing primarily the ramp generator and part of the API +* simavr based for avr (sub folder `./tests/simavr_based`) + + The simavr is an excellent simulator for avr microcontrollers. This allows to check the avr implementation thoroughly: number of steps generated, virtual stepper position and even timing. Tested code is mainly the StepperDemo, which gets fed in a one line sequence of commands to execute. These tests are focused on avr, but help to check the whole library code, used by esp32, too. +* esp32 tests with another pulse counter attached (e.g. `test_seq_08` in StepperDemo) + + The FastAccelStepper-API supports to attach another free pulse counter to a stepper's step and dir pins. This counter counts in the range of -16383 to 16383 with wrap around to 0. The test condition is, that the library's view of the position should match the independently counted one. These tests are still evolving + +* Test for pulse generation using examples/Pulses + + This has been intensively used to debug the esp32 ISR code + +* esp32 hw tests + + These tests live under sub folder `./tests/esp32_hw_based` + +* manual tests using examples/StepperDemo + + These are unstructured tests with listening to the motor and observing the behavior + +## Test sequences from StepperDemo + +Short info, what the test sequences, embedded in StepperDemo in the test mode, do: + +- 01 - Run the stepper like a clock for one minute +- 02 - Run the stepper towards positive position and back to zero repeatedly +- 03/04 - same like 02, both different speed/acceleration +- 05 - Perform 800 times a single step and then 800 steps back in one command +- 06 - Run 32000 steps with speed changes every 100ms in order to reproduce issue #24 + +All those tests have no internal test passed/failed criteria. Those are used for automated tests with `./tests/simavr_based` and `./tests/esp32_hw_based`. The test pass criteria are: They should run smoothly without hiccups and return to start position. + +- 07 - measures timing of several moveByAcceleration(). Should stop at position zero. (should be started from position 0). +- 08 - is an endless running test to check on esp32, if the generated pulses are successfully counted by a second pulse counter. The moves should be all executed in one second with alternating direction and varying speed/acceleration +- 09 - is an endless running test with starting a ramp with random speed/acceleration/direction, which after 1s is stopped with forceStopAndNewPosition(). It contains no internal test criteria, but looking at the log, the match of generated and sent pulses can be checked. And the needed steps for a forceStopAndNewPosition() can be derived out of this +- 10 - runs the stepper forward and every 200 ms changes speed with increasing positive speed deltas and then decreasing negative speed deltas. +- 11 - runs the stepper to position 1000000 and back to 0. This tests, if getCurrentPosition() is counting monotonously up or down respectively. + +## CHANGELOG + +See [changelog](https://github.com/gin66/FastAccelStepper/blob/master/CHANGELOG) + +## ISSUES + +* There is an issue with the esp32 mcpwm: as soon as the mcpwm timer is running, on every cycle an interrupt is serviced - even though no interrupt is enabled. If several steppers are running at high step rate, the interrupt load for this nonsense interrupt could be quite high for the CPU. Need further investigation, but till now haven't found the root cause. +* Compilation using esp-idf 4.4 will yield a deprecation warning for `mcpwm_isr_register()`. This has been raised as [issue](https://github.com/espressif/esp-idf/issues/7890) at espressif +* `framework-arduinoespressif32 @ 3.10006.210326` and later will lead to compile error for esp32, if using compiler options `-Werror -Wall` !!! The problem can be circumvented by applying `-Wno-error=incompatible-pointer-types` + + +## Error investigation + +In case the stepper does not run smoothly, then StepperDemo contains commands to simulate two type of error causes. For avr the commands are `r` and `e`. For esp32 only `e` can be used. +- `r`: The digitalRead() of arduino is a fancy implementation, which checks, if the pin being read is connected to a timer to generate PWM and if yes, turns this off (actually IMHO a broken implementation: only 1 of the needed 2 bits are cleared, and the activation by force compare is missing). As FastAccelStepper controls the step pin, the digitalRead can disturb the step pin (even though I have expected step loss, only difference in noise can be heard). The error simulation in StepperDemo reads the pins in the main loop(), thus the symptom occurs quite reliably. +- `e`: This blocks repeatedly interrupts for ~100us during 64ms out of 256ms. On AVR to see this problem popping up, the stepper rate has to be <~106 us (avr, one stepper running). >~106us it runs quite smoothly. The 106us = 100us block + ~6us ISR runtime. For ESP32 this has no effect. + +For avr: cause of long interrupt being blocked can be e.g.: +- long section of codes between `noInterrupts()/interrupts()` in the application (or used libraries) +- long interrupt service routines in the application (or used libraries). +- port interrupts connected to noisy/bouncy switches causing bursts of interrupts + +Especially in interrupt service routines, the `digitalRead()/digitalWrite()` must be avoided. Alternative solution is described e.g. here: [blog](https://masteringarduino.blogspot.com/2013/10/fastest-and-smallest-digitalread-and.html), or [digitalWriteFast](https://github.com/NicksonYap/digitalWriteFast), or [fast versions](https://forum.arduino.cc/index.php?topic=46896.0). + +This feature of StepperDemo allows to compare non-smooth running stepper in an application with these error types. + +## Lessons Learned + +* Spent more than half a day debugging the esp32-code, till I have found out, that just the cable to the stepper was broken. +* 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 + +Found on youtube: + +* [Testing on NEMA-17](https://www.youtube.com/watch?v=yUTXTRjAOak) +* [high speed Closed Loop nema 34 12nm stepper motor, esp32 FastAccelStepper, SBH860H driver](https://www.youtube.com/watch?v=hPxJekex5zM) +* [Neck mechanism](https://www.youtube.com/watch?v=rY7NDBnz7Cw) +* [Stepper motor at 1500RPM with ESP32 and A4988](https://www.youtube.com/watch?v=sQqezEsiuUU) +* [DIY 3 AXIS CAMERA SLIDER | MOTORIZED CAMERA SLIDER](https://youtu.be/7TkybpSQULk) +* [NEMA 17 Servo: Final Accuracy Test & New Speed Record!! - N-Gnoid TV](https://www.youtube.com/watch?v=EHHDuI3xK94) + +As mentioned by kthod861 in [Issue #110](https://github.com/gin66/FastAccelStepper/issues/110): +* [22 01 2021 Stepper POC3](https://youtu.be/fm2_VkUG10k) + +## Contribution + +- Thanks ixil for pull request (https://github.com/gin66/FastAccelStepper/pull/19) for ATmega2560 +- Thanks gagank1 for esp-idf integration by adding CMakeLists.txt (https://github.com/gin66/FastAccelStepper/pull/81) +- 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) +- 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) +- Thanks HalfVoxel for pull requests (https://github.com/gin66/FastAccelStepper/pull/270) and (https://github.com/gin66/FastAccelStepper/pull/271): improved doc and missing parenthesis in preprocessor macros + diff --git a/extras/ci/platformio.ini b/extras/ci/platformio.ini new file mode 100644 index 0000000..4ccc20f --- /dev/null +++ b/extras/ci/platformio.ini @@ -0,0 +1,567 @@ +; 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 +; +; use build_flags = -E -DM +; to get all preprocessor symbols in .pio/build/xx/src/StepperDemo.cpp.o +; +[platformio] + +[env:esp32_tasmota_3_0_0] +platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.05.12/platform-espressif32.zip +board = esp32dev +framework = arduino +build_flags = -Wall -DCORE_DEBUG_LEVEL=6 -DLOG_LOCAL_LEVEL=6 +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_tasmota_2_0_15] +platform = https://github.com/tasmota/platform-espressif32/releases/download/2024.04.00/platform-espressif32.zip +board = esp32dev +framework = arduino +build_flags = -Wall -DCORE_DEBUG_LEVEL=5 +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32] +platform = espressif32 +board = esp32dev +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32dev +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32dev +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_8_0] +platform = espressif32 @ 6.8.0 +board = esp32dev +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_V6_7_0] +platform = espressif32 @ 6.7.0 +board = esp32dev +framework = arduino +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 +framework = arduino +build_flags = -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 -Wno-unused-variable +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 -Wno-unused-variable +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 -Wno-unused-variable +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 -Wno-unused-variable +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 -Wno-unused-variable -Wno-deprecated-declarations +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s2_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32-s2-saola-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s2_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32-s2-saola-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s2_V6_8_0] +platform = espressif32 @ 6.8.0 +board = esp32-s2-saola-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s2_V6_7_0] +platform = espressif32 @ 6.7.0 +board = esp32-s2-saola-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s2_V6_6_0] +platform = espressif32 @ 6.6.0 +board = esp32-s2-saola-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s2_V6_5_0] +platform = espressif32 @ 6.5.0 +board = esp32-s2-saola-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s3_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32-s3-devkitc-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s3_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32-s3-devkitc-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s3_V6_8_0] +platform = espressif32 @ 6.8.0 +board = esp32-s3-devkitc-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s3_V6_7_0] +platform = espressif32 @ 6.7.0 +board = esp32-s3-devkitc-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s3_V6_6_0] +platform = espressif32 @ 6.6.0 +board = esp32-s3-devkitc-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32s3_V6_5_0] +platform = espressif32 @ 6.5.0 +board = esp32-s3-devkitc-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32c3_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32-c3-devkitm-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32c3_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32-c3-devkitm-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32c3_V6_8_0] +platform = espressif32 @ 6.8.0 +board = esp32-c3-devkitm-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32c3_V6_7_0] +platform = espressif32 @ 6.7.0 +board = esp32-c3-devkitm-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32c3_V6_6_0] +platform = espressif32 @ 6.6.0 +board = esp32-c3-devkitm-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32c3_V6_5_0] +platform = espressif32 @ 6.5.0 +board = esp32-c3-devkitm-1 +framework = arduino +build_flags = -Wall +board_build.f_cpu = 240000000L +lib_extra_dirs = ../../.. + +[env:esp32_idf_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32dev +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes -DCONFIG_RMT_ENABLE_DEBUG_LOG=5 +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32_idf_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32dev +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes -DCONFIG_RMT_ENABLE_DEBUG_LOG=5 +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32_idf_V6_7_0] +platform = espressif32 @ 6.7.0 +board = esp32dev +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes -DCONFIG_RMT_ENABLE_DEBUG_LOG=5 +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32_idf_V5_3_0] +platform = espressif32 @ 5.3.0 +board = esp32dev +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32_idf_V5_2_0] +platform = espressif32 @ 5.2.0 +board = esp32dev +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32_idf_V5_1_0] +platform = espressif32 @ 5.1.0 +board = esp32dev +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32_idf_V5_0_0] +platform = espressif32 @ 5.0.0 +board = esp32dev +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +#[env:esp32_idf_V4_4_0] +#platform = espressif32 @ 4.4.0 +#board = esp32dev +#framework = espidf +#build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +#board_build.f_cpu = 240000000L +#lib_extra_dirs = ../../ + +[env:esp32c3_idf_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32-c3-devkitm-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32c3_idf_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32-c3-devkitm-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32c3_idf_V5_3_0] +platform = espressif32 @ 5.3.0 +board = esp32-c3-devkitm-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32s2_idf_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32-s2-saola-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32s2_idf_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32-s2-saola-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32s2_idf_V5_3_0] +platform = espressif32 @ 5.3.0 +board = esp32-s2-saola-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32s3_idf_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32-s3-devkitc-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32s3_idf_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32-s3-devkitc-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32s3_idf_V5_3_0] +platform = espressif32 @ 5.3.0 +board = esp32-s3-devkitc-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32c6_idf_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32-c6-devkitm-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32c6_idf_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32-c6-devkitm-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32h2_idf_V6_9_0] +platform = espressif32 @ 6.9.0 +board = esp32-h2-devkitm-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +[env:esp32h2_idf_V6_8_1] +platform = espressif32 @ 6.8.1 +board = esp32-h2-devkitm-1 +framework = espidf +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-unused-function -Wno-error=attributes +board_build.f_cpu = 240000000L +lib_extra_dirs = ../.. + +#[env:esp32s2] +#board = esp32-s2-saola-1 +#framework = arduino +#platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.04.02/platform-espressif32.zip +##platform = https://github.com/tasmota/platform-espressif32/releases/download/v.2.0.3/platform-espressif32-v.2.0.3.zip +#build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types +#board_build.f_cpu = 240000000L +#lib_extra_dirs = ../../.. +# +#[env:esp32s3] +#board = esp32-s3-devkitc-1 +#framework = arduino +#platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.02.00/platform-espressif32.zip +#build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types -Wno-error=unused-variable +#board_build.f_cpu = 240000000L +#lib_extra_dirs = ../../.. +# +#[env:esp32c3] +#board = esp32-c3-devkitm-1 +#framework = arduino +#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 +board = nanoatmega168 +framework = arduino +build_flags = -Werror -Wall -Wno-deprecated-declarations +lib_extra_dirs = ../../.. + +[env:nanoatmega328] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall -Wno-deprecated-declarations +lib_extra_dirs = ../../.. + +[env:atmega2560] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -Wno-deprecated-declarations +lib_extra_dirs = ../../.. + +[env:atmelsam] +platform = atmelsam +board = due +framework = arduino +;build_flags = -Werror -Wall -Wno-deprecated-declarations +; cannot use -Werror due to sam platform issue +build_flags = -Wall -Wno-deprecated-declarations +lib_extra_dirs = ../../.. + +[env:atmega32u4] +platform = atmelavr +board = leonardo +framework = arduino +build_flags = -Werror -Wall -Wno-deprecated-declarations -Wno-error=incompatible-pointer-types +lib_extra_dirs = ../../.. diff --git a/extras/doc/FastAccelStepper_API.md b/extras/doc/FastAccelStepper_API.md new file mode 100644 index 0000000..61dfc9d --- /dev/null +++ b/extras/doc/FastAccelStepper_API.md @@ -0,0 +1,722 @@ +# FastAccelStepper + +FastAccelStepper is a high speed alternative for the +[AccelStepper library](http:www.airspayce.com/mikem/arduino/AccelStepper/). +Supported are avr (ATmega 168/328/P, ATmega2560), esp32 and atmelsam due. + +Here is a basic example to run a stepper from position 0 to 1000 and back +again to 0. +``` + +FastAccelStepperEngine engine = FastAccelStepperEngine(); +FastAccelStepper *stepper = NULL; + +#define dirPinStepper 5 +#define enablePinStepper 6 +#define stepPinStepper 9 +void setup() { + engine.init(); + stepper = engine.stepperConnectToPin(stepPinStepper); + if (stepper) { + stepper->setDirectionPin(dirPinStepper); + stepper->setEnablePin(enablePinStepper); + stepper->setAutoEnable(true); + + stepper->setSpeedInHz(500); + stepper->setAcceleration(100); + stepper->moveTo(1000, true); + stepper->moveTo(0, true); + } +} + +void loop() {} +``` + +## FastAccelStepperEngine + +This engine - actually a factory - provides you with instances of steppers. +### Initialization + +The FastAccelStepperEngine is declared with FastAccelStepperEngine(). +This is to occupy the needed memory. +```cpp +FastAccelStepperEngine engine = FastAccelStepperEngine(); +``` +But it still needs to be initialized. +For this init shall be used: +```cpp +void setup() { + engine.init(); +} +``` +In a multitasking and multicore system like ESP32, the steppers are +controlled by a continuously running task. This task can be fixed to one +CPU core with this modified init()-call. ESP32 implementation detail: For +values 0 and 1, xTaskCreatePinnedToCore() is used, or else xTaskCreate() +```cpp + void init(uint8_t cpu_core); +#endif +``` +### Creation of FastAccelStepper + +Using a call to `stepperConnectToPin()` a FastAccelStepper instance is +created. This call tells the stepper, which step pin to use. As the +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. +This call allows to select the respective driver +```cpp +#if defined(SUPPORT_SELECT_DRIVER_TYPE) +#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); +#endif +``` +Comments to valid pins: + +| Device | Comment | +|:----------------|:--------------------------------------------------------------------------------------------------| +| ESP32   | Every output capable GPIO can be used | +| ESP32S2   | Every output capable GPIO can be used | +| Atmega168/328/p | Only the pins connected to OC1A and OC1B are allowed | +| Atmega2560 | Only the pins connected to OC4A, OC4B and OC4C are allowed. | +| Atmega32u4 | Only the pins connected to OC1A, OC1B and OC1C are allowed | +| Atmel SAM | This can be one of each group of pins: 34/67/74/35, 17/36/72/37/42, 40/64/69/41, 9, 8/44, 7/45, 6 | +## External Pins + +If the direction/enable pins are e.g. connected via external HW (shift +registers), then an external callback function can be supplied. The +supplied value is either LOW or HIGH. The return value shall be the status +of the pin (false for LOW or true for HIGH). If returned value and supplied +value do not match, the stepper does not continue, but calls this function +again. + +This function is called from cyclic task/interrupt with 4ms rate, which +creates the commands to put into the command queue. Thus the supplied +function should take much less time than 4ms. Otherwise there is risk, that +other running steppers are running out of commands in the queue. If this +takes longer, then the function should be offloaded and return the new +status, after the pin change has been successfully completed. + +The callback has to be called on the FastAccelStepperEngine. +See examples/ExternalCall + +Stepperpins (enable or direction), which should use this external callback, +need to be or'ed with PIN_EXTERNAL_FLAG ! FastAccelStepper uses this flag +to determine, if a pin is external or internal. +```cpp + void setExternalCallForPin(bool (*func)(uint8_t pin, uint8_t value)); +``` +### Debug LED + +If blinking of a LED is required to indicate, the stepper controller is +still running, then the port. to which the LED is connected, can be told to +the engine. The periodic task will let the associated LED blink with 1 Hz +```cpp + void setDebugLed(uint8_t ledPin); +``` +### Return codes of calls to `move()` and `moveTo()` + +The defined preprocessor macros are MOVE_xxx: +MOVE_OK: All is OK: +MOVE_ERR_NO_DIRECTION_PIN: Negative direction requested, but no direction pin +MOVE_ERR_SPEED_IS_UNDEFINED: The maximum speed has not been set yet +MOVE_ERR_ACCELERATION_IS_UNDEFINED: The acceleration to use has not been set +yet +### Return codes of `rampState()` + +The return value is an uint8_t, which consist of two fields: + +| Bit 7 | Bits 6-5 | Bits 4-0 | +|:--------|:----------|:---------| +|Always 0 | Direction | State | + +The bit mask for direction and state: +```cpp +#define RAMP_DIRECTION_MASK (32 + 64) +#define RAMP_STATE_MASK (1 + 2 + 4 + 8 + 16) +``` +The defined ramp states are: +```cpp +#define RAMP_STATE_IDLE 0 +#define RAMP_STATE_COAST 1 +#define RAMP_STATE_ACCELERATE 2 +#define RAMP_STATE_DECELERATE 4 +#define RAMP_STATE_REVERSE (4 + 8) +#define RAMP_STATE_ACCELERATING_FLAG 2 +#define RAMP_STATE_DECELERATING_FLAG 4 +``` +And the two directions of a move +```cpp +#define RAMP_DIRECTION_COUNT_UP 32 +#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 +control the direction flags + +## Timing values - Architecture dependent + +### AVR +|VARIABLE | Value | Unit | +|:----------------|------------:|:------------------------| +|TICKS_PER_S | 16_000_000 | [ticks/s] | +|MIN_CMD_TICKS | 640 | [1/TICKS_PER_S seconds] | +|MIN_DIR_DELAY_US | 40 | [µs] | +|MAX_DIR_DELAY_US | 4095 | [µs] | + +### ESP32 +|VARIABLE | Value | Unit | +|:----------------|------------:|:------------------------| +|TICKS_PER_S | 16_000_000 | [ticks/s] | +|MIN_CMD_TICKS | 3200 | [1/TICKS_PER_S seconds] | +|MIN_DIR_DELAY_US | 200 | [µs] | +|MAX_DIR_DELAY_US | 4095 | [µs] | + +### SAM DUE +|VARIABLE | Value | Unit | +|:----------------|------------:|:------------------------| +|TICKS_PER_S | 21_000_000 | [ticks/s] | +|MIN_CMD_TICKS | 4200 | [1/TICKS_PER_S seconds] | +|MIN_DIR_DELAY_US | 200 | [µs] | +|MAX_DIR_DELAY_US | 3120 | [µs] | + +# FastAccelStepper +## Step Pin +step pin is defined at creation. Here can retrieve the pin +```cpp + uint8_t getStepPin(); +``` +## Direction Pin +if direction pin is connected, call this function. + +If the pin number is >= 128, then the direction pin is assumed to be +external and the external callback function (set by +`setExternalCallForPin()`) is used to set the pin. For direction pin, this +is implemented for esp32 and its supported derivates, and avr and its +derivates except atmega32u4 + +For slow driver hardware the first step after any polarity change of the +direction pin can be delayed by the value dir_change_delay_us. The allowed +range is MIN_DIR_DELAY_US and MAX_DIR_DELAY_US. The special value of 0 +means, that no delay is added. Values 1 up to MIN_DIR_DELAY_US will be +clamped to MIN_DIR_DELAY_US. Values above MAX_DIR_DELAY_US will be clamped +to MAX_DIR_DELAY_US. For external pins, dir_change_delay_us is ignored, +because the mechanism applied for external pins provides already pause +in the range of ms or more. +```cpp + void setDirectionPin(uint8_t dirPin, bool dirHighCountsUp = true, + uint16_t dir_change_delay_us = 0); + uint8_t getDirectionPin() { return _dirPin; } + bool directionPinHighCountsUp() { return _dirHighCountsUp; } +``` +## Enable Pin +if enable pin is connected, then use this function. + +If the pin number is >= 128, then the enable pin is assumed to be +external and the external callback function (set by +`setExternalCallForPin()`) is used to set the pin. + +In case there are two enable pins: one low and one high active, then +these calls are valid and both pins will be operated: + setEnablePin(pin1, true); + setEnablePin(pin2, false); +If pin1 and pin2 are same, then the last call will be used. +```cpp + void setEnablePin(uint8_t enablePin, bool low_active_enables_stepper = true); + uint8_t getEnablePinHighActive() { return _enablePinHighActive; } + uint8_t getEnablePinLowActive() { return _enablePinLowActive; } +``` +using enableOutputs/disableOutputs the stepper can be enabled and disabled +For a running motor with autoEnable set, disableOutputs() will return false + bool enableOutputs();returns true, if enabled + bool disableOutputs();returns true, if disabled +In auto enable mode, the stepper is enabled before stepping and disabled +afterwards. The delay from stepper enabled till first step and from +last step to stepper disabled can be separately adjusted. +The delay from enable to first step is done in ticks and as such is limited +to MAX_ON_DELAY_TICKS, which translates approximately to 120ms for +esp32 and 60ms for avr at 16 MHz). The delay till disable is done in period +interrupt/task with 4 or 10 ms repetition rate and as such is with several +ms jitter. +```cpp + void setAutoEnable(bool auto_enable); + int8_t setDelayToEnable(uint32_t delay_us); + void setDelayToDisable(uint16_t delay_ms); +#define DELAY_OK 0 +#define DELAY_TOO_LOW -1 +#define DELAY_TOO_HIGH -2 +``` +## Stepper Position +Retrieve the current position of the stepper + +Comment for esp32 with rmt module: +The actual position may be off by the number of steps in the ongoing +command. If precise real time position is needed, attaching a pulse counter +may be of help. +```cpp + int32_t getCurrentPosition(); +``` +Set the current position of the stepper - either in standstill or while +moving. + for esp32: the implementation uses getCurrentPosition(), which does not + consider the steps of the current command + => recommend to use only in standstill +```cpp + void setCurrentPosition(int32_t new_pos); +``` +## Stepper running status +is true while the stepper is running or ramp generation is active +```cpp + bool isRunning(); +``` +## Speed +For stepper movement control by FastAccelStepper's ramp generator + +Speed can be defined in four different units: +- In Hz: This means steps/s +- In millHz: This means in steps/1000s +- In us: This means in us/step + +For the device's maximum allowed speed, the following calls can be used. +```cpp + uint16_t getMaxSpeedInUs(); + uint16_t getMaxSpeedInTicks(); + uint32_t getMaxSpeedInHz(); + uint32_t getMaxSpeedInMilliHz(); +``` +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); +#endif +``` +Setting the speed can be done with the four `setSpeed...()` calls. +The new value will be used only after call of these functions: + +- `move()` +- `moveTo()` +- `runForward()` +- `runBackward()` +- `applySpeedAcceleration()` +- `moveByAcceleration()` + +Note: no update on `stopMove()` + +Returns 0 on success, or -1 on invalid value. +Invalid is faster than MaxSpeed or slower than ~250 Mio ticks/step. +```cpp + int8_t setSpeedInUs(uint32_t min_step_us); + int8_t setSpeedInTicks(uint32_t min_step_ticks); + int8_t setSpeedInHz(uint32_t speed_hz); + int8_t setSpeedInMilliHz(uint32_t speed_mhz); +``` +To retrieve current set speed. This means, while accelerating and/or +decelerating, this is NOT the actual speed ! +```cpp + uint32_t getSpeedInUs() { return _rg.getSpeedInUs(); } + uint32_t getSpeedInTicks() { return _rg.getSpeedInTicks(); } + uint32_t getSpeedInMilliHz() { return _rg.getSpeedInMilliHz(); } +``` +If the current speed is needed, then use `getCurrentSpeed...()`. This +retrieves the actual speed. + +| value | description | +|:-----:|:-----------------------------| +| = 0 | while not moving | +| > 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(bool realtime = true); + int32_t getCurrentSpeedInMilliHz(bool realtime = true); +``` +## Acceleration + setAcceleration() expects as parameter the change of speed + as step/s². + If for example the speed should ramp up from 0 to 10000 steps/s within + 10s, then the acceleration is 10000 steps/s / 10s = 1000 steps/s² + +New value will be used after call to +move/moveTo/runForward/runBackward/applySpeedAcceleration/moveByAcceleration + +note: no update on stopMove() + +Returns 0 on success, or -1 on invalid value (<=0) +```cpp + int8_t setAcceleration(int32_t step_s_s) { + return _rg.setAcceleration(step_s_s); + } + uint32_t getAcceleration() { return _rg.getAcceleration(); } +``` +getCurrentAcceleration() retrieves the actual acceleration. + = 0 while idle or coasting + > 0 while speed is changing towards positive values + < 0 while speed is changeing towards negative values +```cpp + int32_t getCurrentAcceleration() { + return _rg.getCurrentAcceleration(); + } +``` +## Linear Acceleration + setLinearAcceleration expects as parameter the number of steps, + where the acceleration is increased linearly from standstill up to the + configured acceleration value. If this parameter is 0, then there will be + no linear acceleration phase + + If for example the acceleration should ramp up from 0 to 10000 steps/s^2 + within 100 steps, then call setLinearAcceleration(100) + + The speed at which linear acceleration turns into constant acceleration + can be calculated from the parameter linear_acceleration_steps. + Let's call this parameter `s_h` for handover steps. + Then the speed is: + `v_h = sqrt(1.5 * a * s_h)` + +New value will be used after call to +move/moveTo/runForward/runBackward/applySpeedAcceleration/moveByAcceleration + +note: no update on stopMove() +```cpp + void setLinearAcceleration(uint32_t linear_acceleration_steps) { + _rg.setLinearAcceleration(linear_acceleration_steps); + } +``` +## Jump Start +setJumpStart expects as parameter the ramp step to start from standstill. + +The speed at which the stepper will start can be calculated like this: +- If linear acceleration is not in use: + start speed `v = sqrt(2 * a * jump_step)` +- If linear acceleration is in use and `jump_step <= s_h`: + start speed `v = sqrt(1.5*a)/s_h^(1/6) * jump_step^(2/3)` +- If linear acceleration is in use and `jump_step > s_h`: + start speed `v = sqrt(2 * a * (jump_step - s_h/4))` + + +New value will be used after call to +move/moveTo/runForward/runBackward +```cpp + void setJumpStart(uint32_t jump_step) { _rg.setJumpStart(jump_step); } +``` +## Apply new speed/acceleration value +This function applies new values for speed/acceleration. +This is convenient especially, if the stepper is set to continuous running. +```cpp + void applySpeedAcceleration(); +``` +## Move commands +### move() and moveTo() +start/move the stepper for (move) steps or to an absolute position. + +If the stepper is already running, then the current running move will be +updated together with any updated values of acceleration/speed. The move is +relative to the target position of any ongoing move ! If the new +move/moveTo for an ongoing command would reverse the direction, then the +command is silently ignored. +return values are the MOVE_... constants +```cpp + int8_t move(int32_t move, bool blocking = false); + int8_t moveTo(int32_t position, bool blocking = false); +``` +### keepRunning() +This command flags the stepper to keep run continuously into current +direction. It can be stopped by stopMove. +Be aware, if the motor is currently decelerating towards reversed +direction, then keepRunning() will speed up again and not finish direction +reversal first. +```cpp + void keepRunning(); + bool isRunningContinuously() { return _rg.isRunningContinuously(); } +``` +### runForward() and runBackwards() +These commands just let the motor run continuously in one direction. +If the motor is running in the opposite direction, it will reverse +return value as with move/moveTo +```cpp + int8_t runForward(); + int8_t runBackward(); +``` +### forwardStep() and backwardStep() +forwardStep()/backwardstep() can be called, while stepper is not moving +If stepper is moving, this is a no-op. +backwardStep() is a no-op, if no direction pin defined +It will immediately let the stepper perform one single step. +If blocking = true, then the routine will wait till isRunning() is false +```cpp + void forwardStep(bool blocking = false); + void backwardStep(bool blocking = false); +``` +### moveByAcceleration() +moveByAcceleration() can be called, if only the speed of the stepper +is of interest and that speed to be controlled by acceleration. +The maximum speed (in both directions) to be set by setSpeedInUs() before. +The behaviour will be: + acceleration > 0 => accelerate towards positive maximum speed + acceleration = 0 => keep current speed + acceleration < 0 + => accelerate towards negative maximum speed if allow_reverse + => decelerate towards motor stop if allow_reverse = false +return value as with move/moveTo +```cpp + int8_t moveByAcceleration(int32_t acceleration, bool allow_reverse = true); +``` +### stopMove() +Stop the running stepper with normal deceleration. +This only sets a flag and can be called from an interrupt ! +```cpp + void stopMove(); + bool isStopping() { return _rg.isStopping(); } +``` +### stepsToStop() +This returns the current step value of the ramp. +This equals the number of steps for a motor to +reach the current position and speed from standstill +and to come to standstill with deceleration if stopped +immediately. +This value is valid with or without linear acceleration +being used. +Primary use is to forecast possible stop position. +The stop position is: + getCurrentPosition() + stepsToStop() +in case of a motor running in positive direction. +```cpp + uint32_t stepsToStop() { return _rg.stepsToStop(); } +``` +### forceStop() +Abruptly stop the running stepper without deceleration. +This can be called from an interrupt ! + +The stepper command queue will be processed, but no further commands are +added. This means, that the stepper can be expected to stop within approx. +20ms. +```cpp + void forceStop(); +``` +abruptly stop the running stepper without deceleration. +This can be called from an interrupt ! + +No further step will be issued. As this is aborting all commands in the +queue, the actual stop position is lost (recovering this position cannot be +done within an interrupt). So the new position after stop has to be +provided and will be set as current position after stop. +```cpp + void forceStopAndNewPosition(int32_t new_pos); +``` +get the target position for the current move. +As of now, this position is the view of the stepper task. +This means, the value will stay unchanged after a move/moveTo until the +stepper task is executed. +In keep running mode, the targetPos() is not updated +```cpp + int32_t targetPos() { return _rg.targetPosition(); } +``` +### Task planning +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. +But the queue may run out of commands while executing addQueueEntry, +so it is better to set start=true to automatically restart/continue +a running queue. + +If the queue is not running, then the start parameter defines starting it +or not. The latter case is of interest to first fill the queue and then +start it. + +The call addQueueEntry(NULL, true) just starts the queue. This is intended +to achieve a near synchronous start of several steppers. Consequently it +should be called with interrupts disabled and return very fast. +Actually this is necessary, too, in case the queue is full and not +started. +```cpp + int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start = true); +``` +Return codes for addQueueEntry + positive values mean, that caller should retry later +```cpp +#define AQE_OK 0 +#define AQE_QUEUE_FULL 1 +#define AQE_DIR_PIN_IS_BUSY 2 +#define AQE_WAIT_FOR_ENABLE_PIN_ACTIVE 3 +#define AQE_DEVICE_NOT_READY 4 +#define AQE_ERROR_TICKS_TOO_LOW -1 +#define AQE_ERROR_EMPTY_QUEUE_TO_START -2 +#define AQE_ERROR_NO_DIR_PIN_TO_TOGGLE -3 +``` +### check functions for command queue being empty, full or running. +```cpp + bool isQueueEmpty(); + bool isQueueFull(); + bool isQueueRunning(); +``` +### functions to get the fill level of the queue + +To retrieve the forward planning time in the queue, ticksInQueue() +can be used. It sums up all ticks of the not yet processed commands. +For commands defining pauses, the summed up value is entry.ticks. +For commands with steps, the summed up value is entry.steps*entry.ticks +```cpp + uint32_t ticksInQueue(); +``` +This function can be used to check, if the commands in the queue +will last for ticks. This is again without the +currently processed command. +```cpp + bool hasTicksInQueue(uint32_t min_ticks); +``` +This function allows to check the number of commands in the queue. +This is including the currently processed command. +```cpp + uint8_t queueEntries(); +``` +Get the future position of the stepper after all commands in queue are +completed +```cpp + int32_t getPositionAfterCommandsCompleted(); +``` +Get the future speed of the stepper after all commands in queue are +completed. This is in µs. Returns 0 for stopped motor + +This value comes from the ramp generator and is not valid for raw command +queue +==> Will be renamed in future release +```cpp + uint32_t getPeriodInUsAfterCommandsCompleted(); + uint32_t getPeriodInTicksAfterCommandsCompleted(); +``` +Set the future position of the stepper after all commands in queue are +completed. This has immediate effect to getCurrentPosition(). +```cpp + void setPositionAfterCommandsCompleted(int32_t new_pos); +``` +This function provides info, in which state the high level stepper control +is operating. The return value is an `or` of RAMP_STATE_... and +RAMP_DIRECTION_... flags. Definitions are above +```cpp + uint8_t rampState() { return _rg.rampState(); } +``` +returns true, if the ramp generation is active +```cpp + bool isRampGeneratorActive() { return _rg.isRampGeneratorActive(); } +``` +These functions allow to detach and reAttach a step pin for other use. +Pretty low level, use with care or not at all +```cpp + void detachFromPin(); + void reAttachToPin(); +``` +## ESP32 only: Free pulse counter +These four functions are only available on esp32. +The first can attach any of the eight pulse counters to this stepper. +The second then will read the current pulse counter value + +The user is responsible to not use a pulse counter, which is occupied by a +stepper and by this render the stepper or even blow up the uC. + +Pulse counter 6 and 7 are not used by the stepper library and are judged as +available. If only five steppers are defined, then 5 gets available. If +four steppers are defined, then 4 is usable,too. + +These functions are intended primarily for testing, because the library +should always output the correct amount of pulses. Possible application +usage would be an immediate and interrupt friendly version for +getCurrentPosition() + +The pulse counter counts up towards high_value. +If the high_value is reached, then the pulse counter is reset to 0. +Similarly, if direction pin is configured and set to count down, +then the pulse counter counts towards low_value. When the low value is hit, +the pulse counter is reset to 0. + +If low_value and high_value are set to zero, then the pulse counter is just +counting like any int16_t counter: 0...32767,-32768,-32767,...,0 and +backwards accordingly + +Possible application: +Assume the stepper, to which the pulse counter attached to, needs 3200 +steps/revolution. If now attachToPulseCounter is called with -3200 and 3200 +for the low and high values respectively, then the momentary angle of the +stepper (at exact this moment) can be retrieved just by reading the pulse +counter. If the value is negative, then just add 3200. + +Update for idf5 version: +The pcnt_unit value is not used, because the available units are managed +by the system. The parameter is kept for compatibility. + +```cpp +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5) + bool attachToPulseCounter(uint8_t unused_pcnt_unit = 0, + int16_t low_value = -16384, + int16_t high_value = 16384); + int16_t readPulseCounter(); + void clearPulseCounter(); + bool pulseCounterAttached() { return _attached_pulse_unit != NULL; } +#endif +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4) + bool attachToPulseCounter(uint8_t pcnt_unit, int16_t low_value = -16384, + int16_t high_value = 16384); + int16_t readPulseCounter(); + void clearPulseCounter(); + bool pulseCounterAttached() { return _attached_pulse_cnt_unit >= 0; } +#endif +``` diff --git a/extras/doc/application.wxm b/extras/doc/application.wxm new file mode 100644 index 0000000..d7237ec --- /dev/null +++ b/extras/doc/application.wxm @@ -0,0 +1,44 @@ +/* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/ +/* [ Created with wxMaxima version 21.11.0 ] */ +/* [wxMaxima: input start ] */ +/* +With the parameters a_ref and T² and the function f with derivatives and inverses, the formulas are: +*/ +s_max = a_ref * T²; +t = T * f_inv(s / s_max); +s(t,T) := s_max * f(t/T); +v(s,T) := s_max / T * (1 / f_inv_1(s / s_max)); +p(s,T) := T / s_max * f_inv_1(s / s_max); +a(t,T) := a_ref * f_2(t/T); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +It is important, that f_inv_1(x) can be calculated stable for x<<<1 over several decades well. + +Due to this a segmental definition could be useful. +For example x³ and cubic root can be calculated easily for several decades close to 0. +*/ +a:0.5 $ +b:1.46 $ +select(x) := x < a $ +seg1(x) :=1.295* x^3 $ +seg2(x) := 1-(1-seg1(a))*sin((1-x)*%pi/b)/sin((1-a)*%pi/b) $ +seg1_1(x):=diff(seg1(x),x)$ +seg2_1(x):=diff(seg2(x),x)$ +seg1_2(x):=diff(seg1(x),x,2)$ +seg2_2(x):=diff(seg2(x),x,2)$ +f(x) := if select(x) then seg1(x) else seg2(x) $ +[seg1_1(x),seg2_1(x)];wxplot2d([f(x),%[1],%[2]],[x,0,1]); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +[seg1_2(x),seg2_2(x)];wxplot2d([%[1],%[2]],[x,0,1]); +/* [wxMaxima: input end ] */ + + + +/* Old versions of Maxima abort on loading files that end in a comment. */ +"Created with wxMaxima 21.11.0"$ diff --git a/extras/doc/arch_esp32.txt b/extras/doc/arch_esp32.txt new file mode 100644 index 0000000..d5a3b48 --- /dev/null +++ b/extras/doc/arch_esp32.txt @@ -0,0 +1,62 @@ +StepperISR_esp32.cpp + +The driver makes use of mcpwm module(s) and pcnt modules. +The mcpwm generates the timing of the pulses. +The pcnt module counts fast pulses. + +The io-matrix of esp32 is configured, that the mcpwm generated +pulses are routed to the GPIO pad and those pulses are +fed back into the pulse counter. + +If the command queue contains a command with 0 steps aka pause, +then there is nothing to count and thus the mcpwm module interrupt +is enabled. Otherwise the mcpwm module interrupt is disabled +and the a pcnt interrupt is generated, when the number of steps is completed. + +Thereof the interrupt rate is max(n,1)*p ticks, +where n is the number of steps and p the ticks - as defined in the command. + +Timing: + +For steps = 0: =========================================================================== + +Timer 0 1 2 3 4 ... ticks-1 ticks ticks-1 ... 1 0 1 2 3 +TEA X X X +TEP X +STEP 0 0 0 0 0 0 0 0 0 ... +PWMint |---| (|-----|) in case of another pause +PCNTint + +Only PWM interrupt is triggered on time == 1. On down counting, a second event is generated two clock cycles later: too fast for two interrupts. + + +For steps = 1: =========================================================================== + +Timer 0 1 2 3 4 ... ticks-1 ticks ticks-1 ... 1 0 1 2 3 +CMPA X X +TEP X +STEP 0 1 1 1 1 1 0 0 0..... +PCNT 0 1 1 1 1 ............................ +PWMint +PCNTint |---| + +PCNT interrupt is triggered on time == 1. +On time == 1, the step output is triggered high and transition to 0 on ticks + +For steps = 2 =========================================================================== + +Timer 0 1 2 3 4 ... ticks ... 1 0 1 2 3...ticks ... 1 0 +CMPA X X +TEP X X +STEP 0 1 1 1 1 0 0 0 1 1 1... 0 0 0... +PCNT 0 1 1 1 1 1 1 1 2 2 2... +PWMint +PCNTint |---| + +PCNT interrupt is triggered on time == 1 and counter value 2. + +Important: mcpwm update method at TEZ + +Consequently: The ticks wait time is AFTER the pulse and started from L->H transition. +Thus the pulse of the following command starts after this command's period + diff --git a/extras/doc/generalization.wxm b/extras/doc/generalization.wxm new file mode 100644 index 0000000..17ba220 --- /dev/null +++ b/extras/doc/generalization.wxm @@ -0,0 +1,338 @@ +/* [wxMaxima batch file version 1] [ DO NOT EDIT BY HAND! ]*/ +/* [ Created with wxMaxima version 21.11.0 ] */ +/* [wxMaxima: input start ] */ +/* +For a constant acceleration ramp the steps and speed at time t can be calculated by: +*/ +s(t,a) := 1/2 * a * t^2; +v(t,a) := a * t; +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +The time at a given step is then +*/ +t(s,a) := sqrt(2 * s / a); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +and the speed at a given step is then +*/ +v(s,a) := sqrt(2 * s * a); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +and finally the step rate R (time distance between two pulses): +*/ +R(s,a) := 1 / sqrt(2 * s * a); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +With s_ramp being the number of steps to accelerate or decelerate and s_total being the number of the steps for the total ramp including acceleration and deceleration, the complete ramp with acceleration, coasting and deceleration can be written as +*/ +v(s,a, s_ramp, s_total) := if s < s_ramp then sqrt(2 * s * a) else if s < s_total-s_ramp then sqrt(2 * s_ramp * a) else if s < s_total then sqrt(2 * (s_total-s) * a) else 0 $ +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +And the acceleration over steps is simply +*/ +a(s,a, s_ramp, s_total) := if s < s_ramp then a else if s < s_total-s_ramp then 0 else if s < s_total then -a else 0 $ +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +As an example the acceleration over steps for acceleration = 5 m/s², ramp steps = 100 and total ramp steps = 1000: +*/ +wxplot2d([a(s,5,100,1000)],[s,0,1000]); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +wxplot2d([v(s,5,100,1000)],[s,0,1000]); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +In order to generalize the ramp function, we introduce a dimensionless function f, which translates from range [0,1] into the range [0,1]. +*/ +v(s) := v_max * f(s/s_ramp); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +The first derivate of v aka v_1 is with df/fx = f_1: +*/ +v_1(s) := v_max/s_ramp * f_1(s/s_ramp); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Using this definition, the acceleration a(s) can be approximated at steps s: +*/ +a(s) := v_1(s) * v(s); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +With these two functions a(s) is: +*/ +a(s); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Now we need a mathematical function, which can be used to define a smooth acceleration ramp without jumps. +tanh(x) poses problem for the x-range. The smoothstep function starts at x=0 too slow. As we are controlling the speed, +we already get one factor of x. So idea is to have a function coming out linearly from x with a maximum at x=1. + +This time sine function with its first derivative to be used: +*/ +f(x) := sin(x * %pi/2); +diff(f(x),x); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +So the first derivative is simply +*/ +f_1(x) :=%pi*cos(%pi*x/2)/2; +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Drawn over the x-range from 0 to 1: +*/ +wxplot2d([f(x),f_1(x)],[x,0,1]); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +How does this apply to our functions v and a as dependent from step s ? +*/ +fundef(v); fundef(a); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +With this the function to calculate the acceleration is (with constant factors eliminated): +*/ +expand(a(s)); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +The shape of a(s) is: +*/ +wxplot2d([f(x)*f_1(x)],[x,0,1]); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +The maximum of this function is c at x_c +*/ +x_c : 0.5; +c : f(x_c)*f_1(x_c); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +The maximum value of the function is pi/4 +*/ +float(c);float(%pi/4); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Consequently the maximum acceleration is: +*/ +a_max = v_max² / s_ramp * %pi/4; + +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +In FastAccelStepper maximum acceleration and maximum speed are configured, so this equation can be used to determine s_ramp. +*/ +ref: s_ramp = %pi/4 * v_max² / a_max; +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +The speed at step s = 1 is: +*/ +ev(v(s), s=1,ref); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +As sin(x) ~ x for x << 1 the speed at step s=1 is: +*/ +2*a_max/v_max; +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +For reference: with constant acceleration the speed at step s=1 is independent of v_max +*/ +v = sqrt(2*a); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +The issue is, that the relation v(s) in this manner cannot be applied, if the speed shall change from v_max to -v_max. In this scenario, the acceleration at v = 0 aka the turn point shall be at maximum during deceleration/acceleration phase. + +Second problem is an adequate forecast, if at turnpoint the acceleration needs to be reduced in order to not overshoot the targetted speed. +*/; +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +From application point of view, it is better to define acceleration as dependent of time and mathematically being a second derivative: +*/ +a(t,T) := a_ref * f_2(t/T); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Using this approach speed and distance can be deducted easily: +*/ +v(t,T) := T * a_ref * f_1(t/T); +s(t,T) := T² * a_ref * f(t/T); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +with f_inv(x) = y as solution for f(y) = x, time can be derived as dependent of distance: +*/ +t = T * f_inv(s / (a_ref*T²)); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Using this approach v(t,T) can be stated as dependent of s: +*/ +v(s,T) := T * a_ref * f_1(f_inv(s / (a_ref*T²))); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Analyzing the first derivative as function of its inverse: +*/ +F_1(F_inv(y)) = d/dx * F(x = F_inv(y)) ; +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +F_1(F_inv(y)) = d/dy * dy/dx* F(x = F_inv(y)) $ +d/dy * F(x = F_inv(y)) = 1 $ +F_1(F_inv(y)) = dy/dx $ +F_1(F_inv(y)) = 1/F_inv_1(y) ; +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Applied to v(s,T): +*/ +v(s,T) := T * a_ref / f_inv_1(s / (a_ref*T²)); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +And for the period being 1/v: +*/ +p(s,T) := f_inv_1(s / (a_ref*T²)) / (T*a_ref); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Using s_max = a_ref * T² this can be rewritten to: +*/ +p(s,T) := T / s_max * f_inv_1(s / s_max); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +cross check with constant acceleration. => OK +*/ +f_2(x) = 1; +f(x) = x²/2; +f_inv(x) = sqrt(2*x); +f_inv_1(x) = 1/sqrt(2*x); +p(s,T) := 1/sqrt(2*s/a_ref/T²)/(T*a_ref) $ +p(s,T) := sqrt(a_ref*T²)/sqrt(2*s)/(T*a_ref) $ +p(s,T) := 1/sqrt(2*s *a_ref); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +as the math is ok. So try the sine function for ramp up of the speed +*/ +f(x) := sin(x*%pi/2); +wxplot2d([f(x)],[x,0,1]); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +solve(y=f(x),x); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +diff(rhs(%[1]),y); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +wxplot2d([%],[y,0.1,0.9]); +/* [wxMaxima: input end ] */ + + +/* [wxMaxima: input start ] */ +/* +Looks ok, too. Just the cosine will generate maximum acceleration at ramp start and end. So another function is needed. +The smoothstep function would be interesting, but the inverse is algebraically very complex. +*/ +/* [wxMaxima: input end ] */ + + + +/* Old versions of Maxima abort on loading files that end in a comment. */ +"Created with wxMaxima 21.11.0"$ diff --git a/extras/doc/ramp.md b/extras/doc/ramp.md new file mode 100644 index 0000000..c031ff5 --- /dev/null +++ b/extras/doc/ramp.md @@ -0,0 +1,125 @@ +# Ramp generator + +# Introduction + +The ramp generator increases the speed linearly with the acceleration. +If e.g. a=1000 steps/s², then after 1s the speed is 1000 steps/s. + +The full ramp consists of three phases: acceleration+coasting+deceleration. +If the number of steps are less than the required steps for acceleration and deceleration, then there will not be any coasting phase. + +For illustration here an ascii chart of the ramp + +``` +Speed + | ------------------- + | / \ + | / \ + | / \ + 0---/ \------ + |<->|<--------------->|<->| + Ta Tc Td + -----------------------------------> Time + +Ta ... acceleration time. +Tc ... coasting time. +Td ... deceleration time. +``` + +The total ramp time is Ta + Tc + Td = T + +Due to acceleration = deceleration, the related times Ta and Td are same. +$$ +T=2*Ta+Tc +$$ + +The steps executed in coasting are simply: +$$ +steps_{coasting} = v * Tc +$$ + +The steps executed during acceleration and deceleration are: +$$ +steps_{acceleration} = 0.5 * a * Ta² +$$ + +The total steps are: +$$ +\begin{align} +steps &= steps_{acceleration} + steps_{coasting} + steps_{deceleration} \\ + &= 2 * steps_{acceleration} + steps_{coasting} \\ + &= a * Ta² + v * Tc \\ + &= a * Ta² + (a * Ta) * Tc \\ + &= a * Ta * (Ta + Tc) \\ + &= a * Ta * (T - Ta) +\end{align} +$$ +This can be rearranged to calculate the required acceleration to perform `steps` during the total ramp time T and acceleration time Ta: +$$ +a = \frac{steps}{ Ta * (T - Ta) } +$$ + +# Calculation example + +The stepper motor should perform 32000 steps in 10s. + +The required acceleration and speed for different acceleration times are calculated as this: +$$ +\begin{align} +Ta = 1s => a &= \frac{32000}{1 * 9} steps/s² = 3556 steps/s² \\ + v &= a*Ta = 3556 steps/s => 281us/step \\ +Ta = 2s => a &= \frac{32000}{2 * 8} steps/s² = 2000 steps/s² \\ + v &= a*Ta = 4000 steps/s => 250us/step \\ +Ta = 3s => a &= \frac{32000}{3 * 7} steps/s² = 1524 steps/s² \\ + v &= a*Ta = 4571 steps/s => 218us/step \\ +Ta = 4s => a &= \frac{32000}{4 * 6} steps/s² = 1333 steps/s² \\ + v &= a*Ta = 5333 steps/s => 188us/step \\ +Ta = 5s => a &= \frac{32000}{5 * 5} steps/s² = 1280 steps/s² \\ + v &= a*Ta = 6400 steps/s => 156us/step +\end{align} +$$ + +As commands for the StepperDemo: +``` + M1 A3556 V281 R32000 + M1 A2000 V250 R32000 + M1 A1524 V218 R32000 + M1 A1333 V188 R32000 + M1 A1280 V156 R32000 +``` + +# Special case + +The minimum command time for a bundle of steps is `MIN_CMD_TICKS`. The ramp generator need to make sure, +that any command issued to the command queue meets this requirement. + +With high acceleration this can get problematic coming from or going to stand still. + +The required condition to meet this requirement is: +$$ + steps * \frac{1}{v} \ge T_{CMD} +$$ + +From stand still the speed after `MIN_CMD_TICKS`is: +$$ + v_{start} = a * T_{CMD} +$$ + +So the related condition for steps is: +$$ +\begin{align} + steps * \frac{1}{a * T_{CMD}} &\ge T_{CMD} \\ + steps &\ge a * T_{CMD}^2 +\end{align} +$$ + +Based on this there can be deducted two problems: + +1. Any move command with less steps cannot be fulfilled with this acceleration + + => Remedy: Run these steps at maximum allowed speed based on `MIN_CMD_TICKS` + +2. While ramping down the last issued command may violate this condition + + => Remedy: Run these steps at maximum allowed speed based on `MIN_CMD_TICKS`. + The problem is to distinguish this case from a legitimate overshoot situation. diff --git a/extras/doc/ramp_cubic_quadratic.md b/extras/doc/ramp_cubic_quadratic.md new file mode 100644 index 0000000..749d491 --- /dev/null +++ b/extras/doc/ramp_cubic_quadratic.md @@ -0,0 +1,63 @@ +# Ramp generator concept for cubic/quadratic ramp combination + +Piecewise ramp: +- ramp up with s = 1/6 * j * t^3 and v = 1/2 * j * t^2 + so t = sqrt(2*v/j) and s = 1/6 * j * (2*v/j)^(3/2) + and (6*s/j)^(2/3) = 2*v / j + and v = j/2 * (6/j)^(2/3) * s^(2/3) = 6^(2/3)/2 * j^(1/3) * s^(2/3) +- define hand over point s_h +- ramp up to max speed with s = 1/2 * a * t'^2 = 1/2 * v^2 / a + +Conditions at hand over point s_h: +- s_h = 1/6 * j * t_h^3 = 1/2 * a * t'_h^2 + ds +- v_h = 1/2 * j * t_h^2 = a * t'_h +- a_h = j * t_h = a + +Replacing j = a / t_h: +- s_h = 1/6 * a * t_h^2 = 1/2 * a * t'_h^2 + ds +- v_h = 1/2 * a * t_h = a * t'_h + +With t'_h = t_h - dt: +- s_h = 1/6 * a * t_h^2 = 1/2 * a * (t_h^2 - 2*t_h*dt + dt^2) + ds +- v_h = 1/2 * a * t_h = a * t_h - a * dt + +Consequently from v_h: + dt = th/2 + +And for s: +- s_h = 1/6 * a * t_h^2 = 1/8 * a * t_h^2 + ds + +So ds: + ds = (8-6)/48 * a * t_h^2 = 1/24 * a * t_h^2 = s_h/4 + +So we have clear relation from s to the ramp speed: + + if s < s_h, then cubic ramp: v(s) = 6^(2/3)/2 * j^(1/3) * s^(2/3) + if s >= s_h, then quadratic ramp: v(s) = sqrt(2 * a * (s-s_h/4)) + +Still need to reduce j and s_h to one parameter of freedom. +If choose s_h as user defined parameter, then: + + t_h = sqrt(6 * s_h / a) + +and so: + + j = a / t_h = a / sqrt(6 * s_h / a) = sqrt(a^3 / (6 * s_h)) + +For the speed calculation cubic ramp, this yields: + + v(s) = [6^(2/3)/2 * (a^3/6/s_h)^(1/6)] * s^(2/3) + = [6^(4/6)/2 * (a^3)^(1/6) * 6^(-1/6) * s_h^(-1/6)] * s^(2/3) + = [6^(3/6)/2 * sqrt(a) * s_h^(-1/6)] * s^(2/3) + = [sqrt(6)/2 * sqrt(a) * s_h^(-1/6)] * s^(2/3) + = [sqrt(3/2) * sqrt(a) / s_h^(1/6) ] * s^(2/3) + +Check cubic: + v(s_h) = [sqrt(3/2) * sqrt(a) / s_h^(1/6) ] * s_h^(2/3) + = [sqrt(3/2) * sqrt(a) ] * s_h^(1/2) + +and quadratic: + v(s_h) = sqrt(2 * a * s_h * 3 / 4) = sqrt(3/2 * a * s_h) equals cubic + +With s_h = 0 there will be no cubic ramp start. + diff --git a/extras/gen_pmf_const/Makefile b/extras/gen_pmf_const/Makefile new file mode 100644 index 0000000..9a37a60 --- /dev/null +++ b/extras/gen_pmf_const/Makefile @@ -0,0 +1,17 @@ +CPPFLAGS=-I../../src + +../../src/PoorManFloatConst.h: main + ./main >../../src/PoorManFloatConst.h + +main: main.o PoorManFloat.o + +main.o: main.cpp + +PoorManFloat.o: PoorManFloat.cpp + +PoorManFloat.cpp: ../../src/PoorManFloat.cpp + cp ../../src/PoorManFloat.cpp PoorManFloat.cpp + +clean: + rm -f PoorManFloat.cpp + rm -f main main.o PoorManFloat.o diff --git a/extras/gen_pmf_const/README.md b/extras/gen_pmf_const/README.md new file mode 100644 index 0000000..234f74e --- /dev/null +++ b/extras/gen_pmf_const/README.md @@ -0,0 +1,4 @@ +# generate pmf constants + +Those constants are stored in PoorManFloatConst.h and are autogenerated +by invoking make. This builds main from main.cpp. diff --git a/extras/gen_pmf_const/main.cpp b/extras/gen_pmf_const/main.cpp new file mode 100644 index 0000000..b742703 --- /dev/null +++ b/extras/gen_pmf_const/main.cpp @@ -0,0 +1,86 @@ +#include +#include + +#include + +void out(const char *name, float x, pmf_logarithmic value) { + float ld2 = log2(x); + float pmf = ld2 * 512; + int16_t pmf_int = (uint16_t)round(pmf); + puts(""); + printf("// %s = %f %d = 0x%04x\n", name, pmf, pmf_int, pmf_int); + if (pmf_int != value) { + printf("// %s should be 0x%04x\n", name, pmf_int); + } + printf("#define %s ((pmf_logarithmic)0x%04x)\n", name, value); + float back = pow(2.0, float(pmf_int) / 512); + printf("// => converted back => %.2f\n", back); +} + +int main() { + puts("// Autogenerated by extras/gen_pmf_const/main"); + puts("// DO NOT EDIT"); + puts("#ifndef POORMANFLOATCONST_H"); + puts("#define POORMANFLOATCONST_H"); + puts(""); + puts("#include "); + + pmf_logarithmic x; + + x = pmfl_from((uint8_t)1); + out("PMF_CONST_1", 1.0, x); + + x = pmfl_shr(pmfl_from((uint8_t)3), 1); + out("PMF_CONST_3_DIV_2", 1.5, x); + + x = pmfl_multiply(pmfl_from((uint32_t)16e6), pmfl_from((uint32_t)8e6)); + out("PMF_CONST_128E12", 1.28e14, x); + + x = pmfl_from((uint32_t)16e6); + out("PMF_CONST_16E6", 1.6e7, x); + + x = pmfl_from((uint16_t)500); + out("PMF_CONST_500", 500, x); + + x = pmfl_from((uint16_t)1000); + out("PMF_CONST_1000", 1000, x); + + x = pmfl_from((uint16_t)2000); + out("PMF_CONST_2000", 2000, x); + + x = pmfl_from((uint16_t)32000); + out("PMF_CONST_32000", 32000, x); + + x = pmfl_multiply(pmfl_from((uint32_t)16e6), pmfl_from((uint32_t)8e6)); + x = pmfl_sqrt(x); + out("PMF_CONST_16E6_DIV_SQRT_OF_2", 1.6e7 / sqrt(2), x); + + x = pmfl_from((uint32_t)21e6); + out("PMF_CONST_21E6", 21e6, x); + + x = pmfl_from((uint16_t)42000); + out("PMF_CONST_42000", 42000, x); + + x = pmfl_divide(pmfl_from((uint32_t)21e6), pmfl_sqrt(pmfl_from((uint8_t)2))); + out("PMF_CONST_21E6_DIV_SQRT_OF_2", 21e6 / sqrt(2), x); + + x = pmfl_square(pmfl_from((uint32_t)21e6)); + x = pmfl_shr(x, 1); + out("PMF_CONST_2205E11", 2.205e14, x); + + x = pmfl_from(__UINT32_MAX__); + out("PMF_CONST_UINT32_MAX", __UINT32_MAX__, x); + + x = pmfl_from((uint16_t)__UINT16_MAX__); + out("PMF_CONST_UINT16_MAX", __UINT16_MAX__, x); + + puts(""); + puts("// used in PoorManFloat.cpp as example"); + printf("// "); + x = pmfl_from((uint16_t)15373); + out("PMF_CONST_15373", 15373, x); + + puts("#endif"); + + return 0; +} diff --git a/extras/idf_examples/UsageExample/UsageExample.cpp b/extras/idf_examples/UsageExample/UsageExample.cpp new file mode 100644 index 0000000..13de17a --- /dev/null +++ b/extras/idf_examples/UsageExample/UsageExample.cpp @@ -0,0 +1,78 @@ +#include "FastAccelStepper.h" + +#include + +// As in StepperDemo for Motor 1 on ESP32 +#define dirPinStepper 18 +#define enablePinStepper 26 +#define stepPinStepper 17 + +FastAccelStepperEngine engine = FastAccelStepperEngine(); +FastAccelStepper *stepper = NULL; + +void setup() { + printf("START\n"); + + engine.init(0); + + printf("Engine initialized\n"); + + for (uint8_t i = 0; i < 10; i++) { + printf("LOOP %d\n", i); + vTaskDelay(pdMS_TO_TICKS(500)); + // esp_task_wdt_reset(); + } + stepper = engine.stepperConnectToPin(stepPinStepper); + + printf("Stepper connected\n"); + for (uint8_t i = 0; i < 10; i++) { + printf("LOOP %d\n", i); + vTaskDelay(pdMS_TO_TICKS(500)); + // esp_task_wdt_reset(); + } + + 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(1000); + + stepper->setSpeedInUs(1000); // the parameter is us/step !!! + stepper->setAcceleration(100); + +#ifdef SUPPORT_ESP32_PULSE_COUNTER + stepper->attachToPulseCounter(7); +#endif + + printf("Stepper initialized\n"); + } else { + printf("No stepper\n"); + } +} + +extern "C" void app_main() { + setup(); + int32_t target = 0; + while (true) { + while (stepper->isRunning()) { + // esp_task_wdt_reset(); + printf("pos=%" PRId32, stepper->getCurrentPosition()); +#ifdef SUPPORT_ESP32_PULSE_COUNTER + int16_t pcnt = stepper->readPulseCounter(); + printf(" pcnt=%d", pcnt); +#endif + printf("\n"); + vTaskDelay(pdMS_TO_TICKS(500)); + } + printf("done\n"); + vTaskDelay(pdMS_TO_TICKS(500)); + printf("move\n"); + target = 1000 - target; + stepper->moveTo(target); + } + // WARNING: if program reaches end of function app_main() the MCU will + // restart. +} diff --git a/extras/scripts/build-idf-platformio.sh b/extras/scripts/build-idf-platformio.sh new file mode 100755 index 0000000..0efccdb --- /dev/null +++ b/extras/scripts/build-idf-platformio.sh @@ -0,0 +1,39 @@ +#!/bin/sh + +TARGETS=${1:-espidf} +echo "execute for ${TARGETS}" + +if [ "$GITHUB_WORKSPACE" != "" ] +then + # Make sure we are inside the github workspace + cd $GITHUB_WORKSPACE +fi + +# Whatever this script is started from, cd to the top level +ROOT=`git rev-parse --show-toplevel` +cd $ROOT + +# install platformio, if needed +which pio +if [ $? -ne 0 ] +then + # Install PlatformIO CLI + export PATH=$PATH:~/.platformio/penv/bin + curl -fsSL https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py -o get-platformio.py + python3 get-platformio.py + + # Use automated install from pio run + # pio platform install "atmelavr" + # pio platform install "atmelsam" + # pio platform install "espressif32" +fi + +set -e +for i in pio_espidf/* +do + for p in ${TARGETS} + do + echo $p: $i + (cd $i;pio run -s -e $p) + done +done diff --git a/extras/scripts/build-pio-dirs.sh b/extras/scripts/build-pio-dirs.sh new file mode 100755 index 0000000..02d8386 --- /dev/null +++ b/extras/scripts/build-pio-dirs.sh @@ -0,0 +1,59 @@ +#!/bin/sh + +echo "build directories" + +if [ "$GITHUB_WORKSPACE" != "" ] +then + # Make sure we are inside the github workspace + cd $GITHUB_WORKSPACE +fi + +# Whatever this script is started from, cd to the top level +ROOT=`git rev-parse --show-toplevel` +cd $ROOT + +pwd + +# So create the pio_dirs-directory during the github action +rm -fR pio_dirs +mkdir pio_dirs +for i in `ls examples` +do + mkdir -p pio_dirs/$i/src + cd pio_dirs/$i + ln -s ../../extras/ci/platformio.ini . + cd src + FILES=`cd ../../../examples/$i;find . -type f` + for f in $FILES;do ln -s ../../../examples/$i/$f .;done + cd ../../.. +done + +# for espidf as of now, the src/* files need to be linked into the example build directory +rm -fR pio_espidf +mkdir pio_espidf +for i in `cd extras;ls idf_examples` +do + mkdir -p pio_espidf/$i/src + cd pio_espidf/$i + ln -s ../../extras/ci/platformio.ini . + cd src + FILES=`cd ../../../extras/idf_examples/$i;find . -type f` + for f in $FILES;do ln -s ../../../extras/idf_examples/$i/$f .;done + cd ../../.. +done +mkdir -p pio_espidf/StepperDemo/src +(cd pio_espidf/StepperDemo;ln -s ../../extras/ci/platformio.ini;cd src;cp ../../../examples/StepperDemo/* .;mv StepperDemo.ino StepperDemo.cpp) + +# Make one directory to test PoorManFloat on simulator +mkdir pio_dirs/PMF_test +mkdir pio_dirs/PMF_test/src +cd pio_dirs/PMF_test +ln -s ../../extras/ci/platformio.ini . +cd src +#sed -e 's/%d/%ld/g' <../../../tests/test_03.h >test_03.h +ln -s ../../../extras/tests/pc_based/test_03.h . +ln -s ../../../extras/tests/pc_based/PMF_test.ino PMF_test.ino +cd ../../.. + +ls -al pio_* + diff --git a/extras/scripts/build-platformio.sh b/extras/scripts/build-platformio.sh new file mode 100755 index 0000000..73cab5c --- /dev/null +++ b/extras/scripts/build-platformio.sh @@ -0,0 +1,43 @@ +#!/bin/sh + +TARGETS=${1:-nanoatmega168 nanoatmega328 atmega2560 esp32 esp32s2 esp32c3 atmelsam atmega32u4} +echo "execute for ${TARGETS}" + +if [ "$GITHUB_WORKSPACE" != "" ] +then + # Make sure we are inside the github workspace + cd $GITHUB_WORKSPACE +fi + +# Whatever this script is started from, cd to the top level +ROOT=`git rev-parse --show-toplevel` +cd $ROOT + +# install platformio, if needed +which pio +if [ $? -ne 0 ] +then + # Install PlatformIO CLI + export PATH=$PATH:~/.platformio/penv/bin + curl -fsSL https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py -o get-platformio.py + python3 get-platformio.py + + # Use automated install from pio run + # pio platform install "atmelavr" + # pio platform install "atmelsam" + # pio platform install "espressif32" +fi + +set -e +for i in pio_dirs/* +do + for p in ${TARGETS} + do + if [ "$p" = "nanoatmega168" ] && [ "$i" = "pio_dirs/StepperDemo" ]; then + echo $p: Skipping $i for $p due to space constraints + continue + fi + echo $p: $i + (cd $i;pio run -s -e $p) + done +done diff --git a/extras/scripts/format_code.sh b/extras/scripts/format_code.sh new file mode 100755 index 0000000..c226637 --- /dev/null +++ b/extras/scripts/format_code.sh @@ -0,0 +1,11 @@ +#!/bin/sh + +PRJ_ROOT=`git rev-parse --show-toplevel` +VERSION=`git rev-parse --short HEAD` + +FILES=`find ${PRJ_ROOT} -type f -name '*.ino' -or -type f -name '*.cpp' -or -type f -name '*.h'` + +clang-format -style="{BasedOnStyle: Google, SortIncludes: false}" -i ${FILES} +echo ${VERSION} +sed -i -e 's/#define VERSION.*$$/#define VERSION "post-$(VERSION)"/' ${PRJ_ROOT}/examples/StepperDemo/StepperDemo.ino + diff --git a/extras/scripts/header2markdown.sh b/extras/scripts/header2markdown.sh new file mode 100755 index 0000000..8fc2816 --- /dev/null +++ b/extras/scripts/header2markdown.sh @@ -0,0 +1,50 @@ +#!/bin/sh + +# This extract all lines with C-comments // as plain text +# Places all C-lines in quotes + +gawk ' +BEGIN { + in_c_header = 1 + in_code = 0 + quote_code = 0 +} + +/#include/ { next } +(NF != 0) && (in_c_header == 1) { next } +/\/\/ *clang-format/ { next } + +{ in_c_header = 0 } + +(NF == 0) { + if (in_code == 1) { + print("```") + } + in_code = 0 + quote_code = 0 + next +} + +/\/\// { + if (in_code == 1) { + print("```") + } + + gsub("[ \t]*// ?","") + print + in_code = 0 + quote_code = 1 + next +} + +(quote_code == 1) { + print("```cpp") + in_code = 1 + quote_code = 0 +} + +in_code == 1 { + gsub("inline ","") + print +} +' ../../src/FastAccelStepper.h >../doc/FastAccelStepper_API.md diff --git a/extras/tests/esp32_hw_based/Makefile b/extras/tests/esp32_hw_based/Makefile new file mode 100644 index 0000000..346513e --- /dev/null +++ b/extras/tests/esp32_hw_based/Makefile @@ -0,0 +1,29 @@ +all: compile test + +DEV=ttyUSB0 + +test: M1.test M7.test + +%.test: + MOTOR=$* ./seq_04.sh $(DEV) + MOTOR=$* ./seq_05.sh $(DEV) + MOTOR=$* ./seq_06.sh $(DEV) + MOTOR=$* ./seq_01a.sh $(DEV) + MOTOR=$* ./seq_01b.sh $(DEV) + MOTOR=$* ./seq_02.sh $(DEV) + MOTOR=$* ./seq_07a.sh $(DEV) + MOTOR=$* ./seq_07b.sh $(DEV) + MOTOR=$* ./seq_07c.sh $(DEV) + MOTOR=$* ./seq_03.sh $(DEV) + +compile: + (cd ../../../pio_dirs/StepperDemo;rm -fR .pio; pio run -e esp32_V6_8_1 -t upload --upload-port /dev/$(DEV)) + +compile_idf4: + (cd ../../../pio_espidf/StepperDemo;rm -fR .pio; pio run -e esp32_idf_V5_3_0 -t upload --upload-port /dev/$(DEV)) + +compile_idf5: + (cd ../../../pio_espidf/StepperDemo;rm -fR .pio; pio run -e esp32_idf_V6_8_1 -t upload --upload-port /dev/$(DEV)) + +clean: + rm -f seq*.log diff --git a/extras/tests/esp32_hw_based/judge_pcnt_sync.awk b/extras/tests/esp32_hw_based/judge_pcnt_sync.awk new file mode 100644 index 0000000..eb8bc5a --- /dev/null +++ b/extras/tests/esp32_hw_based/judge_pcnt_sync.awk @@ -0,0 +1,51 @@ +BEGIN { + pass = 1 +} + +# This is for running motor +/^M[17]:/ { + api = substr($2,2) + pcnt = substr($3,2,length($3)-2) + if (pcnt < 0) { + if (api > 0) { + while (pcnt < 0) { + pcnt += 32767 + } + } + } + api = api % 32767 + delta = pcnt - api + if (api > pcnt) { + delta = api - pcnt + } + if ((delta > 66) && (delta < 32767-66)) { + print + print api, pcnt + pass = 0 + print "FAIL HERE ^^^" + } +} +# This is for selected motor +/^>> M[17]:/ { + print + api = substr($3,2) + api = api % 32767 + pcnt = substr($4,2,length($4)-2) + if (pcnt < 0) { + pcnt += 32767 + } + if (api != pcnt) { + print api, pcnt + pass = 0 + print "FAIL HERE ^^^" + } +} + +END { + if (pass) { + print "PASS" + } + else { + print "FAIL" + } +} diff --git a/extras/tests/esp32_hw_based/seq_01a.sh b/extras/tests/esp32_hw_based/seq_01a.sh new file mode 100755 index 0000000..3a2e7a9 --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_01a.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 H25000 A10000 f w1000 X w100 pc R100 w100 W R1000 w1000 W " +PASS=">> $MOTOR: @1100 \\[1100\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 10 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_01b.sh b/extras/tests/esp32_hw_based/seq_01b.sh new file mode 100755 index 0000000..78fb035 --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_01b.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 H25000 A10000 f w1000 X W pc R100 w100 W R1000 w1000 W " +PASS=">> $MOTOR: @1100 \\[1100\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 10 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_01c.sh b/extras/tests/esp32_hw_based/seq_01c.sh new file mode 100755 index 0000000..107b580 --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_01c.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 H25000 A10000 f w10 X W pc w 20 R1 W " +PASS=">> $MOTOR: @1 \\[1\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 3 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 10 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else +# grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_02.sh b/extras/tests/esp32_hw_based/seq_02.sh new file mode 100755 index 0000000..c6321f2 --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_02.sh @@ -0,0 +1,32 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 H30000 A100000 R30000 " +PASS=">> $MOTOR: @30000 \\[30000\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 3 -o $LOG +echo + +if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +then + grabserial $DEV -c 'r ' -q StepperDemo -e 1 + echo + echo FAIL $0 pulse counter mismatch + exit 1 +fi +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi diff --git a/extras/tests/esp32_hw_based/seq_03.sh b/extras/tests/esp32_hw_based/seq_03.sh new file mode 100755 index 0000000..02fe09f --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_03.sh @@ -0,0 +1,43 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +COMPLETE="test completed" +PASS="test passed" +MAX_RUN_S=300 + +# rmt has failed once 07, but repetitions are ok +for SEQ in 13 01 02 03 04 06 07 10 11 +do + LOG="$0_$SEQ.log" + CMD="$MOTOR p7,-32767,32767 t $MOTOR $SEQ R " + + echo "reset esp32" + grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 + sleep 2 + + echo "send commands" + grabserial $DEV -c "$CMD" -q "$COMPLETE" -e $MAX_RUN_S -o $LOG + echo + + if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] + then + grabserial $DEV -c 'r ' -q StepperDemo -e 1 + echo + echo FAIL $0 pulse counter mismatch + echo "test sequence $SEQ" + exit 1 + fi + + if [ `grep -c "$PASS" $LOG` -eq 1 ] + then + echo PASS + else + grabserial $DEV -c ' x r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + echo "test sequence $SEQ" + exit 1 + fi +done diff --git a/extras/tests/esp32_hw_based/seq_04.sh b/extras/tests/esp32_hw_based/seq_04.sh new file mode 100755 index 0000000..1310207 --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_04.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 H25000 A10000 R1 W " +PASS=">> $MOTOR: @1 \\[1\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 3 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_05.sh b/extras/tests/esp32_hw_based/seq_05.sh new file mode 100755 index 0000000..cc427a2 --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_05.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR H25000 A10000 R1000 w200 X W p7,-32767,32767 ? R10 w1000 " +PASS=">> $MOTOR: @10 \\[10\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 3 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_06.sh b/extras/tests/esp32_hw_based/seq_06.sh new file mode 100755 index 0000000..acbbe0c --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_06.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 V40 A1000000 R54 W " +PASS=">> $MOTOR: @54 \\[54\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 3 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_07a.sh b/extras/tests/esp32_hw_based/seq_07a.sh new file mode 100755 index 0000000..1cc34af --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_07a.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 V40 A1000 R54 W R-54 W" +PASS=">> $MOTOR: @0 \\[0\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 3 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_07b.sh b/extras/tests/esp32_hw_based/seq_07b.sh new file mode 100755 index 0000000..b64b53f --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_07b.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 V40 A1000 R20 W R-19 W" +PASS=">> $MOTOR: @1 \\[1\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 3 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_07c.sh b/extras/tests/esp32_hw_based/seq_07c.sh new file mode 100755 index 0000000..b39cc6f --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_07c.sh @@ -0,0 +1,34 @@ +#!/bin/sh +TTY=${1:-ttyUSB0} +DEV="-d /dev/${TTY} -b 115200" +MOTOR=${MOTOR:-M1} + +CMD="$MOTOR p7,-32767,32767 V40 A1000 R20 W " +PASS=">> $MOTOR: @20 \\[20\\]" + +LOG="$0.log" + +grabserial $DEV -c ' x reset ' -q "$MOTOR:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e 3 -o $LOG +echo + +#if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +#then +# grabserial $DEV -c 'r ' -q StepperDemo -e 1 +# echo +# echo FAIL $0 pulse counter mismatch +# exit 1 +#fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c ' r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/seq_xxx.sh.off b/extras/tests/esp32_hw_based/seq_xxx.sh.off new file mode 100755 index 0000000..e6ca065 --- /dev/null +++ b/extras/tests/esp32_hw_based/seq_xxx.sh.off @@ -0,0 +1,33 @@ +#!/bin/sh +DEV="-d /dev/ttyUSB0 -b 115200" + +CMD="M1 p7,0,0 t M1 07 R " +PASS="Test passed" +MAX_RUN_S=10 + +LOG="$0.log" + +grabserial $DEV -c 'reset ' -q "M1:" -e 10 +sleep 2 + +grabserial $DEV -c "$CMD" -q "$PASS" -e $MAX_RUN_S -o $LOG +echo + +if [ `gawk -f judge_pcnt_sync.awk $LOG | grep -c PASS` -ne 1 ] +then + grabserial $DEV -c 'r ' -q StepperDemo -e 1 + echo + echo FAIL $0 pulse counter mismatch + exit 1 +fi + +if [ `grep -c "$PASS" $LOG` -eq 1 ] +then + echo PASS +else + grabserial $DEV -c 'r ' -q StepperDemo -e 1 + echo + echo "FAIL $0 result pattern: $PASS" + exit 1 +fi + diff --git a/extras/tests/esp32_hw_based/test_all.sh b/extras/tests/esp32_hw_based/test_all.sh new file mode 100644 index 0000000..2006569 --- /dev/null +++ b/extras/tests/esp32_hw_based/test_all.sh @@ -0,0 +1,13 @@ +#!/bin/sh + +# arduino idf4 based test +make compile +make M1.test M7.test + +# idf4 based test +make compile_idf4 +make M1.test M7.test + +# idf5 only rmt-module supporting 8 steppers +make compile_idf5 +make M1.test diff --git a/extras/tests/pc_based/Makefile b/extras/tests/pc_based/Makefile new file mode 100644 index 0000000..8f33804 --- /dev/null +++ b/extras/tests/pc_based/Makefile @@ -0,0 +1,58 @@ + +PRJ_ROOT=$(shell git rev-parse --show-toplevel) +CFLAGS=-DTEST -Werror -g -I$(PRJ_ROOT)/src +CXXFLAGS=-DTEST -Werror -g -DF_CPU=16000000 -I$(PRJ_ROOT)/src +LDLIBS=-lm -lc + +TESTS=$(basename $(wildcard test_??.cpp)) + +test: $(TESTS) pmf_test rmc_test + ./rmc_test + ./pmf_test + rm -f test.log + $(addsuffix >>test.log &&,$(addprefix ./,$(TESTS))) echo "All tests passed" + +LIB_H=FastAccelStepper.h PoorManFloat.h StepperISR.h \ + RampGenerator.h RampConstAcceleration.h RampCalculator.h \ + fas_arch/common.h +LIB_O=FastAccelStepper.o PoorManFloat.o StepperISR_test.o \ + RampGenerator.o RampConstAcceleration.o RampCalculator.o StepperISR.o + +SRC_LIB_H=$(addprefix $(PRJ_ROOT)/src/,$(LIB_H)) + +test_%: test_%.o $(LIB_O) + gcc -o $@ $< $(LIB_O) $(LDLIBS) + +test_%.o: test_%.cpp $(SRC_LIB_H) RampChecker.h stubs.h + g++ -c $(CXXFLAGS) -o $@ $< + +pmf_test: pmf_test.o PoorManFloat.o +pmf_test.o: pmf_test.cpp $(PRJ_ROOT)/src/PoorManFloat.h stubs.h test_03.h + +rmc_test: rmc_test.o PoorManFloat.o RampCalculator.o +rmc_test.o: rmc_test.cpp $(PRJ_ROOT)/src/PoorManFloat.h $(PRJ_ROOT)/src/RampCalculator.h stubs.h test_03.h + +FastAccelStepper.o: $(PRJ_ROOT)/src/FastAccelStepper.cpp $(SRC_LIB_H) + $(COMPILE.cpp) $< -o $@ + +PoorManFloat.o: $(PRJ_ROOT)/src/PoorManFloat.cpp $(PRJ_ROOT)/src/PoorManFloat.h + $(COMPILE.cpp) $< -o $@ + +RampGenerator.o: $(PRJ_ROOT)/src/RampGenerator.cpp $(SRC_LIB_H) + $(COMPILE.cpp) $< -o $@ + +RampConstAcceleration.o: $(PRJ_ROOT)/src/RampConstAcceleration.cpp $(SRC_LIB_H) + $(COMPILE.cpp) $< -o $@ + +RampCalculator.o: $(PRJ_ROOT)/src/RampCalculator.cpp $(SRC_LIB_H) + $(COMPILE.cpp) $< -o $@ + +StepperISR.o: $(PRJ_ROOT)/src/StepperISR.cpp $(SRC_LIB_H) + $(COMPILE.cpp) $< -o $@ + +StepperISR_test.o: StepperISR_test.cpp $(SRC_LIB_H) + +VERSION=$(shell git rev-parse --short HEAD) + +clean: + rm -f *.o test_[0-9][0-9] *.gnuplot pmf_test rmc_test test.log diff --git a/extras/tests/pc_based/PMF_test.ino b/extras/tests/pc_based/PMF_test.ino new file mode 100644 index 0000000..105d6ba --- /dev/null +++ b/extras/tests/pc_based/PMF_test.ino @@ -0,0 +1,59 @@ +#include + +#include "PoorManFloat.h" + +// +// This file can be renamed to a .ino and compiled as sketch to be run on the +// target e.g. arduino nano. +// + +#include +#ifdef SIMULATOR +#include +#endif + +uint16_t error_cnt = 0; +char buffer[256]; +#define trace(s) Serial.println(s) +#define xprintf(args...) +#define test(x, msg) \ + if (!(x)) { \ + error_cnt++; \ + Serial.print("ERROR: "); \ + Serial.println(__LINE__); \ + }; + +#include "test_03.h" + +void setup() { + Serial.begin(115200); + Serial.println("Start test..."); + bool result = perform_test(); + if (result) { + Serial.println("TEST PASSED"); + } else { + Serial.print("TEST FAILED: "); + Serial.print(error_cnt); + Serial.println(" errors"); + } +#ifdef SIMULATOR + // if result is Ok. Toggle port twice, otherwise once +#define PIN 10 +#define DIRPIN 7 + pinMode(DIRPIN, OUTPUT); + digitalWrite(DIRPIN, HIGH); + pinMode(PIN, OUTPUT); + digitalWrite(PIN, HIGH); + digitalWrite(PIN, LOW); + if (result) { + digitalWrite(PIN, HIGH); + digitalWrite(PIN, LOW); + } + + delay(1000); + noInterrupts(); + sleep_cpu(); +#endif +} + +void loop() {} diff --git a/extras/tests/pc_based/README.txt b/extras/tests/pc_based/README.txt new file mode 100644 index 0000000..fd97ced --- /dev/null +++ b/extras/tests/pc_based/README.txt @@ -0,0 +1,46 @@ +Tests; + +- test_01 + check queue functionality + +- test_02 + checks ramp timing + +- pmf_test + checks PoorManFloat implementation + +- test_04 + one test case with speed change during ramp + +- test_05 + check for move/moveTo while ramp is processing + Introduce concept of interrupt generation during noInterrupts call + +- test_06 + check for stop during move + +- test_07 + test case with varying speed + +- test_08 + ramp to max speed with step wise increased length + +- test_09 + simple test case for V30 A1000000 R53 W R53 + +- test_10 + test case for V30 a17164 w2000 a-1000 + +- test 11 + test case for M1 A1000 V10000 f w300 V100000 U + This is stuck in state RED. + Revised test: M1 A1000 V10000 P100 w300 V100000 U + +- test 12 + ramp up with 1 step/s^2 to 1000us/step + +- test 13 + tests with maximum high acceleration + +- test 14 + test case for issue #178: Speed jump instead of decrease diff --git a/extras/tests/pc_based/RampChecker.h b/extras/tests/pc_based/RampChecker.h new file mode 100644 index 0000000..ccfec37 --- /dev/null +++ b/extras/tests/pc_based/RampChecker.h @@ -0,0 +1,142 @@ +#include +#include + +class RampChecker { + public: + uint64_t total_ticks; + uint32_t last_dt; + uint32_t min_dt; + bool increase_ok; + bool flat_ok; + bool decrease_ok; + bool first; + bool dir_high; + bool reversing_allowed; + uint32_t accelerate_till; + uint32_t coast_till; + uint32_t time_coasting; + uint32_t pos; + uint32_t ticks_since_last_step; + float avg_accel; + FILE *gp_file; + char filename[100]; + + void next_ramp() { + increase_ok = true; + decrease_ok = false; + last_dt = ~0; + min_dt = ~0; + first = true; + dir_high = true; + coast_till = 0; + time_coasting = 0; + accelerate_till = 0; + reversing_allowed = false; + } + RampChecker() { + ticks_since_last_step = 0; + avg_accel = 0; + gp_file = NULL; + total_ticks = 0; + pos = 0; + next_ramp(); + } + void start_plot(char *fname) { + int n = strlen(fname) - 8; // remove .gnuplot + strncpy(filename, fname, n); + filename[n] = 0; + gp_file = fopen(fname, "w"); + fprintf(gp_file, "$data <steps; + if (steps == 0) { + // Just a pause + if (ticks_since_last_step <= 0xffff0000) { + ticks_since_last_step += e->ticks; + } + total_ticks += e->ticks; + printf("process pause %d => %u\n", e->ticks, ticks_since_last_step); + return; + } + if (e->toggle_dir) { + assert(reversing_allowed); + dir_high = !dir_high; + increase_ok = true; + last_dt = ~0; + decrease_ok = false; + } + if (dir_high) { + pos += steps; + } else { + pos -= steps; + } + uint32_t curr_dt = ticks_since_last_step; + total_ticks += steps * e->ticks; + if (!first) { + min_dt = min(min_dt, curr_dt); + } + float accel = 0; + if (last_dt != ~0) { + accel = (16000000.0 / float(curr_dt) - 16000000.0 / float(last_dt)) / + (1.0 / 16000000.0 * float(steps * curr_dt)); + avg_accel += (accel - avg_accel) / (steps * 20); + } + printf( + "process command in ramp checker @%.6fs: steps = %d last = %u current " + "= %u " + " min_dt " + "= %u accel=%.6f inc=%s dec=%s\n", + total_ticks / 16000000.0, steps, last_dt, curr_dt, min_dt, accel, + increase_ok ? "ALLOW" : "NO", decrease_ok ? "ALLOW" : "NO"); + + if (gp_file != NULL) { + fprintf(gp_file, "%.6f %.2f %d %d %f\n", total_ticks / 16000000.0, + 16000000.0 / last_dt, last_dt, pos, avg_accel); + } + + assert(first || (steps * curr_dt > 0)); + + if (last_dt > curr_dt) { + assert(increase_ok); + accelerate_till = total_ticks; + decrease_ok = true; + } else if (last_dt < curr_dt) { + if (increase_ok) { + coast_till = total_ticks - curr_dt; + } + assert(decrease_ok); + increase_ok = false; + } else { + time_coasting += steps * curr_dt; + } + + if (!first) { + last_dt = curr_dt; + } + + ticks_since_last_step = e->ticks; + first = false; + } +}; 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 0000000..f2815a0 --- /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/extras/tests/pc_based/pmf_test.cpp b/extras/tests/pc_based/pmf_test.cpp new file mode 100644 index 0000000..5a02b99 --- /dev/null +++ b/extras/tests/pc_based/pmf_test.cpp @@ -0,0 +1,29 @@ +#include + +#include "PoorManFloat.h" + +// +// This file can be renamed to a .ino and compiled as sketch to be run on the +// target e.g. arduino nano. +// + +#include +#include +#include +#define trace puts +#define xprintf printf +#define test(x, msg) \ + if (!(x)) { \ + puts(msg); \ + assert(false); \ + }; + +unsigned int error_cnt = 0; + +#include "test_03.h" + +int main() { + if (perform_test()) { + xprintf("TEST_03 PASSED\n"); + } +} diff --git a/extras/tests/pc_based/rmc_test.cpp b/extras/tests/pc_based/rmc_test.cpp new file mode 100644 index 0000000..194060a --- /dev/null +++ b/extras/tests/pc_based/rmc_test.cpp @@ -0,0 +1,94 @@ +#include +#include +#include +#include + +#include "RampCalculator.h" + +// Not a real test case + +int main() { + uint32_t res; + + // Calculation is pre_calc/sqrt(steps) + // + float ramp_acceleration = 10000.0; + uint32_t max_speed_in_ticks = 1600; + + struct ramp_config_s c; + c.init(); + c.parameters.setAcceleration(ramp_acceleration); + c.parameters.setSpeedInTicks(max_speed_in_ticks); + + char fname[100]; + snprintf(fname, 100, "ramp.gnuplot"); + FILE *gp_file = fopen(fname, "w"); + fprintf(gp_file, "$data <= s ? rs - s : s - rs; + uint32_t err_ticks = + ticks >= ticks_back ? ticks - ticks_back : ticks_back - ticks; + float speed = 16000000.0 / float(ticks); + float speed_back = 16000000.0 / float(ticks_back); + old_speed = speed; + float err_speed = + speed <= speed_back ? speed - speed_back : speed_back - speed; + printf("%d: %d %d %f %d delta=%d delta_ticks=%d speed=%f\n", s, + 16000000 / ticks, ticks, float(sum_ticks) / 16000000.0, rs, err, + err_ticks, err_speed); + fprintf(gp_file, "%d %f %d %d %d %d %d %f %f\n", s, + float(sum_ticks) / 16000000.0, 16000000 / ticks, ticks, rs, err, + err_ticks, err_speed, ideal_speed); + } + fprintf(gp_file, "EOF\n"); + // fprintf(gp_file, "plot $data using 2:3 with linespoints\n"); + // fprintf(gp_file, "set terminal pngcairo size 1024,768\n"); + // fprintf(gp_file, "set output \"ramp.png\"\n"); + fprintf(gp_file, "set terminal qt\n"); + fprintf(gp_file, "set term qt size 1024,768\n"); + fprintf(gp_file, + "set multiplot title \"Acceleration=%f max speed=%d steps/s\" layout " + "2,2 columnsfirst margins 0.1,0.9,0.1,0.9 spacing 0.1 columnsfirst\n", + ramp_acceleration, 16000000 / max_speed_in_ticks); + + fprintf(gp_file, "set xlabel \"ramp steps\"\n"); + fprintf(gp_file, "set ylabel \"speed in steps/s\"\n"); + fprintf( + gp_file, + "plot $data using 1:3 with line title \"step to speed dependency\"\n"); + + fprintf(gp_file, "set xlabel \"ramp steps\"\n"); + fprintf(gp_file, "set ylabel \"recovered ramp steps\"\n"); + fprintf(gp_file, + "plot $data using 1:5 with line title \"steps(speed(steps))\"\n"); + + fprintf(gp_file, "set xlabel \"time in s\"\n"); + fprintf(gp_file, "set ylabel \"speed in steps/s\"\n"); + fprintf(gp_file, "plot $data using 2:3 with line title \"speed over time\","); + fprintf(gp_file, " $data using 2:9 with line title \"ideal speed\"\n"); + + fprintf(gp_file, "set xlabel \"time in s\"\n"); + fprintf(gp_file, "set ylabel \"speed error in steps/s\"\n"); + fprintf(gp_file, "set yrange [-10:10]\n"); + fprintf( + gp_file, + "plot $data using 2:8 with line title \"speed error on ramp change\","); + fprintf( + gp_file, + " $data using 2:($3-$9) with line title \"speed error to ideal\"\n"); + + fprintf(gp_file, "unset multiplot\n"); + + fprintf(gp_file, "pause -1\n"); + fclose(gp_file); + // assert(false); + return 0; +} diff --git a/extras/tests/pc_based/stubs.h b/extras/tests/pc_based/stubs.h new file mode 100644 index 0000000..02eb68a --- /dev/null +++ b/extras/tests/pc_based/stubs.h @@ -0,0 +1,41 @@ +#ifndef STUBS_H +#define STUBS_H + +#define PROGMEM +#define pgm_read_byte_near(x) (*(x)) + +// For inducing interrupts while testing +void noInterrupts(); +void interrupts(); +void inject_fill_interrupt(int mark); + +#define _BV(x) 0 +#define ISR(x) void x() +#define inline +#define micros() 0 + +#include + +#define abs(x) ((x) > 0 ? (x) : -(x)) +#define min(a, b) ((a) > (b) ? (b) : (a)) +#define max(a, b) ((a) > (b) ? (a) : (b)) + +#define digitalWrite(a, b) \ + {} +#define pinMode(a, b) \ + {} + +extern char TCCR1A; +extern char TCCR1B; +extern char TCCR1C; +extern char TIMSK1; +extern char TIFR1; +extern unsigned short OCR1A; +extern unsigned short OCR1B; + +#define test(x, msg) \ + if (!(x)) { \ + puts(msg); \ + assert(false); \ + }; +#endif diff --git a/extras/tests/pc_based/test_01.cpp b/extras/tests/pc_based/test_01.cpp new file mode 100644 index 0000000..07034e9 --- /dev/null +++ b/extras/tests/pc_based/test_01.cpp @@ -0,0 +1,118 @@ +#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() {} + +void init_queue() { + fas_queue[0].read_idx = 0; + fas_queue[0].next_write_idx = 0; + fas_queue[1].read_idx = 0; + fas_queue[1].next_write_idx = 0; +} + +void basic_test() { + puts("basic_test..."); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + assert(0 == s.getCurrentPosition()); + assert(s.isQueueEmpty()); + assert(s.isQueueEmpty()); + struct stepper_command_s cmd = { + .ticks = 10000, .steps = 100, .count_up = true}; + int res = s.addQueueEntry(&cmd); + assert(res == AQE_OK); + assert(!s.isQueueEmpty()); +} + +void queue_full() { + puts("queue_full..."); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + assert(0 == s.getCurrentPosition()); + assert(s.isQueueEmpty()); + assert(s.isQueueEmpty()); + printf("Queue read/write = %d/%d\n", fas_queue[0].read_idx, + fas_queue[0].next_write_idx); + struct stepper_command_s cmd = { + .ticks = 10000, .steps = 100, .count_up = true}; + for (int i = 0; i < QUEUE_LEN - 1; i++) { + s.addQueueEntry(&cmd); + assert(!s.isQueueEmpty()); + assert(!s.isQueueFull()); + printf("Queue read/write = %d/%d\n", fas_queue[0].read_idx, + fas_queue[0].next_write_idx); + } + s.addQueueEntry(&cmd); + printf("Queue read/write = %d/%d\n", fas_queue[0].read_idx, + fas_queue[0].next_write_idx); + assert(!s.isQueueEmpty()); + assert(s.isQueueFull()); + puts("...done"); +} + +void queue_out_of_range() { + int8_t res; + + puts("queue_out_of_range..."); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + assert(s.isQueueEmpty()); + assert(0 == s.getCurrentPosition()); + assert(s.isQueueEmpty()); + assert(s.isQueueEmpty()); + + uint16_t ticks = s.getMaxSpeedInTicks(); + ticks = ticks - 1; + struct stepper_command_s cmd2 = { + .ticks = ticks, .steps = 255, .count_up = true}; + + res = s.addQueueEntry(&cmd2); + test(res == AQE_ERROR_TICKS_TOO_LOW, "Too low ticks should trigger an error"); + assert(s.isQueueEmpty()); + + struct stepper_command_s cmd3 = { + .ticks = MIN_CMD_TICKS - 1, .steps = 1, .count_up = true}; + + res = s.addQueueEntry(&cmd3); + test(res == AQE_ERROR_TICKS_TOO_LOW, + "Too short command time should trigger an error"); + assert(s.isQueueEmpty()); +} + +void end_pos_test() { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + assert(0 == s.getPositionAfterCommandsCompleted()); + struct stepper_command_s cmd = {.ticks = 65535, .steps = 1, .count_up = true}; + + assert(AQE_OK == s.addQueueEntry(&cmd)); + assert(1 == s.getPositionAfterCommandsCompleted()); +} + +int main() { + // assert(sizeof(struct queue_entry) == 6); + basic_test(); + queue_out_of_range(); + queue_full(); + end_pos_test(); + printf("TEST_01 PASSED\n"); +} diff --git a/extras/tests/pc_based/test_02.cpp b/extras/tests/pc_based/test_02.cpp new file mode 100644 index 0000000..5517fff --- /dev/null +++ b/extras/tests/pc_based/test_02.cpp @@ -0,0 +1,307 @@ +#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 with_empty_queue() { + printf("Test with empty queue\n"); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + assert(s.isQueueEmpty()); + s.setSpeedInUs(10000); + s.setAcceleration(100); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(1000); + s.fill_queue(); + assert(!s.isQueueEmpty()); + for (int i = 0; i < 1000; i++) { + if (false) { + 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(); + while (!s.isQueueEmpty()) { + rc.check_section( + &fas_queue_A.entry[fas_queue[0].read_idx & QUEUE_LEN_MASK]); + fas_queue[0].read_idx++; + } + } + test(!s.isRampGeneratorActive(), "too many commands created"); + printf("min_dt=%u\n", rc.min_dt); + test(rc.min_dt == 160000, "max speed not reached"); + } + + void with_pars(const char *name, int32_t steps, uint32_t travel_dt, + uint32_t accel, bool reach_max_speed, float min_time, + float max_time, float allowed_ramp_time_delta, + bool call_moveTo_repeatedly = false, + bool call_setAccelertion_repeatedly = false, + bool alternatingAccelerationValue = false, + bool reversing_allowed = false, + uint32_t linear_acceleration_steps = 0, + uint32_t jump_step = 0) { + printf("Test %s test_with_pars steps=%d travel_dt=%d accel=%d dir=%s\n", + name, steps, travel_dt, accel, reach_max_speed ? "CW" : "CCW"); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + s.setDirectionPin(0); + RampChecker rc = RampChecker(); + rc.reversing_allowed = reversing_allowed; + assert(0 == s.getCurrentPosition()); + + assert(s.isQueueEmpty()); + assert(0 == s.setSpeedInUs(travel_dt)); + s.setAcceleration(accel); + s.setLinearAcceleration(linear_acceleration_steps); + s.setJumpStart(jump_step); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(steps); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + char fname[100]; + snprintf(fname, 100, "test_02_%s.gnuplot", name); + rc.start_plot(fname); + for (int i = 0; i < steps * 100; i++) { + if (call_moveTo_repeatedly) { + s.moveTo(steps); + } + if (call_setAccelertion_repeatedly) { + if (alternatingAccelerationValue) { + s.setAcceleration(accel + (i & 1) * 100); + } else { + s.setAcceleration(accel); + } + } + 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(); + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + rc.check_section( + &fas_queue_A.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 (old=%.6fs)\n", planned_time, + old_planned_time_in_buffer); + // 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; + } + // Empty the queue + while (!s.isQueueEmpty()) { + rc.check_section( + &fas_queue_A.entry[fas_queue[0].read_idx & QUEUE_LEN_MASK]); + fas_queue[0].read_idx++; + } + rc.finish_plot(); + printf("TEST=%s\n", name); + test(!s.isRampGeneratorActive(), "too many commands created"); + printf("Total time %f < %f < %f ?\n", min_time, + rc.total_ticks / 16000000.0, max_time); + test(rc.total_ticks / 16000000.0 > min_time, "ramp too fast"); + test(rc.total_ticks / 16000000.0 < max_time, "ramp too slow"); + if (reach_max_speed) { + printf("%d = %d ?\n", rc.min_dt, travel_dt * 16); + test(rc.min_dt == travel_dt * 16, "max speed not reached"); + } else { + printf("%d > %d ?\n", rc.min_dt, travel_dt * 16); + test(rc.min_dt > travel_dt * 16, "max speed reached"); + } + float up_time, down_time; + if (reach_max_speed) { + printf("Ramp time up/coast/down/total="); + up_time = 1.0 * rc.accelerate_till / 16000000.0; + down_time = (1.0 * rc.total_ticks - 1.0 * rc.coast_till) / 16000000.0; + printf(" %f", 1.0 * rc.accelerate_till / 16000000.0); + printf(" %f", 1.0 * (rc.coast_till - rc.accelerate_till) / 16000000.0); + printf(" %f", 1.0 * (rc.total_ticks - rc.coast_till) / 16000000.0); + printf(" %f\n", 1.0 * rc.total_ticks / 16000000.0); + assert(rc.total_ticks > rc.coast_till); + } else { + printf("Ramp time up/down/total ="); + up_time = 1.0 * rc.accelerate_till / 16000000.0; + down_time = + (1.0 * rc.total_ticks - 1.0 * rc.accelerate_till) / 16000000.0; + printf(" %f", 1.0 * rc.accelerate_till / 16000000.0); + printf(" %f", 1.0 * (rc.total_ticks - rc.accelerate_till) / 16000000.0); + printf(" %f\n", 1.0 * rc.total_ticks / 16000000.0); + } + // turned off + // test(abs(up_time - down_time) < + // 0.5 * (up_time + down_time) * allowed_ramp_time_delta, + // "assymmetric ramp"); +#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; + + float nc = 0.0; // if new ramp calculation is enabled + + test.with_empty_queue(); + // steps ticks_us accel maxspeed min/max_total_time + + // jumps in speed in real on esp32 + test.with_pars("f1", 1000, 4300, 10000, true, 4.5 - 0.2, 4.5 + 0.2, 0.5, true, + true); + + // ramp 2*2s, 2*200 steps, coasting: 9600steps, 48s + test.with_pars("f2", 10000, 5000, 100, true, 2 * 2.0 + 48.0 - 0.2 - 0.4 * nc, + 2 * 2.0 + 48.0 + 0.2, 0.2); + // ramp 2*0.02s, 2*2 steps, coasting: 1596 steps, 7.98s + test.with_pars("f3", 1600, 5000, 10000, true, 7.94, 8.02, 0.2); + // ramp 2*0.2s, 2*20 steps, coasting: 1560 steps, 7.8s + test.with_pars("f4", 1600, 5000, 1000, true, 2 * 0.2 + 7.8 - 0.1, + 2 * 0.2 + 7.8 + 0.1, 0.2); + // ramp 2*1s, 5000 steps, coasting: 5000steps, 0.5s + test.with_pars("f5", 15000, 100, 10000, true, 2 * 1.0 + 0.5 - 0.1, + 2 * 1.0 + 0.5 + 0.1, 0.2); + // ramp 2*0.02s, 2*2 steps, coasting: 96steps, 0.48 + test.with_pars("f6", 100, 5000, 10000, true, 2 * 0.02 + 0.48 - 0.02, + 2 * 0.02 + 0.48 + 0.02, 0.2); + // ramp 2s, 20000 steps => only ramp 2*0.4s + test.with_pars("f7", 1600, 50, 10000, false, 2 * 0.4 - 0.02 - 0.4 * nc, + 2 * 0.4 + 0.02, 0.2); + // ramp 2*4s, 2*8000 steps, coasting 112000steps, 28s + test.with_pars("f8", 128000, 250, 1000, true, 2 * 4.0 + 28.0 - 0.1 - 0.1 * nc, + 2 * 4.0 + 28.0 + 0.1, 0.2); + // ramp 2*4s, 2*8000 steps, coasting 56000steps, 14s + test.with_pars("f9", 72000, 250, 1000, true, 2 * 4.0 + 14.0 - 0.1 - 0.1 * nc, + 2 * 4.0 + 14.0 + 0.1, 0.2); + // ramp 2*4s, 2*8000 steps, coasting 28000steps, 7s + test.with_pars("f10", 44000, 250, 1000, true, 2 * 4.0 + 7.0 - 0.1 - 0.1 * nc, + 2 * 4.0 + 7.0 + 0.1, 0.2); + // ramp 2*4s, 2*8000 steps, coasting 2steps, 0.0005s + // fails with 16030 + test.with_pars("f11", 16040, 250, 1000, true, 2 * 4.0 + 0.0 - 0.1 - 0.1 * nc, + 2 * 4.0 + 0.1 + 0.1, 0.2); + // ramp 2*50s => 2*1s + test.with_pars("f12", 1000, 20, 1000, false, 2 * 1.0 - 0.15, 2 * 1.0 + 0.1, + 0.2); + + // The following five ramps are too fast. + // The first step should come after ~0.6s and + // the second after 0.89s. Implementation issues first step immediately + // with pause to 2nd step of 0.36s (actually 0.315s). + // So the first steps are issued within 0.36s instead of 0.89s. + // + // The implementation issues in addition the last two steps with 0.315s pause + float rd = 0.7; // rd means ramp deviation + // + // ramp 2*50s, thus with 500steps max speed not reached. 250steps need 10s + test.with_pars("f13", 500, 4000, 5, false, 20.0 - rd - 0.1 - 1.4 * nc, + 20.0 - rd + 0.2, 0.2); + test.with_pars("f14", 2000, 4000, 5, false, 40.0 - rd - 0.1 - 1.7 * nc, + 40.0 - rd + 0.2, 0.2); + // ramp 2*50s with 2*6250 steps => 100 steps at max speed using 0.4s + test.with_pars("f15", 12600, 4000, 5, true, 100.0 + 0.4 - 0.3 - rd - 2.3 * nc, + 100.0 + 0.4 - rd + 0.24, 0.2); + // ramp 2*50s with 2*6250 steps => 4000 steps at max speed using 16s + test.with_pars("f16", 16500, 4000, 5, true, 116.0 - 0.3 - rd - 2.2 * nc, + 116.0 + 0.23 - rd, 0.2); + // slow ramp: 2*50steps, 2*10s + rd = 1.4; + test.with_pars("f17", 100, 40, 1, false, 20.0 - 0.1 - rd - 2.0 * nc, + 20.0 + 0.1 - rd, 1.0); + + // jumps in speed in real => WORKS NOW + test.with_pars("f18", 256000, 40, 5000, true, 15.2 - 0.1, 15.2 + 0.2, 0.2); + + // ramp time 625s, 7812500 steps + // test.with_pars("f19", 2000000, 40, 40, false, 2*223.0, 2*223.0); + + // name, steps, travel_dt, accel, reach_max_speed, min_time, max_time, + // allowed_ramp_time_delta slow ramp time Those are anomalies (see github + // issue #8) on avr, but not on PC + // test.with_pars("f20", 50000, 270000, 10, true, 62.0, 63.0, 1.0); + test.with_pars("f20", 10, 1000000, 1, true, 9.9, 10.1, 1.0); + + // no ramp time, just constant run time + test.with_pars("f21", 15000, 4000, 100000, true, 50.9, 60.1, 0.1); + test.with_pars("f22", 14634, 4100, 100000, true, 50.9, 60.1, 0.1); + + // single step + test.with_pars("f23", 1, 100, 1000, false, 0.02, 0.05, 0.1); + + // try to identify issue #40 + test.with_pars("f24a", 5000, 200, 9999, true, 1.48 + 0.1 * nc, 1.5 + 0.1 * nc, + 0.1, true, true, false, true); + test.with_pars("f24b", 5000, 200, 9999, true, 1.48 - 0.04 * nc, + 1.50 - 0.04 * nc, 0.1, false, false, false); + test.with_pars("f24c", 5000, 200, 9999, true, 1.48 - 0.04 * nc, + 1.50 - 0.04 * nc, 0.1, false, false, true); + test.with_pars("f24d", 5000, 200, 9999, true, 1.48 - 0.04 * nc, + 1.50 - 0.04 * nc, 0.1, true, false, true); + + test.with_pars("f25", 1000, 40, 0x7fffffff, true, 0.039, 0.041, 0.1); + + // very short ramp. detected by esp32_hw_based tests seq_06.sh + test.with_pars("seq_06.sh", 54, 40, 1000000, false, 0.012, 0.018, 0.1); + + // ramp with jump start + test.with_pars("f5_jumpstart", 15000, 100, 10000, true, 2 * 1.0 + 0.0 - 0.1, + 2 * 1.0 + 0.5 + 0.1, 0.2, false, false, false, false, 0, 100); + + // ramp with linea acceleration + test.with_pars("f5_linear_a", 15000, 100, 10000, true, 3, 4, 0.2, false, + false, false, false, 1000); + + printf("TEST_02 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_03.h b/extras/tests/pc_based/test_03.h new file mode 100644 index 0000000..7c8a383 --- /dev/null +++ b/extras/tests/pc_based/test_03.h @@ -0,0 +1,438 @@ +#include + +struct const_tab { + uint32_t val_nom; + uint32_t val_denom; + bool squared; + pmf_logarithmic c; +}; + +bool perform_test() { +#define NR_OF_CONSTANTS 13 + static const struct const_tab constants[NR_OF_CONSTANTS] = { + {1, 1, false, PMF_CONST_1}, + {16000000, 1, false, PMF_CONST_16E6}, + {3, 2, false, PMF_CONST_3_DIV_2}, + {500, 1, false, PMF_CONST_500}, + {1000, 1, false, PMF_CONST_1000}, + {2000, 1, false, PMF_CONST_2000}, + {32000, 1, false, PMF_CONST_32000}, + {11313708, 1, false, PMF_CONST_16E6_DIV_SQRT_OF_2}, + {21000000, 1, false, PMF_CONST_21E6}, + {42000, 1, false, PMF_CONST_42000}, + // The additional 4000 to make the test case pass + {14849242 + 4000, 1, false, PMF_CONST_21E6_DIV_SQRT_OF_2}, + {16000000, 2, true, PMF_CONST_128E12}, // (16e6)^2 / 2 + {21000000, 2, true, PMF_CONST_2205E11} // (21e6)^2 / 2 + }; + uint16_t l1; + pmf_logarithmic p1; + + trace("Check leading_zeros()"); + for (int16_t x_8 = 0; x_8 <= 255; x_8++) { + uint8_t leading = leading_zeros(x_8); + test(x_8 < (1 << (8 - leading)), "leading zeros too much"); + test(x_8 >= (0x80 >> leading), "leading zeros too less"); + } + + trace("Check conversion u8 <=> pmfl"); + p1 = pmfl_from((uint8_t)1); + l1 = pmfl_to_u16(p1); + xprintf("%x %d\n", p1, l1); + test(p1 == 0x0000, "value 1"); + test(l1 == 1, "value 1"); + + trace("Check conversion u8 <=> pmfl by shift 8bit"); + for (uint8_t n = 1; n < 8; n++) { + uint8_t v = 1 << n; + p1 = pmfl_from((uint8_t)v); + l1 = pmfl_to_u16(p1); + xprintf("8bit: %x %d\n", p1, l1); + test(p1 == ((int16_t)n) << 9, "value"); + test(l1 == v, "value"); + } + + trace("Check conversion u8 <=> pmfl by shift 16bit"); + for (uint8_t n = 1; n < 16; n++) { + uint16_t v = 1 << n; + p1 = pmfl_from((uint16_t)v); + l1 = pmfl_to_u16(p1); + xprintf("16bit: %x %d\n", p1, l1); + test(p1 == ((int16_t)n) << 9, "value"); + test(l1 == v, "value"); + } + + trace("Check conversion u8 <=> pmfl by shift 32bit"); + for (uint8_t n = 1; n < 32; n++) { + uint32_t v = 1; + v <<= n; + p1 = pmfl_from((uint32_t)v); + uint32_t res = pmfl_to_u32(p1); + xprintf("32bit: %x %u\n", p1, res); + test(p1 == (((int16_t)n) << 9), "value"); + test(res == v, "value"); + } + + trace("Check conversion u8 <=> pmfl for all values"); + for (uint8_t x_8 = 255; x_8 > 0; x_8--) { + p1 = pmfl_from((uint8_t)x_8); + uint16_t res_16 = pmfl_to_u16(p1); + if (res_16 != x_8) { + xprintf("%u => %x => %u\n", x_8, p1, res_16); + } + test(res_16 == x_8, "conversion error from uint8_t and back to uint16_t"); + } + + for (uint8_t n = 1; n <= 8; n++) { + for (uint8_t x_8 = 255; x_8 > 0; x_8--) { + uint16_t x_16 = x_8; + x_16 <<= n; + p1 = pmfl_from((uint8_t)x_8); + p1 = pmfl_shl(p1, n); + uint16_t res_16 = pmfl_to_u16(p1); + uint16_t delta = x_16 - res_16; + if (res_16 > x_16) { + delta = res_16 - x_16; + } + uint16_t limit = 1; + limit <<= n - 1; + if (delta > limit) { + xprintf("%u: %u => %x => %u, shifted: %d\n", x_8, x_16, p1, res_16, n); + } + test(delta <= limit, + "conversion error from uint8_t and back to uint16_t with shift"); + } + } + + for (uint8_t n = 1; n <= 24; n++) { + for (uint8_t x_8 = 255; x_8 > 0; x_8--) { + uint32_t x_32 = x_8; + x_32 <<= n; + p1 = pmfl_from((uint8_t)x_8); + p1 = pmfl_shl(p1, n); + uint32_t res_32 = pmfl_to_u32(p1); + uint32_t delta = x_32 - res_32; + if (res_32 > x_32) { + delta = res_32 - x_32; + } + uint32_t limit = 1; + limit <<= n - 1; + if (delta > limit) { + xprintf("%u: %u => %x => %u, shifted: %d\n", x_8, x_32, p1, res_32, n); + } + test(delta <= limit, + "conversion error from uint8_t and back to uint32_t with shift"); + } + } + trace("Check conversion u16 <=> pmfl"); + uint16_t limit = 0x100; + uint16_t trigger_16 = 0x8000; + for (uint16_t x_16 = 0xffff; x_16 > 0; x_16--) { + if ((x_16 & trigger_16) == 0) { + limit >>= 1; + trigger_16 >>= 1; + } + pmf_logarithmic p = pmfl_from((uint16_t)x_16); + uint16_t res_16 = pmfl_to_u16(p); + uint16_t delta = x_16 - res_16; + if (res_16 > x_16) { + delta = res_16 - x_16; + } + if (delta > limit) { + xprintf("%x => %x => %x (limit=%x)\n", x_16, p, res_16, limit); + } + test(delta <= limit, "conversion error from uint16_t and back to uint16_t"); + } + + for (uint8_t n = 1; n <= 16; n++) { + uint32_t msb = 32768; + for (uint16_t x_16 = 65535; x_16 > 256; x_16--) { + if ((x_16 & msb) == 0) { + msb >>= 1; + } + uint32_t x_32 = x_16; + x_32 <<= n; + p1 = pmfl_from((uint16_t)x_16); + p1 = pmfl_shl(p1, n); + uint32_t res_32 = pmfl_to_u32(p1); + uint32_t delta = x_32 - res_32; + uint32_t limit = (msb << n) >> 8; + limit += limit >> 2; + if (res_32 > x_32) { + delta = res_32 - x_32; + } + if (delta > limit) { + xprintf("%u: %u => %x => %u, shifted: %d, delta: %d > %d\n", x_16, x_32, + p1, res_32, n, delta, limit); + } + test(delta <= limit, + "conversion error from uint16_t and back to uint32_t with shift"); + } + } + + p1 = pmfl_from((uint32_t)0x10000); + test(pmfl_to_u16(p1) == 0xffff, "wrong overflow 16bit"); + p1 = pmfl_from((uint32_t)0x80000000); + p1 = pmfl_shl(p1, 1); + test(pmfl_to_u32(p1) == 0xffffffff, "wrong overflow 32bit"); + +#ifndef SIMULATOR + trace("Check conversion u32 <=> pmfl"); + uint32_t trigger_32 = 0x80000000; + uint32_t delta_32 = 0x01000000; + for (uint32_t x_32 = 0xffffffff; x_32 > 0; x_32 -= delta_32) { + if ((x_32 & trigger_32) == 0) { + trigger_32 >>= 1; + delta_32 >>= 1; + if (delta_32 == 0) { + delta_32 = 1; + } + } + pmf_logarithmic px = pmfl_from((uint32_t)x_32); + uint32_t res_32 = pmfl_to_u32(px); + uint32_t delta = x_32 - res_32; + if (res_32 > x_32) { + delta = res_32 - x_32; + } + if (delta > delta_32 + 1) { + xprintf("%x => %x => %x (delta=%x > %x)\n", x_32, px, res_32, delta, + delta_32); + } + test(delta <= delta_32 + 1, + "conversion error from uint32_t and back to uint32_t"); + } + + trace("Check multiply"); + for (int16_t sa = -40; sa <= 40; sa++) { + for (uint32_t a_32 = 1; a_32 <= 0x1ff; a_32++) { + for (uint32_t b_32 = 1; b_32 <= 0x1ff; b_32++) { + p1 = pmfl_from(a_32); + pmf_logarithmic p2 = pmfl_from(b_32); + if (sa > 0) { + p1 = pmfl_shl(p1, sa); + } else if (sa < 0) { + p1 = pmfl_shr(p1, -sa); + } + pmf_logarithmic p = pmfl_multiply(p1, p2); + if (sa > 0) { + p = pmfl_shr(p, sa); + } else if (sa < 0) { + p = pmfl_shl(p, -sa); + } + uint32_t res = pmfl_to_u32(p); + uint32_t real_res = a_32 * b_32; + uint32_t repr_real = pmfl_to_u32(pmfl_from(real_res)); + uint32_t delta = res - repr_real; + if (res < repr_real) { + delta = repr_real - res; + } + uint32_t limit = real_res >> 7; + if (delta > limit) { + xprintf("%d*%d=%d ~ %d =?= %d, diff=%d\n", a_32, b_32, a_32 * b_32, + repr_real, res, (int32_t)res - (int32_t)repr_real); + } + test(delta <= limit, "pmfl_multiply error"); + } + } + } +#endif + + trace("Check pmf constants"); + bool error = false; + for (uint8_t i = 0; i < NR_OF_CONSTANTS; i++) { + const struct const_tab *dut = &constants[i]; + pmf_logarithmic val = pmfl_from(dut->val_nom); + if (dut->squared) { + val += val; + } + if (dut->val_denom > 1) { + pmf_logarithmic val_denom = pmfl_from(dut->val_denom); + val -= val_denom; + } + pmf_logarithmic c = dut->c; + if (c != val) { + xprintf("(%d/%d)^%d => %x != %x\n", dut->val_nom, dut->val_denom, + dut->squared ? 2 : 1, val, c); + error = true; + } + } + test(!error, "constants"); + + trace("Check rsqrt"); + for (int16_t sa = -20; sa <= 20; sa++) { + for (uint32_t a_32 = 1; a_32 <= 0x1ff; a_32++) { + p1 = pmfl_from(a_32); + if (sa > 0) { + p1 = pmfl_shl(p1, sa); + } else if (sa < 0) { + p1 = pmfl_shr(p1, -sa); + } + pmf_logarithmic p = pmfl_rsqrt(p1); + pmf_logarithmic pe = + pmfl_multiply(p1, pmfl_multiply(p, p)); // sqrt not yet tested + // pe should be approximately 1 + uint32_t res = pmfl_to_u32(pmfl_shl(pe, 16)); + int32_t diff = (int32_t)res - 0x10000; + if (abs(diff) > 384) { + xprintf("a=%d pmfl(x)=%x pmfl(rsqrt(x))=%x pmfl(rsqrt(x)^2*x)=%x ", + a_32, p1, p, pe); + xprintf("shift=%d rsqrt(%d)^2*%d*0x10000=%x, diff=%d\n", sa, a_32, a_32, + res, diff); + } + test(abs(diff) <= 384, "rsqrt error"); + } + } + + trace("Check square"); + for (int16_t sa = -20; sa <= 20; sa++) { + for (uint32_t a_32 = 1; a_32 <= 0x1ff; a_32++) { + p1 = pmfl_from(a_32); + if (sa > 0) { + p1 = pmfl_shl(p1, sa); + } else if (sa < 0) { + p1 = pmfl_shr(p1, -sa); + } + pmf_logarithmic p = pmfl_square(p1); + pmf_logarithmic pe = pmfl_multiply(p1, p1); + int32_t diff = (int32_t)p - (int32_t)pe; + if (diff > 1) { // square has better precision than multiply + xprintf("a=%d pmfl(x)=%x pmfl(square(x))=%x pmfl(x*x)=%x ", a_32, p1, + p, pe); + xprintf("shift=%d, diff=%d\n", sa, 0); + } + test(diff <= 1, "square error"); + } + } + + trace("Check reciprocal square"); + for (int16_t sa = -20; sa <= 20; sa++) { + for (uint32_t a_32 = 1; a_32 <= 0x1ff; a_32++) { + p1 = pmfl_from(a_32); + if (sa > 0) { + p1 = pmfl_shl(p1, sa); + } else if (sa < 0) { + p1 = pmfl_shr(p1, -sa); + } + pmf_logarithmic p = pmfl_rsquare(p1); + + pmf_logarithmic pe = pmfl_multiply(p, pmfl_square(p1)); + // pe should be approximately 1 + uint32_t res = pmfl_to_u32(pmfl_shl(pe, 16)); + int32_t diff = (int32_t)res - 0x10000; + if (abs(diff) > 384) { + xprintf("a=%d pmfl(x)=%x pmfl(rsquare(x))=%x pmfl(rsquare(x)*x^2)=%x ", + a_32, p1, p, pe); + xprintf("shift=%d rsquare(%d)*%d^2*0x10000=%x, diff=%d\n", sa, a_32, + a_32, res, diff); + } + test(abs(diff) <= 384, "rsquare error"); + } + } + + trace("Check reciprocal"); + for (int16_t sa = -20; sa <= 20; sa++) { + for (uint32_t a_32 = 1; a_32 <= 0x1ff; a_32++) { + p1 = pmfl_from(a_32); + if (sa > 0) { + p1 = pmfl_shl(p1, sa); + } else if (sa < 0) { + p1 = pmfl_shr(p1, -sa); + } + pmf_logarithmic p = pmfl_reciprocal(p1); + + pmf_logarithmic pe = pmfl_multiply(p, p1); + // xe should be approximately 1 + uint32_t res = pmfl_to_u32(pmfl_shl(pe, 16)); + int32_t diff = (int32_t)res - 0x10000; + if (abs(diff) > 384) { + xprintf( + "a=%d pmfl(x)=%x pmfl(reciprocal(x))=%x pmfl(reciprocal(x)*x)=%x ", + a_32, p1, p, pe); + xprintf("shift=%d reciprocal(%d)*%d*0x10000=%x, diff=%d\n", sa, a_32, + a_32, res, diff); + } + test(abs(diff) <= 384, "reciprocal error"); + } + } + + trace("Check specific use cases"); + pmf_logarithmic x, x1, x2; + x1 = pmfl_from((uint32_t)0x0ffff); + x2 = pmfl_from((uint32_t)0x10100); + x = pmfl_multiply(x1, x2); + unsigned long back = pmfl_to_u32(x); + test(back == 0xffffffff, "overflow not catched"); + + x1 = pmfl_from((uint32_t)0x5555); + x2 = pmfl_from((uint32_t)0x0055); + x = pmfl_divide(x1, x2); + back = pmfl_to_u32(x); + xprintf("%x/%x=%x (back=%ld)\n", x1, x2, x, back); + test(back == 0x0101, "wrong division"); + + x1 = pmfl_from((uint32_t)0xf455); + x2 = pmfl_from((uint32_t)0x0030); + x = pmfl_divide(x1, x2); + back = pmfl_to_u32(x); + back--; // result is too high by one + xprintf("%x/%x=%x (%ld) f455/0030=%d\n", x1, x2, x, back, 0xf455 / 0x30); + test((back * 0x0030) <= 0xf455, "wrong division 1"); + test((back * 0x0031) > 0xf455, "wrong division 2"); + + x1 = pmfl_from((uint32_t)0xf4555); + x = pmfl_shl(x1, 4); + back = pmfl_to_u32(x); + xprintf("%x => %x (%lx)\n", x1, x, back); + test(back == 0xf44000, "wrong pmfl_shl"); + x1 = pmfl_from((uint32_t)0xf4555); + x = pmfl_shr(x1, 4); + back = pmfl_to_u32(x); + xprintf("%x => %x (%lx)\n", x1, x, back); + test(back == 0xf440, "wrong pmfl_shr"); + + x1 = pmfl_from((uint32_t)250); + x2 = pmfl_from((uint32_t)10000); + x = pmfl_divide(x1, x2); + back = pmfl_to_u32(x); + xprintf("%x/%x=%x (%ld)\n", x1, x2, x, back); + test(back == 0, "pmfl_divide 1"); + x = pmfl_multiply(x, x2); + back = pmfl_to_u32(x); + xprintf("%x/%x*%x=%x (%ld)\n", x1, x2, x2, x, back); + back--; // value is one too high + test(back == 249, "pmfl_divide 2"); + + x1 = pmfl_from((uint32_t)250); + x2 = pmfl_from((uint32_t)10000); + x = pmfl_divide(x1, x2); + back = pmfl_to_u32(x); + xprintf("%x/%x=%x (%ld)\n", x1, x2, x, back); + test(back == 0, "pmfl_divide"); + x = pmfl_shl(x, 10); + back = pmfl_to_u32(x); + xprintf("pmfl_shl(%x/%x,10)=%x (%ld)\n", x1, x2, x, back); + test(back == 25, "pmfl_divide/pmfl_shl"); + + x1 = pmfl_from((uint32_t)1600); + x2 = pmfl_from((uint32_t)1000000); + x = pmfl_divide(x1, x2); + back = pmfl_to_u32(x); + xprintf("%x/%x=%x (%ld)\n", x1, x2, x, back); + test(back == 0, "pmfl_divide"); + x = pmfl_shl(x, 20); + back = pmfl_to_u32(x); + xprintf("pmfl_shl(%x/%x,20)=%x (%ld)\n", x1, x2, x, back); + test(back + 2 == 1678, "pmfl_divide/pmfl_shl"); + x = pmfl_shr(x, 20); + back = pmfl_to_u32(x); + xprintf("pmfl_shr(pmfl_shl(%x/%x,20),20)=%x (%ld)\n", x1, x2, x, back); + test(back == 0, "pmfl_divide/pmfl_shl"); + + x1 = pmf_logarithmic((uint32_t)1500); + x = pmfl_pow_div_3(x1); + xprintf("%d/3=%d\n", x1, x); + // +1 is deviation + test(x + 1 == 500, "pmfl_pow_div_3"); + + return (error_cnt == 0); +} diff --git a/extras/tests/pc_based/test_04.cpp b/extras/tests/pc_based/test_04.cpp new file mode 100644 index 0000000..140d50e --- /dev/null +++ b/extras/tests/pc_based/test_04.cpp @@ -0,0 +1,169 @@ +#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 speed_increase() { + puts("Test test_speed_increase"); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + int32_t steps = 10000; + + // Increase speed to 400, then further to 300 + // Identified bug was a fast jump to 300 without acceleration + assert(s.isQueueEmpty()); + s.setSpeedInUs(400); + s.setAcceleration(1000); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(steps); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + int speed_increased = false; + for (int i = 0; i < steps; i++) { + if (!speed_increased && (s.getCurrentPosition() >= 5000)) { + puts("Change speed"); + s.fill_queue(); // ensure queue is not empty + speed_increased = true; + s.setSpeedInUs(300); + s.move(steps); + } + 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(); + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + 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; + } + test(!s.isRampGeneratorActive(), "too many commands created"); + 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 + } + + void speed_decrease() { + puts("Test test_speed_decrease"); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + int32_t steps = 10000; + + // Increase speed to 400, then further to 300 + // Identified bug was a fast jump to 300 without acceleration + assert(s.isQueueEmpty()); + s.setSpeedInUs(400); + s.setAcceleration(1000); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(steps); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + int speed_decreased = false; + uint32_t count_state_dec = 0; + for (int i = 0; i < steps; i++) { + if (!speed_decreased && (s.getCurrentPosition() >= 5000)) { + puts("Change speed"); + s.fill_queue(); // ensure queue is not empty + speed_decreased = true; + s.setSpeedInUs(500); + s.move(steps); + } + 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 ((s.rampState() & RAMP_STATE_MASK) == RAMP_STATE_DECELERATE) { + count_state_dec++; + } + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + 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; + } + test(!s.isRampGeneratorActive(), "too many commands created"); + test(count_state_dec > 10, "no deceleration to new speed"); + 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.speed_increase(); + test.speed_decrease(); + printf("TEST_04 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_05.cpp b/extras/tests/pc_based/test_05.cpp new file mode 100644 index 0000000..9b7f713 --- /dev/null +++ b/extras/tests/pc_based/test_05.cpp @@ -0,0 +1,160 @@ +#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; + +FastAccelStepper *stepper; +StepperQueue fas_queue[NUM_QUEUES]; + +int enable_inject_on_mark = -1; +bool enable_stepper_manage_on_interrupts = false; +bool enable_stepper_manage_on_noInterrupts = false; +bool in_manage = false; + +#include "RampChecker.h" +class FastAccelStepperTest { + public: + void init_queue() { + fas_queue[0]._initVars(); + fas_queue[1]._initVars(); + } + void inject() { stepper->fill_queue(); } + void do_test() { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + stepper = &s; + + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + int32_t steps = 10000; + + // Increase speed to 400, then further to 300 + // Identified bug was a fast jump to 300 without acceleration + assert(s.isQueueEmpty()); + s.setSpeedInUs(400); + s.setAcceleration(10000); + in_manage = true; + s.fill_queue(); + in_manage = false; + assert(s.isQueueEmpty()); + assert(!s.isRunning()); + s.moveTo(3000); + assert(s.isRunning()); + in_manage = true; + s.fill_queue(); + in_manage = false; + assert(!s.isQueueEmpty()); + + float old_planned_time_in_buffer = 0; + int moveto_done = false; + for (int i = 0; i < steps; i++) { + if (!moveto_done && (s.getCurrentPosition() >= 500)) { + moveto_done = true; + s.moveTo(4000); + } + 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"); + } + in_manage = true; + if (!s.isRampGeneratorActive() && s.isQueueEmpty()) { + break; + } + s.fill_queue(); + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + rc.check_section( + &fas_queue[0].entry[fas_queue[0].read_idx & QUEUE_LEN_MASK]); + fas_queue[0].read_idx++; + } + in_manage = false; + uint32_t to_dt = rc.total_ticks; + float planned_time = (to_dt - from_dt) * 1.0 / 16000000; + printf("%d: planned time in buffer: %.6fs\n", i, planned_time); + // This must be ensured, so that the stepper does not run out of commands + old_planned_time_in_buffer = planned_time; + } + test(!s.isRampGeneratorActive(), "too many commands created"); + 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 + printf("Total steps = %d\n", rc.pos); + assert(rc.pos == 4000); + + printf("TEST_05 Part PASSED\n"); + } +}; + +FastAccelStepperTest test; +void inject_fill_interrupt(int mark) { + if ((mark == enable_inject_on_mark) && !in_manage) { + in_manage = true; + test.inject(); + in_manage = false; + } +} +void noInterrupts() { + if (enable_stepper_manage_on_noInterrupts && !in_manage) { + in_manage = true; + test.inject(); + in_manage = false; + } +} +void interrupts() { + if (enable_stepper_manage_on_interrupts && !in_manage) { + in_manage = true; + test.inject(); + in_manage = false; + } +} +int main() { + enable_stepper_manage_on_interrupts = false; + enable_stepper_manage_on_noInterrupts = false; + enable_inject_on_mark = -1; + test.do_test(); + + enable_stepper_manage_on_interrupts = false; + enable_stepper_manage_on_noInterrupts = false; + enable_inject_on_mark = 0; + test.do_test(); + + enable_stepper_manage_on_interrupts = false; + enable_stepper_manage_on_noInterrupts = false; + enable_inject_on_mark = 1; + test.do_test(); + + enable_stepper_manage_on_interrupts = false; + enable_stepper_manage_on_noInterrupts = false; + enable_inject_on_mark = 2; + test.do_test(); + + enable_stepper_manage_on_interrupts = false; + enable_stepper_manage_on_noInterrupts = true; + enable_inject_on_mark = -1; + test.do_test(); + + enable_stepper_manage_on_interrupts = true; + enable_stepper_manage_on_noInterrupts = false; + enable_inject_on_mark = -1; + test.do_test(); + + printf("TEST_05 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_06.cpp b/extras/tests/pc_based/test_06.cpp new file mode 100644 index 0000000..99b8a99 --- /dev/null +++ b/extras/tests/pc_based/test_06.cpp @@ -0,0 +1,189 @@ +#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 do_test1() { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + int32_t steps = 15000; + + // This sequence caused no stop: + // M1 N A100000 V4000 R15000 + // V4300 U + // S + + assert(s.isQueueEmpty()); + s.setSpeedInUs(4000); + s.setAcceleration(100000); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(steps); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + bool speed_changed = false; + bool stop_initiated = false; + for (int i = 0; i < steps; i++) { + if (!speed_changed && (s.getCurrentPosition() >= 1000)) { + puts("Change speed to 4300us"); + s.fill_queue(); // ensure queue is not empty + speed_changed = true; + s.setSpeedInUs(4300); + s.applySpeedAcceleration(); + } + if (!stop_initiated && (s.getCurrentPosition() >= 2000)) { + puts("Init stop"); + s.fill_queue(); // ensure queue is not empty + stop_initiated = true; + s.stopMove(); + } + 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(); + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + 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; + } + test(!s.isRampGeneratorActive(), "too many commands created"); + test(s.getCurrentPosition() != steps, "has not stopped"); + 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 + } + + void do_test2(uint32_t stop_at_position) { + printf("do_test2 with stop at %d\n", stop_at_position); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + int32_t steps = 15000; + + // This sequence does not run to position 9999: + // M1 N A100 V100 P9999 w100 S W P9999 + + assert(s.isQueueEmpty()); + s.setSpeedInUs(100); + s.setAcceleration(100); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.moveTo(9999); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + bool stop_initiated = false; + bool restarted = false; + for (int i = 0; i < steps; i++) { + if (!stop_initiated && (s.getCurrentPosition() >= stop_at_position)) { + puts("Init stop"); + s.fill_queue(); // ensure queue is not empty + stop_initiated = true; + s.stopMove(); + } + 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"); + } + s.fill_queue(); + uint32_t from_dt = rc.total_ticks; + if (!s.isQueueEmpty()) { + while (!s.isQueueEmpty()) { + 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; + } + if (!s.isRampGeneratorActive()) { + if (restarted) { + break; + } + puts("Continue move to end position"); + rc.next_ramp(); + restarted = true; + s.moveTo(9999); + } + } + printf("do_test2 with stop at %d\n", stop_at_position); + test(!s.isRampGeneratorActive(), "too many commands created"); + printf("getCurrentPosition() = %d\n", s.getCurrentPosition()); + test(s.getCurrentPosition() == 9999, "has not reached end position"); + 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.do_test1(); + test.do_test2(100); + test.do_test2(5000); + test.do_test2(9000); + printf("TEST_06 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_07.cpp b/extras/tests/pc_based/test_07.cpp new file mode 100644 index 0000000..c54ccc9 --- /dev/null +++ b/extras/tests/pc_based/test_07.cpp @@ -0,0 +1,132 @@ +#include +#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 do_test(uint64_t dt) { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + // Reproduce test sequence 06 + + assert(s.getDirectionPin() == PIN_UNDEFINED); + uint32_t speed_us = 100; + int32_t steps = 32000; + assert(s.isQueueEmpty()); + s.setSpeedInUs(speed_us); + s.setAcceleration(10000); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(steps); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + uint64_t next_speed_change = dt; + uint64_t mid_point_ticks = 0; + + char fname[100]; + snprintf(fname, 100, "test_07.gnuplot"); + rc.start_plot(fname); + for (int i = 0; i < 10 * steps; i++) { + if (rc.total_ticks > next_speed_change) { + next_speed_change += TICKS_PER_S / 10; + speed_us = 190 - speed_us; + printf("Change speed to %d\n", speed_us); + s.setSpeedInUs(speed_us); + s.applySpeedAcceleration(); + } + 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(); + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + if ((mid_point_ticks == 0) && (rc.pos >= steps / 2)) { + mid_point_ticks = rc.total_ticks; + } + 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; + test(s.getCurrentPosition() <= steps, "has overshot"); + } + rc.finish_plot(); + test(!s.isRampGeneratorActive(), "too many commands created"); + test(s.getCurrentPosition() == steps, "has not reached target position"); + printf("Total time %f\n", rc.total_ticks / 16000000.0); + + printf("Time coasting = %d\n", rc.time_coasting); + test(rc.time_coasting < 46000000, "too much coasting"); + + printf("mid point @ %" PRIu64 " => total = %" PRIu64 + ", total ticks = %" PRIu64 "\n", + mid_point_ticks, 2 * mid_point_ticks, rc.total_ticks); +#define ALLOWED_ASYMMETRY 1000000L + printf("%ld\n", ALLOWED_ASYMMETRY); + test(mid_point_ticks * 2 < rc.total_ticks + ALLOWED_ASYMMETRY, + "ramp is not symmetric 1"); + test(mid_point_ticks * 2 > rc.total_ticks - ALLOWED_ASYMMETRY, + "ramp is not symmetric 2"); + +#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; + for (uint64_t time_shift = 0; time_shift < TICKS_PER_S / 10; + time_shift += TICKS_PER_S / 7000) { + test.do_test(time_shift); + } + printf("TEST_07 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_08.cpp b/extras/tests/pc_based/test_08.cpp new file mode 100644 index 0000000..8885cd9 --- /dev/null +++ b/extras/tests/pc_based/test_08.cpp @@ -0,0 +1,103 @@ +#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(uint32_t accel, uint32_t steps) { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + // Reproduce test sequence 06 + + uint32_t speed_us = 40; + assert(s.isQueueEmpty()); + s.setSpeedInUs(speed_us); + s.setAcceleration(accel); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(steps); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + + char fname[100]; + snprintf(fname, 100, "test_08.gnuplot"); + rc.start_plot(fname); + for (int i = 0; i < 100 * steps; i++) { + 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(); + 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; + } + rc.finish_plot(); + test(!s.isRampGeneratorActive(), "too many commands created"); + test(s.getCurrentPosition() == steps, "has not reached target position"); + 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(100000, 400); + test.ramp(100000, 600); + for (uint32_t i = 0; i < 30; i++) { + test.ramp(1000000, 1000 + i); + } + printf("TEST_08 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_09.cpp b/extras/tests/pc_based/test_09.cpp new file mode 100644 index 0000000..3215e1f --- /dev/null +++ b/extras/tests/pc_based/test_09.cpp @@ -0,0 +1,99 @@ +#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(uint32_t steps) { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + uint32_t speed_us = 40; + assert(s.isQueueEmpty()); + s.setSpeedInUs(speed_us); + s.setAcceleration(1000000); + s.fill_queue(); + assert(s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + + char fname[100]; + snprintf(fname, 100, "test_09.gnuplot"); + rc.start_plot(fname); + for (int j = 0; j < 2; j++) { + s.move(steps); + for (int i = 0; i < 100 * steps; i++) { + 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(); + 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; + } + } + rc.finish_plot(); + test(!s.isRampGeneratorActive(), "too many commands created"); + test(s.getCurrentPosition() == 2 * steps, + "has not reached target position"); + 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(53); + printf("TEST_09 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_10.cpp b/extras/tests/pc_based/test_10.cpp new file mode 100644 index 0000000..fa0bf32 --- /dev/null +++ b/extras/tests/pc_based/test_10.cpp @@ -0,0 +1,107 @@ +#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 reduce_acceleration() { + puts("Test test_speed_decrease"); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + int32_t steps = 100000; + + // Increase speed to 400, then further to 300 + // Identified bug was a fast jump to 300 without acceleration + assert(s.isQueueEmpty()); + s.setSpeedInUs(30); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.moveByAcceleration(17164); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + int accel_decreased = false; + uint32_t count_state_dec = 0; + for (int i = 0; i < steps * 10; i++) { + if (!accel_decreased && (s.getCurrentPosition() >= 35000)) { + puts("Change acceleration"); + accel_decreased = true; + s.moveByAcceleration(-1000); + s.fill_queue(); // ensure queue is not empty + } + if (accel_decreased && (s.getCurrentPosition() >= 37000)) { + test((s.rampState() & RAMP_STATE_MASK) != RAMP_STATE_COAST, + "Coasting is wrong state here"); + break; + } + 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 ((s.rampState() & RAMP_STATE_MASK) == RAMP_STATE_DECELERATE) { + count_state_dec++; + } + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + 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; + } + 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.reduce_acceleration(); + printf("TEST_10 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_11.cpp b/extras/tests/pc_based/test_11.cpp new file mode 100644 index 0000000..9046d94 --- /dev/null +++ b/extras/tests/pc_based/test_11.cpp @@ -0,0 +1,108 @@ +#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 reduce_speed() { + puts("Test test_speed_decrease"); + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + int32_t steps = 100; + + // M1 A1000 V10000 P100 w300 V100000 U + assert(s.isQueueEmpty()); + s.setAcceleration(1000); + s.setSpeedInUs(10000); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.moveTo(100); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + int speed_decreased = false; + uint32_t count_state_dec = 0; + for (int i = 0; i < steps * 10; i++) { + if (!speed_decreased && (s.getCurrentPosition() >= 35)) { + puts("Change speed"); + speed_decreased = true; + s.setSpeedInUs(100000); + s.applySpeedAcceleration(); + s.fill_queue(); // ensure queue is not empty + } + if (speed_decreased && (s.getCurrentPosition() >= 90)) { + test((s.rampState() & RAMP_STATE_MASK) == RAMP_STATE_COAST, + "Coasting is required state here"); + break; + } + 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 ((s.rampState() & RAMP_STATE_MASK) == RAMP_STATE_DECELERATE) { + count_state_dec++; + } + uint32_t from_dt = rc.total_ticks; + while (!s.isQueueEmpty()) { + 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; + } + 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.reduce_speed(); + printf("TEST_11 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_12.cpp b/extras/tests/pc_based/test_12.cpp new file mode 100644 index 0000000..e239844 --- /dev/null +++ b/extras/tests/pc_based/test_12.cpp @@ -0,0 +1,103 @@ +#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(uint32_t accel, uint32_t speed_us, uint32_t steps, + bool reach_coasting) { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + assert(s.isQueueEmpty()); + s.setSpeedInUs(speed_us); + s.setAcceleration(accel); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(steps); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + + char fname[100]; + snprintf(fname, 100, "test_12.gnuplot"); + rc.start_plot(fname); + bool coast = false; + for (int i = 0; i < 100 * steps; i++) { + 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(); + 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++; + } + if ((s.rampState() & RAMP_STATE_MASK) == RAMP_STATE_COAST) { + coast = true; + } + 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; + } + rc.finish_plot(); + test(!s.isRampGeneratorActive(), "too many commands created"); + test(s.getCurrentPosition() == steps, "has not reached target position"); + test(coast == reach_coasting, "coasting target not met"); + 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(1, 1000, 999000, false); + test.ramp(1, 1000, 1001000, true); + printf("TEST_12 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_13.cpp b/extras/tests/pc_based/test_13.cpp new file mode 100644 index 0000000..ddaf24f --- /dev/null +++ b/extras/tests/pc_based/test_13.cpp @@ -0,0 +1,113 @@ +#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(uint32_t accel, uint32_t speed_us, uint32_t steps, + bool reach_coasting) { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + assert(s.isQueueEmpty()); + s.setSpeedInUs(speed_us); + s.setAcceleration(accel); + s.fill_queue(); + assert(s.isQueueEmpty()); + s.move(steps); + s.fill_queue(); + assert(!s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + + char fname[100]; + snprintf(fname, 100, "test_13.gnuplot"); + rc.start_plot(fname); + bool coast = false; + for (int i = 0; i < 100 * steps; i++) { + 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(); + 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++; + } + if ((s.rampState() & RAMP_STATE_MASK) == RAMP_STATE_COAST) { + coast = true; + } + 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; + } + 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++; + } + rc.finish_plot(); + test(!s.isRampGeneratorActive(), "too many commands created"); + test(s.getCurrentPosition() == steps, "has not reached target position"); + test(coast == reach_coasting, "coasting target not met"); + 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; + for (uint16_t s = 1; s <= 255; s++) { + printf("test with steps s=%d\n", s); + test.ramp(INT32_MAX, 50, s, false); + puts(""); + } + printf("TEST_13 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_14.cpp b/extras/tests/pc_based/test_14.cpp new file mode 100644 index 0000000..1c6b243 --- /dev/null +++ b/extras/tests/pc_based/test_14.cpp @@ -0,0 +1,106 @@ +#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()); + + uint32_t speed_us = 1000000 / 3600; + assert(s.isQueueEmpty()); + s.setSpeedInUs(speed_us); + s.setAcceleration(320); + s.fill_queue(); + assert(s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + + char fname[100]; + snprintf(fname, 100, "test_14.gnuplot"); + rc.start_plot(fname); + s.runForward(); + for (int i = 0; i < 2000; i++) { + if (i == 1000) { + printf("Change speed\n"); + s.setSpeedInUs(10000); + s.applySpeedAcceleration(); + } + 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(); + 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_14 PASSED\n"); + return 0; +} diff --git a/extras/tests/pc_based/test_15.cpp b/extras/tests/pc_based/test_15.cpp new file mode 100644 index 0000000..4ce86b1 --- /dev/null +++ b/extras/tests/pc_based/test_15.cpp @@ -0,0 +1,119 @@ +#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(uint8_t forward_planning, uint32_t expected_steps) { + init_queue(); + FastAccelStepper s = FastAccelStepper(); + s.init(NULL, 0, 0); + RampChecker rc = RampChecker(); + assert(0 == s.getCurrentPosition()); + + uint32_t speed_us = 1000000 / 3600; + assert(s.isQueueEmpty()); + s.setSpeedInUs(speed_us); + s.setAcceleration(320); + s.setForwardPlanningTimeInMs(forward_planning); + s.fill_queue(); + assert(s.isQueueEmpty()); + float old_planned_time_in_buffer = 0; + + char fname[100]; + float sum_planning_time = 0; + float points = 0; + snprintf(fname, 100, "test_15_%dms.gnuplot", forward_planning); + rc.start_plot(fname); + s.runForward(); + for (int i = 0; i < 2000; i++) { + if (i == 1000) { + printf("Change speed\n"); + s.setSpeedInUs(10000); + s.applySpeedAcceleration(); + } + 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(); + 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); + sum_planning_time += planned_time; + points += 1.0; + // 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"); + printf("current position = %d\n", s.getCurrentPosition()); + test(s.getCurrentPosition() > expected_steps - 10, "stepper runs too slow"); + test(s.getCurrentPosition() < expected_steps + 10, "stepper runs too fast"); + printf("Total time %f\n", rc.total_ticks / 16000000.0); + float avg_time = sum_planning_time / points * 1000.0; + printf("Average planning time: %f ms\n", avg_time); + test(avg_time < forward_planning + 1, "too much forward planning"); + +#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; + // run the ramp twice with 20 and with 5ms planning time. + // the ramp will change speed after half of the loops. + // The 5ms ramp will not have 20ms coasting in the buffer and as such runs much shorter. + test.ramp(20, 76936); + test.ramp(5, 11273); + printf("TEST_15 PASSED\n"); + return 0; +} diff --git a/extras/tests/simavr_based/.gitignore b/extras/tests/simavr_based/.gitignore new file mode 100644 index 0000000..8089a5a --- /dev/null +++ b/extras/tests/simavr_based/.gitignore @@ -0,0 +1,2 @@ +.links +.makefiles diff --git a/extras/tests/simavr_based/Makefile b/extras/tests/simavr_based/Makefile new file mode 100644 index 0000000..150ca2b --- /dev/null +++ b/extras/tests/simavr_based/Makefile @@ -0,0 +1,62 @@ +ifndef SILENCE + SILENCE=0 +endif + +PRJ_ROOT=$(shell git rev-parse --show-toplevel) +TESTS=$(wildcard test_*) +TESTS_SD=$(wildcard test_*sd_*) + +TEST_FILES=$(addsuffix /.tested,$(TESTS)) +SD_SRC_DIRS=$(addsuffix /src/.dir,$(TESTS_SD)) + +SRC=$(wildcard ../../src/*) + +test: $(TEST_FILES) pmf externalCall + +pmf: + make -C test_pmf + +externalCall: + make -C test_externalCall + +%/.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 $@) + cd $(dir $@); ln -sf $(PRJ_ROOT)/examples/StepperDemo/* . + +run_avr: simavr/simavr/run_avr + ln -s simavr/simavr/run_avr . + +simavr/simavr/run_avr: simavr/simavr/sim/run_avr.c + make -C simavr build-simavr + +simavr/simavr/sim/run_avr.c: + # git clone https://github.com/gin66/simavr.git + git clone https://github.com/buserror/simavr.git + (cd simavr;git checkout 132cc67) + +proper: clean + rm -f run_avr + rm -fR simavr + +clean: + rm -fR */.pio */.tested */x.vcd */result.txt + find . -type l -delete + find . -type d -empty -delete + rm -f .links .makefiles + diff --git a/extras/tests/simavr_based/Makefile.test b/extras/tests/simavr_based/Makefile.test new file mode 100644 index 0000000..f621550 --- /dev/null +++ b/extras/tests/simavr_based/Makefile.test @@ -0,0 +1,127 @@ +# +# 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 0 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 + cat expect.txt + +x.vcd: $(SRC) ../run_avr platformio.ini + env pio run -e $(DUT) || ~/.platformio/penv/bin/pio run -e $(DUT) || ~/.local/bin/pio run -e $(DUT) + ../run_avr $(FIRMWARE) -m $(DEVICE) -o x.vcd $(TRACES) + +clean: + rm -fR .pio .tested x.vcd result.txt diff --git a/extras/tests/simavr_based/Raw/platformio.ini b/extras/tests/simavr_based/Raw/platformio.ini new file mode 100644 index 0000000..709f819 --- /dev/null +++ b/extras/tests/simavr_based/Raw/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 SIM_TEST_INPUT='"? M1 V60 A40000 f w400 A40000 P0 w1000 W "' + +[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/eval.awk b/extras/tests/simavr_based/eval.awk new file mode 100644 index 0000000..ea82497 --- /dev/null +++ b/extras/tests/simavr_based/eval.awk @@ -0,0 +1,128 @@ +BEGIN { + ref = 16*100 + dump_all = 0 +} + +dump_all == 1{print} + +/^\$var wire 1/ { + names[++name_i] = $5 + sym[$4] = $5 + to_sym[$5] = $4 + period_hl_hl[$4] = 0 + period_lh_lh[$4] = 0 + time_h[$4] = 0 + time_l[$4] = 0 + transition_l_h[$4] = 0 + transition_h_l[$4] = 0 + cnt_l_h[$4] = 0 + cnt_h_l[$4] = 0 + max_time_h[$4] = 0 + sum_time_h[$4] = 0 + state[$4] = "X" +} + +/^#/ { time = substr($1,2) + 0 } + +/^1.$/ { + s = substr($1,2) + if(!SILENCE) printf("%s=1 ", sym[s]) + if (state[s] == 0) { + # transition L->H + cnt_l_h[s]++ + last = transition_l_h[s] + transition_l_h[s] = time + if (last > 0) { + period_lh_lh[s] = time - last + } + last = transition_h_l[s] + if (last > 0) { + time_l[s] = time - last + } + + if (sym[s] ~ /Step/) { + channel = substr(sym[s],5) + if(!SILENCE) printf("%s: ", channel) + dir = "Dir" channel + if (dir in to_sym) { + dir_sym = to_sym[dir] + if (state[dir_sym] == 0) { + position[channel]-- + } + else { + position[channel]++ + } + if(!SILENCE) printf("position=%d ",position[channel]) + } + if(!SILENCE) printf("period=%.1fus high time=%.1fus", + period_lh_lh[s]/ref,time_h[s]/ref) + } + } + if(!SILENCE) printf("\n") + state[s] = 1 +} +/^0.$/ { + s = substr($1,2) + if(!SILENCE) printf("%s=0 ", sym[s]) + if (state[s] == 1) { + # transition H->L + cnt_h_l[s]++ + last = transition_h_l[s] + transition_h_l[s] = time + if (last > 0) { + period_hl_hl[s] = time - last + } + last = transition_l_h[s] + if (last > 0) { + time_h[s] = time - last + } + if (sym[s] ~ /FillISR/) { + if(!SILENCE) printf("period=%.1fus ", period_lh_lh[s]/ref) + } + h_time = time_h[s]/ref + if(!SILENCE) printf("high time=%.1fus", h_time) + sum_time_h[s] += h_time + if (h_time > max_time_h[s]) { + max_time_h[s] = h_time + } + } + if(!SILENCE) printf("\n") + state[s] = 0 +} + +END { + n = asort(names) + for (i = 1;i <= n;i++) { + name = names[i] + s = to_sym[name] + info = sprintf("%8s: %8d*L->H, %8d*H->L",name,cnt_l_h[s],cnt_h_l[s]) + if (name ~ /Step/) { + info = sprintf("%s, Max High=%dus Total High=%dus", info, max_time_h[s], sum_time_h[s]) + } + if(!SILENCE) print(info) + if (name !~ /ISR/) { + print(info) >"result.txt" + } + } + channels["A"]=1 + channels["B"]=1 + channels["C"]=1 + for (ch in channels) { + if (ch in position) { + info = sprintf("Position[%s]=%d\n",ch,position[ch]) + if(!SILENCE) print(info) + print(info) >"result.txt" + } + } + + for (i = 1;i <= n;i++) { + name = names[i] + s = to_sym[name] + if (max_time_h[s] > 0) { + info = sprintf("Time in %s max=%d us, total=%d us\n",name,max_time_h[s], sum_time_h[s]) + if(!SILENCE) print(info) + print(info) >"result.txt" + } + } +} + diff --git a/extras/tests/simavr_based/judge.awk b/extras/tests/simavr_based/judge.awk new file mode 100644 index 0000000..848ced2 --- /dev/null +++ b/extras/tests/simavr_based/judge.awk @@ -0,0 +1,38 @@ +BEGIN { + ok = 1 + timing = 0 + if (DIR ~ /timing/) { + timing = 1 + } +} + +FNR == NR { + # result.txt + lines[FNR] = $0 +} + +FNR != NR { + # expect.txt + r = lines[FNR] + e = $0 + + r_test = r + e_test = e + + if (timing == 0) { + gsub(/[0-9 ]*us/,"", r_test) + gsub(/[0-9 ]*us/,"", e_test) + } + if (e_test != r_test) { + print("result:",r) + print("expect:",e) + ok = 0 + } +} + +END { + if (ok == 1) { + print("PASS") + print("PASS") > ".tested" + } +} diff --git a/extras/tests/simavr_based/judge_pos0.awk b/extras/tests/simavr_based/judge_pos0.awk new file mode 100644 index 0000000..db0488c --- /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/off_test_seq_03/platformio.ini b/extras/tests/simavr_based/off_test_seq_03/platformio.ini new file mode 100644 index 0000000..c50a5dd --- /dev/null +++ b/extras/tests/simavr_based/off_test_seq_03/platformio.ini @@ -0,0 +1,26 @@ +; 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] + +[env:avr] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall -D SIM_TEST_INPUT='"t M1 03 I R W "' -DSIMAVR_FOC_WORKAROUND +lib_extra_dirs = ../../../../.. + +[env:atmega2560] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/off_test_seq_04/platformio.ini b/extras/tests/simavr_based/off_test_seq_04/platformio.ini new file mode 100644 index 0000000..104625e --- /dev/null +++ b/extras/tests/simavr_based/off_test_seq_04/platformio.ini @@ -0,0 +1,26 @@ +; 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] + +[env:avr] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall -D SIM_TEST_INPUT='"t M1 04 I R W "' -DSIMAVR_FOC_WORKAROUND +lib_extra_dirs = ../../../../.. + +[env:atmega2560] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/off_test_seq_05/platformio.ini b/extras/tests/simavr_based/off_test_seq_05/platformio.ini new file mode 100644 index 0000000..ccb3ba5 --- /dev/null +++ b/extras/tests/simavr_based/off_test_seq_05/platformio.ini @@ -0,0 +1,26 @@ +; 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] + +[env:avr] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall -D SIM_TEST_INPUT='"t M1 05 I R W "' -DSIMAVR_FOC_WORKAROUND +lib_extra_dirs = ../../../../.. + +[env:atmega2560] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/off_test_timing/expect.txt b/extras/tests/simavr_based/off_test_timing/expect.txt new file mode 100644 index 0000000..e69de29 diff --git a/extras/tests/simavr_based/off_test_timing/platformio.ini b/extras/tests/simavr_based/off_test_timing/platformio.ini new file mode 100644 index 0000000..b7e6869 --- /dev/null +++ b/extras/tests/simavr_based/off_test_timing/platformio.ini @@ -0,0 +1,30 @@ +; 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] +build_flags = -DTEST_TIMING + +[env:atmega328p] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall -DSIMAVR_FOC_WORKAROUND ${common.build_flags} +lib_extra_dirs = ../../../../.. diff --git a/extras/tests/simavr_based/off_test_timing/src/main.ino b/extras/tests/simavr_based/off_test_timing/src/main.ino new file mode 100644 index 0000000..cfda1b5 --- /dev/null +++ b/extras/tests/simavr_based/off_test_timing/src/main.ino @@ -0,0 +1,40 @@ +#include +#include +#include + +float acceleration_f; +uint32_t acceleration_i; +uint32_t steps; + +void setup() { + Serial.begin(115200); + Serial.println("Start"); + + digitalWrite(stepPinStepperA, LOW); + digitalWrite(stepPinStepperB, LOW); + pinMode(stepPinStepperA, OUTPUT); + pinMode(stepPinStepperB, OUTPUT); + + acceleration_f = 12345.0; + acceleration_i = 12345.0; + steps = 10; +} +void loop() { + digitalWrite(stepPinStepperA, HIGH); + uint32_t x; + // x = calculate_ticks_v1(steps, acceleration_f); + // x = calculate_ticks_v2(steps, acceleration_f); + // x = calculate_ticks_v3(steps, acceleration_f); + // x = calculate_ticks_v4(steps, acceleration_i); + // x = calculate_ticks_v5(steps, acceleration_i); + // x = calculate_ticks_v7(0x1234000, 1000); + x = calculate_ticks_v8(0x1234000, 1000); + digitalWrite(stepPinStepperA, LOW); + + digitalWrite(stepPinStepperB, HIGH); + digitalWrite(stepPinStepperB, LOW); + Serial.println(x); + delay(100); + noInterrupts(); + sleep_cpu(); +} diff --git a/extras/tests/simavr_based/test_externalCall/.gitignore b/extras/tests/simavr_based/test_externalCall/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /dev/null +++ b/extras/tests/simavr_based/test_externalCall/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_externalCall/Makefile b/extras/tests/simavr_based/test_externalCall/Makefile new file mode 100644 index 0000000..557a2c6 --- /dev/null +++ b/extras/tests/simavr_based/test_externalCall/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 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 + cat expect.txt + +x.vcd: $(SRC) ../run_avr platformio.ini src/ExternalCall.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/ExternalCall.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/ExternalCall/ExternalCall.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt diff --git a/extras/tests/simavr_based/test_externalCall/expect.txt b/extras/tests/simavr_based/test_externalCall/expect.txt new file mode 100644 index 0000000..20c8caf --- /dev/null +++ b/extras/tests/simavr_based/test_externalCall/expect.txt @@ -0,0 +1,22 @@ + DirA: 3*L->H, 2*H->L + DirB: 3*L->H, 2*H->L + EnableA: 2*L->H, 1*H->L + EnableB: 2*L->H, 1*H->L + StepA: 2890*L->H, 2890*H->L, Max High=9us Total High=13574us + StepB: 2890*L->H, 2890*H->L, Max High=9us Total High=13454us +Position[A]=190 + +Position[B]=190 + +Time in DirA max=10063958 us, total=15122519 us + +Time in DirB max=39260160 us, total=44318721 us + +Time in EnableA max=4162 us, total=4162 us + +Time in EnableB max=29200356 us, total=29200356 us + +Time in StepA max=9 us, total=13574 us + +Time in StepB max=9 us, total=13454 us + diff --git a/extras/tests/simavr_based/test_externalCall/platformio.ini b/extras/tests/simavr_based/test_externalCall/platformio.ini new file mode 100644 index 0000000..ed29ed7 --- /dev/null +++ b/extras/tests/simavr_based/test_externalCall/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 + +[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_issue150/.gitignore b/extras/tests/simavr_based/test_issue150/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /dev/null +++ b/extras/tests/simavr_based/test_issue150/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue150/Makefile b/extras/tests/simavr_based/test_issue150/Makefile new file mode 100644 index 0000000..4bf477f --- /dev/null +++ b/extras/tests/simavr_based/test_issue150/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/Issue150.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/Issue150.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue150/Issue150.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue150/expect.txt b/extras/tests/simavr_based/test_issue150/expect.txt new file mode 100644 index 0000000..01c02d1 --- /dev/null +++ b/extras/tests/simavr_based/test_issue150/expect.txt @@ -0,0 +1,16 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 2*L->H, 1*H->L + EnableB: 0*L->H, 0*H->L + StepA: 4000*L->H, 4000*H->L, Max High=10us Total High=20775us + StepB: 2*L->H, 2*H->L, Max High=4us Total High=9us +Position[A]=4000 + +Position[B]=2 + +Time in EnableA max=4194 us, total=4194 us + +Time in StepA max=10 us, total=20775 us + +Time in StepB max=4 us, total=9 us + diff --git a/extras/tests/simavr_based/test_issue150/platformio.ini b/extras/tests/simavr_based/test_issue150/platformio.ini new file mode 100644 index 0000000..ed29ed7 --- /dev/null +++ b/extras/tests/simavr_based/test_issue150/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 + +[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_issue151/.gitignore b/extras/tests/simavr_based/test_issue151/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /dev/null +++ b/extras/tests/simavr_based/test_issue151/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue151/Makefile b/extras/tests/simavr_based/test_issue151/Makefile new file mode 100644 index 0000000..153e952 --- /dev/null +++ b/extras/tests/simavr_based/test_issue151/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/Issue151.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/Issue151.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue151/Issue151.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue151/expect.txt b/extras/tests/simavr_based/test_issue151/expect.txt new file mode 100644 index 0000000..2ce22e8 --- /dev/null +++ b/extras/tests/simavr_based/test_issue151/expect.txt @@ -0,0 +1,16 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 2*L->H, 1*H->L + EnableB: 0*L->H, 0*H->L + StepA: 20000*L->H, 20000*H->L, Max High=10us Total High=101913us + StepB: 2*L->H, 2*H->L, Max High=4us Total High=9us +Position[A]=20000 + +Position[B]=2 + +Time in EnableA max=4203 us, total=4203 us + +Time in StepA max=10 us, total=101913 us + +Time in StepB max=4 us, total=9 us + diff --git a/extras/tests/simavr_based/test_issue151/platformio.ini b/extras/tests/simavr_based/test_issue151/platformio.ini new file mode 100644 index 0000000..ed29ed7 --- /dev/null +++ b/extras/tests/simavr_based/test_issue151/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 + +[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_issue152/.gitignore b/extras/tests/simavr_based/test_issue152/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /dev/null +++ b/extras/tests/simavr_based/test_issue152/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue152/Makefile b/extras/tests/simavr_based/test_issue152/Makefile new file mode 100644 index 0000000..4a3dd31 --- /dev/null +++ b/extras/tests/simavr_based/test_issue152/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/Issue152.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/Issue152.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue152/Issue152.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue152/expect.txt b/extras/tests/simavr_based/test_issue152/expect.txt new file mode 100644 index 0000000..50f39e0 --- /dev/null +++ b/extras/tests/simavr_based/test_issue152/expect.txt @@ -0,0 +1,20 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 2*L->H, 1*H->L + EnableB: 0*L->H, 0*H->L + StepA: 20000*L->H, 20000*H->L, Max High=10us Total High=104644us + StepB: 2*L->H, 2*H->L, Max High=4us Total High=9us +Position[A]=20000 + +Position[B]=2 + +Time in EnableA max=4199 us, total=4199 us + +Time in FillISR max=1848 us, total=59230 us + +Time in StepA max=10 us, total=104644 us + +Time in StepB max=4 us, total=9 us + +Time in StepISR max=7 us, total=85706 us + diff --git a/extras/tests/simavr_based/test_issue152/platformio.ini b/extras/tests/simavr_based/test_issue152/platformio.ini new file mode 100644 index 0000000..9eae37d --- /dev/null +++ b/extras/tests/simavr_based/test_issue152/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 = ../../../../.. diff --git a/extras/tests/simavr_based/test_issue172/.gitignore b/extras/tests/simavr_based/test_issue172/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /dev/null +++ b/extras/tests/simavr_based/test_issue172/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue172/Makefile b/extras/tests/simavr_based/test_issue172/Makefile new file mode 100644 index 0000000..df8ded5 --- /dev/null +++ b/extras/tests/simavr_based/test_issue172/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/Issue172.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/Issue172.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue172/Issue172.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue172/expect.txt b/extras/tests/simavr_based/test_issue172/expect.txt new file mode 100644 index 0000000..bdc1faa --- /dev/null +++ b/extras/tests/simavr_based/test_issue172/expect.txt @@ -0,0 +1,20 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 2*L->H, 1*H->L + EnableB: 0*L->H, 0*H->L + StepA: 2060*L->H, 2060*H->L, Max High=9us Total High=8067us + 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=4204 us + +Time in FillISR max=1737 us, total=39497 us + +Time in StepA max=9 us, total=8067 us + +Time in StepB max=4 us, total=9 us + +Time in StepISR max=6 us, total=8184 us + diff --git a/extras/tests/simavr_based/test_issue172/platformio.ini b/extras/tests/simavr_based/test_issue172/platformio.ini new file mode 100644 index 0000000..9eae37d --- /dev/null +++ b/extras/tests/simavr_based/test_issue172/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 = ../../../../.. diff --git a/extras/tests/simavr_based/test_issue173/.gitignore b/extras/tests/simavr_based/test_issue173/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /dev/null +++ b/extras/tests/simavr_based/test_issue173/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue173/Makefile b/extras/tests/simavr_based/test_issue173/Makefile new file mode 100644 index 0000000..b70e556 --- /dev/null +++ b/extras/tests/simavr_based/test_issue173/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/Issue173.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/Issue173.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue173/Issue173.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue173/expect.txt b/extras/tests/simavr_based/test_issue173/expect.txt new file mode 100644 index 0000000..5358c49 --- /dev/null +++ b/extras/tests/simavr_based/test_issue173/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: 1050*L->H, 1050*H->L, Max High=9us Total High=4119us + 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=8409 us + +Time in FillISR max=1917 us, total=25558 us + +Time in StepA max=9 us, total=4119 us + +Time in StepB max=4 us, total=9 us + +Time in StepISR max=5 us, total=4224 us + diff --git a/extras/tests/simavr_based/test_issue173/platformio.ini b/extras/tests/simavr_based/test_issue173/platformio.ini new file mode 100644 index 0000000..9eae37d --- /dev/null +++ b/extras/tests/simavr_based/test_issue173/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 = ../../../../.. diff --git a/extras/tests/simavr_based/test_issue208/.gitignore b/extras/tests/simavr_based/test_issue208/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /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 0000000..d57f9fb --- /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 0000000..1bde710 --- /dev/null +++ b/extras/tests/simavr_based/test_issue208/expect.txt @@ -0,0 +1,18 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 0*L->H, 0*H->L + EnableB: 0*L->H, 0*H->L + StepA: 234999*L->H, 234999*H->L, Max High=10us Total High=923957us + StepB: 2*L->H, 2*H->L, Max High=12us Total High=25us +Position[A]=37271 + +Position[B]=2 + +Time in FillISR max=776 us, total=1220640 us + +Time in StepA max=10 us, total=923957 us + +Time in StepB max=12 us, total=25 us + +Time in StepISR max=6 us, total=894406 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 0000000..9eae37d --- /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 = ../../../../.. diff --git a/extras/tests/simavr_based/test_issue250/.gitignore b/extras/tests/simavr_based/test_issue250/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /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 0000000..ba9091d --- /dev/null +++ b/extras/tests/simavr_based/test_issue250/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/expect.txt b/extras/tests/simavr_based/test_issue250/expect.txt new file mode 100644 index 0000000..e69de29 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 0000000..9eae37d --- /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 = ../../../../.. 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 0000000..8eba6c8 --- /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 0000000..ba9091d --- /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 0000000..e69de29 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 0000000..4136b31 --- /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/extras/tests/simavr_based/test_issue280/.gitignore b/extras/tests/simavr_based/test_issue280/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /dev/null +++ b/extras/tests/simavr_based/test_issue280/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_issue280/Makefile b/extras/tests/simavr_based/test_issue280/Makefile new file mode 100644 index 0000000..afdd3ed --- /dev/null +++ b/extras/tests/simavr_based/test_issue280/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/Issue280.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/Issue280.ino: + mkdir -p src + cd src; ln -s ../../../../../examples/Issue280/Issue280.ino . + +clean: + rm -fR .pio .tested x.vcd result.txt + diff --git a/extras/tests/simavr_based/test_issue280/expect.txt b/extras/tests/simavr_based/test_issue280/expect.txt new file mode 100644 index 0000000..2106a9f --- /dev/null +++ b/extras/tests/simavr_based/test_issue280/expect.txt @@ -0,0 +1,16 @@ + DirA: 0*L->H, 1*H->L + DirB: 0*L->H, 0*H->L + EnableA: 3*L->H, 2*H->L + EnableB: 0*L->H, 0*H->L + StepA: 33872*L->H, 33872*H->L, Max High=10us Total High=133363us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in EnableA max=8284 us, total=12482 us + +Time in FillISR max=1734 us, total=175782 us + +Time in StepA max=10 us, total=133363 us + +Time in StepISR max=5 us, total=128886 us + diff --git a/extras/tests/simavr_based/test_issue280/platformio.ini b/extras/tests/simavr_based/test_issue280/platformio.ini new file mode 100644 index 0000000..9eae37d --- /dev/null +++ b/extras/tests/simavr_based/test_issue280/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 = ../../../../.. diff --git a/extras/tests/simavr_based/test_pmf/.gitignore b/extras/tests/simavr_based/test_pmf/.gitignore new file mode 100644 index 0000000..8eba6c8 --- /dev/null +++ b/extras/tests/simavr_based/test_pmf/.gitignore @@ -0,0 +1 @@ +src/ diff --git a/extras/tests/simavr_based/test_pmf/Makefile b/extras/tests/simavr_based/test_pmf/Makefile new file mode 100644 index 0000000..35f2835 --- /dev/null +++ b/extras/tests/simavr_based/test_pmf/Makefile @@ -0,0 +1,135 @@ +# +# 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 + cat expect.txt + +x.vcd: $(SRC) ../run_avr platformio.ini src/PMF_test.ino src/test_03.h + ~/.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/PMF_test.ino: + mkdir -p src + cd src; ln -s ../../../pc_based/PMF_test.ino . + +src/test_03.h: + mkdir -p src + cd src; ln -s ../../../pc_based/test_03.h . + +clean: + rm -fR .pio .tested x.vcd result.txt diff --git a/extras/tests/simavr_based/test_pmf/expect.txt b/extras/tests/simavr_based/test_pmf/expect.txt new file mode 100644 index 0000000..fcb6255 --- /dev/null +++ b/extras/tests/simavr_based/test_pmf/expect.txt @@ -0,0 +1,10 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 0*L->H, 0*H->L + EnableB: 0*L->H, 0*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 2*L->H, 2*H->L, Max High=12us Total High=16us +Position[B]=2 + +Time in StepB max=12 us, total=16 us + diff --git a/extras/tests/simavr_based/test_pmf/platformio.ini b/extras/tests/simavr_based/test_pmf/platformio.ini new file mode 100644 index 0000000..ed29ed7 --- /dev/null +++ b/extras/tests/simavr_based/test_pmf/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 + +[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_01a_2560t1/expect.txt b/extras/tests/simavr_based/test_sd_01a_2560t1/expect.txt new file mode 100644 index 0000000..d627db1 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_2560t1/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + EnableC: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=13us Total High=17018us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepC: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=450673 us, total=450673 us + +Time in StepA max=13 us, total=17018 us + diff --git a/extras/tests/simavr_based/test_sd_01a_2560t1/platformio.ini b/extras/tests/simavr_based/test_sd_01a_2560t1/platformio.ini new file mode 100644 index 0000000..b5de431 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_2560t1/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 SIM_TEST_INPUT='"? M1 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer1] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=1 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01a_2560t3/expect.txt b/extras/tests/simavr_based/test_sd_01a_2560t3/expect.txt new file mode 100644 index 0000000..80a60c0 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_2560t3/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + EnableC: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=13us Total High=17029us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepC: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=450673 us, total=450673 us + +Time in StepA max=13 us, total=17029 us + diff --git a/extras/tests/simavr_based/test_sd_01a_2560t3/platformio.ini b/extras/tests/simavr_based/test_sd_01a_2560t3/platformio.ini new file mode 100644 index 0000000..8d5714f --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_2560t3/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 SIM_TEST_INPUT='"? M1 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer3] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=3 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01a_2560t4/expect.txt b/extras/tests/simavr_based/test_sd_01a_2560t4/expect.txt new file mode 100644 index 0000000..80a60c0 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_2560t4/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + EnableC: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=13us Total High=17029us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepC: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=450673 us, total=450673 us + +Time in StepA max=13 us, total=17029 us + diff --git a/extras/tests/simavr_based/test_sd_01a_2560t4/platformio.ini b/extras/tests/simavr_based/test_sd_01a_2560t4/platformio.ini new file mode 100644 index 0000000..6dcccc3 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_2560t4/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 SIM_TEST_INPUT='"? M1 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer4] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01a_2560t5/expect.txt b/extras/tests/simavr_based/test_sd_01a_2560t5/expect.txt new file mode 100644 index 0000000..70a6e0e --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_2560t5/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + EnableC: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=13us Total High=17029us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepC: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=450673 us, total=450673 us + +Time in StepA max=13 us, total=17029 us + diff --git a/extras/tests/simavr_based/test_sd_01a_2560t5/platformio.ini b/extras/tests/simavr_based/test_sd_01a_2560t5/platformio.ini new file mode 100644 index 0000000..19393ef --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_2560t5/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 SIM_TEST_INPUT='"? M1 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer5] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=5 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01a_328p/expect.txt b/extras/tests/simavr_based/test_sd_01a_328p/expect.txt new file mode 100644 index 0000000..56dbb10 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=12us Total High=16420us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=442486 us, total=442486 us + +Time in StepA max=12 us, total=16420 us + diff --git a/extras/tests/simavr_based/test_sd_01a_328p/platformio.ini b/extras/tests/simavr_based/test_sd_01a_328p/platformio.ini new file mode 100644 index 0000000..8b5bd15 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_328p/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 SIM_TEST_INPUT='"? M1 A1000 V100 R3200 W t W "' + +[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_01a_leonardo/expect.txt b/extras/tests/simavr_based/test_sd_01a_leonardo/expect.txt new file mode 100644 index 0000000..da2a709 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_leonardo/expect.txt @@ -0,0 +1,12 @@ + DirA: 1*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=11us Total High=15936us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=335993 us, total=335993 us + +Time in StepA max=11 us, total=15936 us + diff --git a/extras/tests/simavr_based/test_sd_01a_leonardo/platformio.ini b/extras/tests/simavr_based/test_sd_01a_leonardo/platformio.ini new file mode 100644 index 0000000..73708b1 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01a_leonardo/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 SIM_TEST_INPUT='"? M1 A1000 V100 R3200 W t W "' + +[env:atmega32u4] +platform = atmelavr +board = leonardo +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01b_2560t1/expect.txt b/extras/tests/simavr_based/test_sd_01b_2560t1/expect.txt new file mode 100644 index 0000000..57634f1 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_2560t1/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 2*L->H, 1*H->L + EnableC: 1*L->H, 0*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 3200*L->H, 3200*H->L, Max High=13us Total High=16810us + StepC: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[B]=3200 + +Time in EnableB max=450586 us, total=450586 us + +Time in StepB max=13 us, total=16810 us + diff --git a/extras/tests/simavr_based/test_sd_01b_2560t1/platformio.ini b/extras/tests/simavr_based/test_sd_01b_2560t1/platformio.ini new file mode 100644 index 0000000..c27c416 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_2560t1/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 SIM_TEST_INPUT='"? M2 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer1] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=1 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01b_2560t3/expect.txt b/extras/tests/simavr_based/test_sd_01b_2560t3/expect.txt new file mode 100644 index 0000000..4588816 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_2560t3/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 2*L->H, 1*H->L + EnableC: 1*L->H, 0*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 3200*L->H, 3200*H->L, Max High=13us Total High=16811us + StepC: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[B]=3200 + +Time in EnableB max=450586 us, total=450586 us + +Time in StepB max=13 us, total=16811 us + diff --git a/extras/tests/simavr_based/test_sd_01b_2560t3/platformio.ini b/extras/tests/simavr_based/test_sd_01b_2560t3/platformio.ini new file mode 100644 index 0000000..9065780 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_2560t3/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 SIM_TEST_INPUT='"? M2 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer3] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=3 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01b_2560t4/expect.txt b/extras/tests/simavr_based/test_sd_01b_2560t4/expect.txt new file mode 100644 index 0000000..4588816 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_2560t4/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 2*L->H, 1*H->L + EnableC: 1*L->H, 0*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 3200*L->H, 3200*H->L, Max High=13us Total High=16811us + StepC: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[B]=3200 + +Time in EnableB max=450586 us, total=450586 us + +Time in StepB max=13 us, total=16811 us + diff --git a/extras/tests/simavr_based/test_sd_01b_2560t4/platformio.ini b/extras/tests/simavr_based/test_sd_01b_2560t4/platformio.ini new file mode 100644 index 0000000..8c92285 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_2560t4/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 SIM_TEST_INPUT='"? M2 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer4] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01b_2560t5/expect.txt b/extras/tests/simavr_based/test_sd_01b_2560t5/expect.txt new file mode 100644 index 0000000..01da8bf --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_2560t5/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 1*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 2*L->H, 1*H->L + EnableC: 1*L->H, 0*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 3200*L->H, 3200*H->L, Max High=13us Total High=16811us + StepC: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[B]=3200 + +Time in EnableB max=450586 us, total=450586 us + +Time in StepB max=13 us, total=16811 us + diff --git a/extras/tests/simavr_based/test_sd_01b_2560t5/platformio.ini b/extras/tests/simavr_based/test_sd_01b_2560t5/platformio.ini new file mode 100644 index 0000000..9d28af9 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_2560t5/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 SIM_TEST_INPUT='"? M2 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer5] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=5 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01b_328p/expect.txt b/extras/tests/simavr_based/test_sd_01b_328p/expect.txt new file mode 100644 index 0000000..b9e0b0b --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 2*L->H, 1*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 3200*L->H, 3200*H->L, Max High=12us Total High=16206us +Position[B]=3200 + +Time in EnableB max=442405 us, total=442405 us + +Time in StepB max=12 us, total=16206 us + diff --git a/extras/tests/simavr_based/test_sd_01b_328p/platformio.ini b/extras/tests/simavr_based/test_sd_01b_328p/platformio.ini new file mode 100644 index 0000000..1e5bee5 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01b_328p/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 SIM_TEST_INPUT='"? M2 A1000 V100 R3200 W t W "' + +[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_01c_2560t1/expect.txt b/extras/tests/simavr_based/test_sd_01c_2560t1/expect.txt new file mode 100644 index 0000000..a7b501d --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01c_2560t1/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 1*L->H, 0*H->L + EnableC: 2*L->H, 1*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepC: 3200*L->H, 3200*H->L, Max High=13us Total High=16832us +Position[C]=3200 + +Time in EnableC max=450495 us, total=450495 us + +Time in StepC max=13 us, total=16832 us + diff --git a/extras/tests/simavr_based/test_sd_01c_2560t1/platformio.ini b/extras/tests/simavr_based/test_sd_01c_2560t1/platformio.ini new file mode 100644 index 0000000..43ec528 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01c_2560t1/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 SIM_TEST_INPUT='"? M3 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer1] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=1 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01c_2560t3/expect.txt b/extras/tests/simavr_based/test_sd_01c_2560t3/expect.txt new file mode 100644 index 0000000..0604b6a --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01c_2560t3/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 1*L->H, 0*H->L + EnableC: 2*L->H, 1*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepC: 3200*L->H, 3200*H->L, Max High=13us Total High=16822us +Position[C]=3200 + +Time in EnableC max=450494 us, total=450494 us + +Time in StepC max=13 us, total=16822 us + diff --git a/extras/tests/simavr_based/test_sd_01c_2560t3/platformio.ini b/extras/tests/simavr_based/test_sd_01c_2560t3/platformio.ini new file mode 100644 index 0000000..508e805 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01c_2560t3/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 SIM_TEST_INPUT='"? M3 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer3] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=3 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01c_2560t4/expect.txt b/extras/tests/simavr_based/test_sd_01c_2560t4/expect.txt new file mode 100644 index 0000000..0604b6a --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01c_2560t4/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 1*L->H, 0*H->L + EnableC: 2*L->H, 1*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepC: 3200*L->H, 3200*H->L, Max High=13us Total High=16822us +Position[C]=3200 + +Time in EnableC max=450494 us, total=450494 us + +Time in StepC max=13 us, total=16822 us + diff --git a/extras/tests/simavr_based/test_sd_01c_2560t4/platformio.ini b/extras/tests/simavr_based/test_sd_01c_2560t4/platformio.ini new file mode 100644 index 0000000..62f88f4 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01c_2560t4/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 SIM_TEST_INPUT='"? M3 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer4] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_01c_2560t5/expect.txt b/extras/tests/simavr_based/test_sd_01c_2560t5/expect.txt new file mode 100644 index 0000000..61845f3 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01c_2560t5/expect.txt @@ -0,0 +1,15 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 1*L->H, 0*H->L + EnableA: 1*L->H, 0*H->L + EnableB: 1*L->H, 0*H->L + EnableC: 2*L->H, 1*H->L + StepA: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us + StepC: 3200*L->H, 3200*H->L, Max High=13us Total High=16822us +Position[C]=3200 + +Time in EnableC max=450494 us, total=450494 us + +Time in StepC max=13 us, total=16822 us + diff --git a/extras/tests/simavr_based/test_sd_01c_2560t5/platformio.ini b/extras/tests/simavr_based/test_sd_01c_2560t5/platformio.ini new file mode 100644 index 0000000..22adec6 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_01c_2560t5/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 SIM_TEST_INPUT='"? M3 A1000 V100 R3200 W t W "' + +[env:atmega2560_timer5] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall -DFAS_TIMER_MODULE=5 ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_sd_02_328p/expect.txt b/extras/tests/simavr_based/test_sd_02_328p/expect.txt new file mode 100644 index 0000000..e5342e5 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_02_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=11us Total High=16389us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=442485 us, total=442485 us + +Time in StepA max=11 us, total=16389 us + diff --git a/extras/tests/simavr_based/test_sd_02_328p/platformio.ini b/extras/tests/simavr_based/test_sd_02_328p/platformio.ini new file mode 100644 index 0000000..2c77da2 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_02_328p/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 SIM_TEST_INPUT='"? M1 A1000000 V50 R3200 W t W "' + +[env:atmega328p] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. \ No newline at end of file diff --git a/extras/tests/simavr_based/test_sd_03_328p/expect.txt b/extras/tests/simavr_based/test_sd_03_328p/expect.txt new file mode 100644 index 0000000..b994948 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_03_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=12us Total High=16367us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=225396 us, total=225396 us + +Time in StepA max=12 us, total=16367 us + diff --git a/extras/tests/simavr_based/test_sd_03_328p/platformio.ini b/extras/tests/simavr_based/test_sd_03_328p/platformio.ini new file mode 100644 index 0000000..a177e0b --- /dev/null +++ b/extras/tests/simavr_based/test_sd_03_328p/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 SIM_TEST_INPUT='"M1 A1000000 V50 R3200 W t W "' + +[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 new file mode 100644 index 0000000..c2f75c5 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/expect.txt @@ -0,0 +1,29 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + DirC: 0*L->H, 0*H->L + 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=314366us + StepB: 64000*L->H, 64000*H->L, Max High=30us Total High=360899us + StepC: 64000*L->H, 64000*H->L, Max High=33us Total High=459850us +Position[A]=64000 + +Position[B]=64000 + +Position[C]=64000 + +Time in EnableA max=233600 us, total=233600 us + +Time in EnableB max=246089 us, total=246089 us + +Time in EnableC max=254247 us, total=254247 us + +Time in FillISR max=2867 us, total=2123376 us + +Time in StepA max=25 us, total=314366 us + +Time in StepB max=30 us, total=360899 us + +Time in StepC max=33 us, total=459850 us + diff --git a/extras/tests/simavr_based/test_sd_04_timing_2560/platformio.ini b/extras/tests/simavr_based/test_sd_04_timing_2560/platformio.ini new file mode 100644 index 0000000..2fb1215 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_04_timing_2560/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 SIM_TEST_INPUT='"M1 A10000 V50 R64000 M2 A11000 V50 R64000 M3 A12000 V50 R64000 W M2 W M1 W t W "' -D SIMAVR_TIME_MEASUREMENT_QUEUE + +[env:atmega2560_timer4] +platform = atmelavr +board = megaatmega2560 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. + 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 new file mode 100644 index 0000000..0b39255 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/expect.txt @@ -0,0 +1,22 @@ + DirA: 0*L->H, 0*H->L + 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=12us Total High=4227us + StepB: 1000*L->H, 1000*H->L, Max High=16us Total High=4975us +Position[A]=1000 + +Position[B]=1000 + +Time in EnableA max=225400 us, total=225400 us + +Time in EnableB max=238121 us, total=238121 us + +Time in FillISR max=2674 us, total=48012 us + +Time in StepA max=12 us, total=4227 us + +Time in StepB max=16 us, total=4975 us + +Time in StepISR max=6 us, total=8566 us + diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p/platformio.ini b/extras/tests/simavr_based/test_sd_04_timing_328p/platformio.ini new file mode 100644 index 0000000..3c07918 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_04_timing_328p/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 SIM_TEST_INPUT='"M1 A100000 V40 R1000 M2 A100000 V40 R1000 W M1 W t W "' -D SIMAVR_TIME_MEASUREMENT + +[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_328p_37k/expect.txt b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt new file mode 100644 index 0000000..84227f3 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/expect.txt @@ -0,0 +1,16 @@ + DirA: 0*L->H, 0*H->L + 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=3941us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=1000 + +Time in EnableA max=225401 us, total=225401 us + +Time in FillISR max=2037 us, total=27769 us + +Time in StepA max=10 us, total=3941 us + +Time in StepISR max=5 us, total=3975 us + diff --git a/extras/tests/simavr_based/test_sd_04_timing_328p_37k/platformio.ini b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/platformio.ini new file mode 100644 index 0000000..46a94a5 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_04_timing_328p_37k/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 SIM_TEST_INPUT='"M1 A100000 V27 R1000 W "' -D SIMAVR_TIME_MEASUREMENT + +[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_05_328p/expect.txt b/extras/tests/simavr_based/test_sd_05_328p/expect.txt new file mode 100644 index 0000000..af1d67e --- /dev/null +++ b/extras/tests/simavr_based/test_sd_05_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 14440*L->H, 14440*H->L, Max High=11us Total High=55558us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in EnableA max=442487 us, total=442487 us + +Time in StepA max=11 us, total=55558 us + diff --git a/extras/tests/simavr_based/test_sd_05_328p/platformio.ini b/extras/tests/simavr_based/test_sd_05_328p/platformio.ini new file mode 100644 index 0000000..709f819 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_05_328p/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 SIM_TEST_INPUT='"? M1 V60 A40000 f w400 A40000 P0 w1000 W "' + +[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_06_328p/expect.txt b/extras/tests/simavr_based/test_sd_06_328p/expect.txt new file mode 100644 index 0000000..a173fbb --- /dev/null +++ b/extras/tests/simavr_based/test_sd_06_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 1*L->H, 1*H->L, Max High=5us Total High=5us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=1 + +Time in EnableA max=225397 us, total=225397 us + +Time in StepA max=5 us, total=5 us + diff --git a/extras/tests/simavr_based/test_sd_06_328p/platformio.ini b/extras/tests/simavr_based/test_sd_06_328p/platformio.ini new file mode 100644 index 0000000..b48d740 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_06_328p/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 SIM_TEST_INPUT='"M1 V40 A1000000 R1 w300 ? W "' + +[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_07_328p/expect.txt b/extras/tests/simavr_based/test_sd_07_328p/expect.txt new file mode 100644 index 0000000..c9c498b --- /dev/null +++ b/extras/tests/simavr_based/test_sd_07_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 106*L->H, 106*H->L, Max High=10us Total High=565us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=106 + +Time in EnableA max=225396 us, total=225396 us + +Time in StepA max=10 us, total=565 us + diff --git a/extras/tests/simavr_based/test_sd_07_328p/platformio.ini b/extras/tests/simavr_based/test_sd_07_328p/platformio.ini new file mode 100644 index 0000000..e052316 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_07_328p/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 SIM_TEST_INPUT='"M1 V40 A1000000 R53 w1000 R53 w1000 ? W "' + +[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_08_328p/expect.txt b/extras/tests/simavr_based/test_sd_08_328p/expect.txt new file mode 100644 index 0000000..1ed2df8 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_08_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 2*L->H, 2*H->L, Max High=5us Total High=10us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=2 + +Time in EnableA max=225403 us, total=225403 us + +Time in StepA max=5 us, total=10 us + diff --git a/extras/tests/simavr_based/test_sd_08_328p/platformio.ini b/extras/tests/simavr_based/test_sd_08_328p/platformio.ini new file mode 100644 index 0000000..1461a72 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_08_328p/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 SIM_TEST_INPUT='"M1 V100 A300 R1 w1000 R1 w1000 ? W "' + +[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_09_328p/expect.txt b/extras/tests/simavr_based/test_sd_09_328p/expect.txt new file mode 100644 index 0000000..6fc3f03 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_09_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 628*L->H, 628*H->L, Max High=10us Total High=3215us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=372 + +Time in EnableA max=225400 us, total=225400 us + +Time in StepA max=10 us, total=3215 us + diff --git a/extras/tests/simavr_based/test_sd_09_328p/platformio.ini b/extras/tests/simavr_based/test_sd_09_328p/platformio.ini new file mode 100644 index 0000000..0dae49a --- /dev/null +++ b/extras/tests/simavr_based/test_sd_09_328p/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 SIM_TEST_INPUT='"M1 A1000000 V40 R500 W A100 b w1000 S ? W "' + +[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_10_328p/expect.txt b/extras/tests/simavr_based/test_sd_10_328p/expect.txt new file mode 100644 index 0000000..040351b --- /dev/null +++ b/extras/tests/simavr_based/test_sd_10_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 120000*L->H, 120000*H->L, Max High=13us Total High=618850us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=120000 + +Time in EnableA max=225397 us, total=225397 us + +Time in StepA max=13 us, total=618850 us + diff --git a/extras/tests/simavr_based/test_sd_10_328p/platformio.ini b/extras/tests/simavr_based/test_sd_10_328p/platformio.ini new file mode 100644 index 0000000..c55e9cd --- /dev/null +++ b/extras/tests/simavr_based/test_sd_10_328p/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 SIM_TEST_INPUT='"M1 V40 A10000 P120000 A70000000 w5000 V50 U w1000 W "' + +[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_11_328p/expect.txt b/extras/tests/simavr_based/test_sd_11_328p/expect.txt new file mode 100644 index 0000000..919fc7b --- /dev/null +++ b/extras/tests/simavr_based/test_sd_11_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 1*L->H, 1*H->L + 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=11us Total High=385248us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=50536 + +Time in EnableA max=225395 us, total=225395 us + +Time in StepA max=11 us, total=385248 us + diff --git a/extras/tests/simavr_based/test_sd_11_328p/platformio.ini b/extras/tests/simavr_based/test_sd_11_328p/platformio.ini new file mode 100644 index 0000000..3e1f5f0 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_11_328p/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 SIM_TEST_INPUT='"M1 V40 a1000000 w1000 b w1000 f w2000 S W "' + +[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_12_328p/expect.txt b/extras/tests/simavr_based/test_sd_12_328p/expect.txt new file mode 100644 index 0000000..52f7843 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_12_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 31255*L->H, 31255*H->L, Max High=11us Total High=121197us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=31255 + +Time in EnableA max=225397 us, total=225397 us + +Time in StepA max=11 us, total=121197 us + diff --git a/extras/tests/simavr_based/test_sd_12_328p/platformio.ini b/extras/tests/simavr_based/test_sd_12_328p/platformio.ini new file mode 100644 index 0000000..c605cbf --- /dev/null +++ b/extras/tests/simavr_based/test_sd_12_328p/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 SIM_TEST_INPUT='"M1 H25000 a1000000 w100 A100 H1000 U w1000 A100000 U S ? W "' + +[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_13_328p/expect.txt b/extras/tests/simavr_based/test_sd_13_328p/expect.txt new file mode 100644 index 0000000..06b6612 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_13_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 3200*L->H, 3200*H->L, Max High=12us Total High=16501us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=3200 + +Time in EnableA max=225396 us, total=225396 us + +Time in StepA max=12 us, total=16501 us + diff --git a/extras/tests/simavr_based/test_sd_13_328p/platformio.ini b/extras/tests/simavr_based/test_sd_13_328p/platformio.ini new file mode 100644 index 0000000..ea14fca --- /dev/null +++ b/extras/tests/simavr_based/test_sd_13_328p/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 SIM_TEST_INPUT='"M1 V300 A10000 R3200 ? W "' + +[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_14_328p/expect.txt b/extras/tests/simavr_based/test_sd_14_328p/expect.txt new file mode 100644 index 0000000..be67ebc --- /dev/null +++ b/extras/tests/simavr_based/test_sd_14_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 81682*L->H, 81682*H->L, Max High=11us Total High=313229us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=81682 + +Time in EnableA max=229492 us, total=229492 us + +Time in StepA max=11 us, total=313229 us + diff --git a/extras/tests/simavr_based/test_sd_14_328p/platformio.ini b/extras/tests/simavr_based/test_sd_14_328p/platformio.ini new file mode 100644 index 0000000..e34e192 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_14_328p/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 SIM_TEST_INPUT='"M1 u A1000000 V40 f w2000 a-10000 W "' + +[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_15_328p/expect.txt b/extras/tests/simavr_based/test_sd_15_328p/expect.txt new file mode 100644 index 0000000..ecfe230 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_15_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 1*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + 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]=-1033 + +Time in EnableA max=225396 us, total=225396 us + +Time in StepA max=12 us, total=17723 us + diff --git a/extras/tests/simavr_based/test_sd_15_328p/platformio.ini b/extras/tests/simavr_based/test_sd_15_328p/platformio.ini new file mode 100644 index 0000000..81b5b41 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_15_328p/platformio.ini @@ -0,0 +1,40 @@ +; 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 SIM_TEST_INPUT='"M1 A10000 V400 b w1000 X w100 P1200 W "' + +#works +#build_flags = -D SIM_TEST_INPUT='"M1 A10000 V400 b w1000 X w100 P1200 w2000 W "' + +#fails +build_flags = -D SIM_TEST_INPUT='"M1 A10000 V400 b w1000 X w1000 P1200 w2000 W "' + +#fails +#build_flags = -D SIM_TEST_INPUT='"M1 A10000 V400 b w1000 X w100 P1200 ? W "' + +[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_16_328p/expect.txt b/extras/tests/simavr_based/test_sd_16_328p/expect.txt new file mode 100644 index 0000000..dae7d2d --- /dev/null +++ b/extras/tests/simavr_based/test_sd_16_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 1124*L->H, 1124*H->L, Max High=9us Total High=4326us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=1124 + +Time in EnableA max=225397 us, total=225397 us + +Time in StepA max=9 us, total=4326 us + diff --git a/extras/tests/simavr_based/test_sd_16_328p/platformio.ini b/extras/tests/simavr_based/test_sd_16_328p/platformio.ini new file mode 100644 index 0000000..cb9ea0a --- /dev/null +++ b/extras/tests/simavr_based/test_sd_16_328p/platformio.ini @@ -0,0 +1,34 @@ +; 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 SIM_TEST_INPUT='"M1 A10000 V1000 f w100 A100 V100 U w1000 X W "' + +# Expectation is, that after the first wait the speed is 1000 steps/s +# After the second wait, the speed should have increased to 1100 steps/s + +[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_17_328p/expect.txt b/extras/tests/simavr_based/test_sd_17_328p/expect.txt new file mode 100644 index 0000000..fe966dc --- /dev/null +++ b/extras/tests/simavr_based/test_sd_17_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + 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=10us Total High=5503us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=1431 + +Time in EnableA max=229527 us, total=229527 us + +Time in StepA max=10 us, total=5503 us + diff --git a/extras/tests/simavr_based/test_sd_17_328p/platformio.ini b/extras/tests/simavr_based/test_sd_17_328p/platformio.ini new file mode 100644 index 0000000..8bf7fdb --- /dev/null +++ b/extras/tests/simavr_based/test_sd_17_328p/platformio.ini @@ -0,0 +1,34 @@ +; 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 SIM_TEST_INPUT='"M1 A10 V100 j100000 f w1000 A100000 U X W "' + +# Expectation is, that after the first wait the speed is 1000 steps/s +# After the second wait, the speed should have increased to 1100 steps/s + +[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_18_328p/expect.txt b/extras/tests/simavr_based/test_sd_18_328p/expect.txt new file mode 100644 index 0000000..075c581 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_18_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 3864*L->H, 3864*H->L, Max High=11us Total High=14834us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in EnableA max=229524 us, total=229524 us + +Time in StepA max=11 us, total=14834 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 new file mode 100644 index 0000000..f672387 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_18_328p/platformio.ini @@ -0,0 +1,35 @@ +; 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 SIM_TEST_INPUT='"Q M1 A10 V100 j100000 f w1000 A100000 U X w100 R-1431 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. +# This value should be adjusted, if the SW runtime behavior changes + +[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_19_328p/expect.txt b/extras/tests/simavr_based/test_sd_19_328p/expect.txt new file mode 100644 index 0000000..1a38ed4 --- /dev/null +++ b/extras/tests/simavr_based/test_sd_19_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 0*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 1010*L->H, 1010*H->L, Max High=11us Total High=5190us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=1010 + +Time in EnableA max=229497 us, total=229497 us + +Time in StepA max=11 us, total=5190 us + diff --git a/extras/tests/simavr_based/test_sd_19_328p/platformio.ini b/extras/tests/simavr_based/test_sd_19_328p/platformio.ini new file mode 100644 index 0000000..cdc162e --- /dev/null +++ b/extras/tests/simavr_based/test_sd_19_328p/platformio.ini @@ -0,0 +1,35 @@ +; 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 SIM_TEST_INPUT='"Q M1 A1000 V100 P1000 W X w100 R10 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. +# This value should be adjusted, if the SW runtime behavior changes + +[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_seq_sd_01_328p/expect.txt b/extras/tests/simavr_based/test_seq_sd_01_328p/expect.txt new file mode 100644 index 0000000..63ef433 --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_01_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 6400*L->H, 6400*H->L, Max High=12us Total High=32942us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in EnableA max=1237094 us, total=1237094 us + +Time in StepA max=12 us, total=32942 us + diff --git a/extras/tests/simavr_based/test_seq_sd_01_328p/platformio.ini b/extras/tests/simavr_based/test_seq_sd_01_328p/platformio.ini new file mode 100644 index 0000000..189317e --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_01_328p/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 SIM_TEST_INPUT='"t M1 01 R W ? "' + +[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_seq_sd_02_328p/expect.txt b/extras/tests/simavr_based/test_seq_sd_02_328p/expect.txt new file mode 100644 index 0000000..2a61ea5 --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_02_328p/expect.txt @@ -0,0 +1,14 @@ + DirA: 33*L->H, 34*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 65988*L->H, 65988*H->L, Max High=11us Total High=338042us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in DirA max=5111808 us, total=45805592 us + +Time in EnableA max=237683 us, total=237683 us + +Time in StepA max=11 us, total=338042 us + diff --git a/extras/tests/simavr_based/test_seq_sd_02_328p/platformio.ini b/extras/tests/simavr_based/test_seq_sd_02_328p/platformio.ini new file mode 100644 index 0000000..74713c7 --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_02_328p/platformio.ini @@ -0,0 +1,19 @@ +; 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] + +[env:atmega328p] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall -D SIM_TEST_INPUT='"t M1 02 I R W "' -DSIMAVR_FOC_WORKAROUND +lib_extra_dirs = ../../../../.. + diff --git a/extras/tests/simavr_based/test_seq_sd_06_328p/expect.txt b/extras/tests/simavr_based/test_seq_sd_06_328p/expect.txt new file mode 100644 index 0000000..493962d --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_06_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 64000*L->H, 64000*H->L, Max High=11us Total High=327315us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in EnableA max=237677 us, total=237677 us + +Time in StepA max=11 us, total=327315 us + diff --git a/extras/tests/simavr_based/test_seq_sd_06_328p/platformio.ini b/extras/tests/simavr_based/test_seq_sd_06_328p/platformio.ini new file mode 100644 index 0000000..d9e29b6 --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_06_328p/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 SIM_TEST_INPUT='"t M1 06 I R W "' + +[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_seq_sd_07_328p/expect.txt b/extras/tests/simavr_based/test_seq_sd_07_328p/expect.txt new file mode 100644 index 0000000..4790351 --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_07_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 1*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 103230*L->H, 103230*H->L, Max High=9us Total High=393675us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in EnableA max=237680 us, total=237680 us + +Time in StepA max=9 us, total=393675 us + diff --git a/extras/tests/simavr_based/test_seq_sd_07_328p/platformio.ini b/extras/tests/simavr_based/test_seq_sd_07_328p/platformio.ini new file mode 100644 index 0000000..7d95816 --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_07_328p/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 SIM_TEST_INPUT='"t M1 07 I R W "' + +[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_seq_sd_11_328p/expect.txt b/extras/tests/simavr_based/test_seq_sd_11_328p/expect.txt new file mode 100644 index 0000000..ca6e6dc --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_11_328p/expect.txt @@ -0,0 +1,12 @@ + DirA: 0*L->H, 1*H->L + DirB: 1*L->H, 0*H->L + EnableA: 1*L->H, 1*H->L + EnableB: 1*L->H, 0*H->L + StepA: 2000000*L->H, 2000000*H->L, Max High=13us Total High=10642797us + StepB: 0*L->H, 0*H->L, Max High=0us Total High=0us +Position[A]=0 + +Time in EnableA max=237678 us, total=237678 us + +Time in StepA max=13 us, total=10642797 us + diff --git a/extras/tests/simavr_based/test_seq_sd_11_328p/platformio.ini b/extras/tests/simavr_based/test_seq_sd_11_328p/platformio.ini new file mode 100644 index 0000000..5b8f7b3 --- /dev/null +++ b/extras/tests/simavr_based/test_seq_sd_11_328p/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 SIM_TEST_INPUT='"t M1 11 I R W "' + +[env:atmega328p] +platform = atmelavr +board = nanoatmega328 +framework = arduino +build_flags = -Werror -Wall ${common.build_flags} +lib_extra_dirs = ../../../../.. + diff --git a/keywords.txt b/keywords.txt new file mode 100644 index 0000000..ea93ace --- /dev/null +++ b/keywords.txt @@ -0,0 +1,24 @@ +####################################### +# Syntax Coloring Map For AccelStepper +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +FastAccelStepper KEYWORD1 +Speed KEYWORD1 +Acceleration KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +disableOutputs KEYWORD2 +enableOutputs KEYWORD2 +setEnablePin KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/shell.nix b/shell.nix new file mode 100644 index 0000000..042377b --- /dev/null +++ b/shell.nix @@ -0,0 +1,4 @@ +{ pkgs ? import {} }: +pkgs.mkShell { + nativeBuildInputs = with pkgs; [ picocom grabserial clang wxmaxima gcc libelf ]; +} diff --git a/src/AVRStepperPins.h b/src/AVRStepperPins.h new file mode 100644 index 0000000..4c32b27 --- /dev/null +++ b/src/AVRStepperPins.h @@ -0,0 +1,80 @@ +#ifndef AVRSTEPPERPINS_H +#define AVRSTEPPERPINS_H + +#if defined(ARDUINO_ARCH_AVR) +#include + +/** * Warning: Other libraries may also use the timers! + * + * For example Serial library and delay() functions for example. + * Using the same timer may cause strange effects, you are best to avoid using + * those other libraries at the some time or use a different pin where + * possible! + */ + +#if !(defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || \ + defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) || \ + defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__)) +#error "Unsupported AVR derivate" +#endif + +// The ATmega328P has one 16 bit timer: Timer 1 +// The ATmega2560 has four 16 bit timers: Timer 1, 3, 4 and 5 +#if (defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || \ + defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__)) +#define FAS_TIMER_MODULE 1 +#define stepPinStepper1A 9 /* OC1A */ +#define stepPinStepper1B 10 /* OC1B */ +#elif defined(__AVR_ATmega2560__) +#ifndef FAS_TIMER_MODULE +#define FAS_TIMER_MODULE 4 +#endif +#define stepPinStepper1A 11 /* OC1A */ +#define stepPinStepper1B 12 /* OC1B */ +#define stepPinStepper1C 13 /* OC1C */ +#define stepPinStepper3A 5 /* OC3A */ +#define stepPinStepper3B 2 /* OC3B */ +#define stepPinStepper3C 3 /* OC3C */ +#define stepPinStepper4A 6 /* OC4A */ +#define stepPinStepper4B 7 /* OC4B */ +#define stepPinStepper4C 8 /* OC4C */ +#define stepPinStepper5A 46 /* OC5A */ +#define stepPinStepper5B 45 /* OC5B */ +#define stepPinStepper5C 44 /* OC5C */ + +#elif defined(__AVR_ATmega32U4__) +#define FAS_TIMER_MODULE 1 +#define stepPinStepper1A 9 /* OC1A */ +#define stepPinStepper1B 10 /* OC1B */ +#define stepPinStepper1C 11 /* OC1C */ +#endif + +#if (FAS_TIMER_MODULE == 1) +#define stepPinStepperA stepPinStepper1A +#define stepPinStepperB stepPinStepper1B +#if defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__) +#define stepPinStepperC stepPinStepper1C +#endif +#elif (FAS_TIMER_MODULE == 3) +#define stepPinStepperA stepPinStepper3A +#define stepPinStepperB stepPinStepper3B +#if defined(__AVR_ATmega2560__) +#define stepPinStepperC stepPinStepper3C +#endif +#elif (FAS_TIMER_MODULE == 4) +#define stepPinStepperA stepPinStepper4A +#define stepPinStepperB stepPinStepper4B +#if defined(__AVR_ATmega2560__) +#define stepPinStepperC stepPinStepper4C +#endif +#elif (FAS_TIMER_MODULE == 5) +#define stepPinStepperA stepPinStepper5A +#define stepPinStepperB stepPinStepper5B +#if defined(__AVR_ATmega2560__) +#define stepPinStepperC stepPinStepper5C +#endif +#endif + +#endif // ARDUINO_ARCH_AVR + +#endif // AVRSTEPPERPINS_H diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp new file mode 100644 index 0000000..341b907 --- /dev/null +++ b/src/FastAccelStepper.cpp @@ -0,0 +1,906 @@ +#include "FastAccelStepper.h" +#include "StepperISR.h" + +// This define in order to not shoot myself. +#ifndef TEST +#define printf DO_NOT_USE_PRINTF +#define puts DO_NOT_USE_PUTS +#endif + +// Here are the global variables to interface with the interrupts + +// To realize the 1 Hz debug led +static uint8_t fas_ledPin = PIN_UNDEFINED; +static uint16_t fas_debug_led_cnt = 0; + +// dynamic allocation seems to not work so well on avr +FastAccelStepper fas_stepper[MAX_STEPPER]; + +//************************************************************************************************* +//************************************************************************************************* +void FastAccelStepperEngine::init() { + _externalCallForPin = nullptr; + _stepper_cnt = 0; + fas_init_engine(this, 255); + for (auto & i : _stepper) { + i = nullptr; + } +} + +#if defined(SUPPORT_CPU_AFFINITY) +void FastAccelStepperEngine::init(uint8_t cpu_core) { + _externalCallForPin = nullptr; + _stepper_cnt = 0; + fas_init_engine(this, cpu_core); +} +#endif +void FastAccelStepperEngine::setExternalCallForPin( + bool (*func)(uint8_t pin, uint8_t value)) { + _externalCallForPin = func; +} +//************************************************************************************************* +bool FastAccelStepperEngine::_isValidStepPin(uint8_t step_pin) { + return StepperQueue::isValidStepPin(step_pin); +} +//************************************************************************************************* +bool FastAccelStepperEngine::isDirPinBusy(uint8_t dir_pin, + uint8_t except_stepper) { + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + if (i != except_stepper) { + FastAccelStepper* s = _stepper[i]; + if (s) { + if (s->getDirectionPin() == dir_pin) { + if (s->isQueueRunning()) { + return true; + } + } + } + } + } + return false; +} +//************************************************************************************************* +#if !defined(SUPPORT_SELECT_DRIVER_TYPE) +FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin(uint8_t step_pin) +#else +FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( + uint8_t step_pin, uint8_t driver_type) +#endif +{ + // Check if already connected + for (auto s : _stepper) { + if (s) { + if (s->getStepPin() == step_pin) { + return nullptr; + } + } + } + if (!_isValidStepPin(step_pin)) { + return nullptr; + } +#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) { + return nullptr; + } + fas_stepper_num = _stepper_cnt; + } +#else + uint8_t queue_from = 0; + uint8_t queue_to = QUEUES_MCPWM_PCNT + QUEUES_RMT; + if (driver_type == DRIVER_MCPWM_PCNT) { + queue_to = QUEUES_MCPWM_PCNT; + } else if (driver_type == DRIVER_RMT) { + queue_from = QUEUES_MCPWM_PCNT; + } + int8_t fas_stepper_num = -1; + for (uint8_t i = queue_from; i < queue_to; i++) { + FastAccelStepper* s = _stepper[i]; + if (s == NULL) { + fas_stepper_num = i; + break; + } + } + if (fas_stepper_num < 0) { + return NULL; + } +#endif + _stepper_cnt++; + + FastAccelStepper* s = &fas_stepper[fas_stepper_num]; + _stepper[fas_stepper_num] = s; + s->init(this, fas_stepper_num, step_pin); + for (auto sx : _stepper) { + if (sx) { + fas_queue[sx->_queue_num].adjustSpeedToStepperCount(_stepper_cnt); + } + } + return s; +} +//************************************************************************************************* +void FastAccelStepperEngine::setDebugLed(uint8_t ledPin) { + fas_ledPin = ledPin; + PIN_OUTPUT(fas_ledPin, LOW); +} +//************************************************************************************************* +void FastAccelStepperEngine::manageSteppers() { +#ifdef DEBUG_LED_HALF_PERIOD + if (fas_ledPin != PIN_UNDEFINED) { + fas_debug_led_cnt++; + if (fas_debug_led_cnt == DEBUG_LED_HALF_PERIOD) { + digitalWrite(fas_ledPin, HIGH); + } + if (fas_debug_led_cnt == 2 * DEBUG_LED_HALF_PERIOD) { + digitalWrite(fas_ledPin, LOW); + fas_debug_led_cnt = 0; + } + } +#endif + for (auto s : _stepper) { + if (s) { +#ifdef SUPPORT_EXTERNAL_DIRECTION_PIN + if (s->externalDirPinChangeCompletedIfNeeded()) { + s->fill_queue(); + } +#else + s->fill_queue(); +#endif + } + } + + // Check for auto disable + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + FastAccelStepper* s = _stepper[i]; + if (s) { + if (s->needAutoDisable()) { + uint8_t high_active_pin = s->getEnablePinHighActive(); + uint8_t low_active_pin = s->getEnablePinLowActive(); + + // fasDisableInterrupts(); // TODO + bool agree = true; + for (uint8_t j = 0; j < MAX_STEPPER; j++) { + if (i != j) { + FastAccelStepper* other = _stepper[j]; + if (other) { + if (other->usesAutoEnablePin(high_active_pin) || + other->usesAutoEnablePin(low_active_pin)) { + if (!other->agreeWithAutoDisable()) { + agree = false; + break; + } + } + } + } + } + if (agree) { + for (auto current : _stepper) { + if (current) { + if (current->usesAutoEnablePin(high_active_pin) || + current->usesAutoEnablePin(low_active_pin)) { + // if successful, then the _auto_disable_delay_counter is zero + // Otherwise in next loop will be checked for auto disable again + current->disableOutputs(); + } + } + } + } + // fasEnableInterrupts(); + } + } + } + + // Update the auto disable counters + for (auto s : _stepper) { + if (s) { + fasDisableInterrupts(); + // update the counters down to 1 + s->updateAutoDisable(); + fasEnableInterrupts(); + } + } +} + +//************************************************************************************************* +//************************************************************************************************* +// +// FastAccelStepper provides: +// - movement control +// either raw access to the stepper command queue +// or ramp generator driven by speed/acceleration and move +// - stepper position +// +// This implements auto enable and delay from direction change to first step +// +//************************************************************************************************* +//************************************************************************************************* + +//************************************************************************************************* +int8_t FastAccelStepper::addQueueEntry(const struct stepper_command_s* cmd, + bool start) { + StepperQueue* q = &fas_queue[_queue_num]; + if (cmd == nullptr) { + return q->addQueueEntry(nullptr, start); + } + if (cmd->ticks < q->max_speed_in_ticks) { + return AQE_ERROR_TICKS_TOO_LOW; + } + + if (_dirPin != PIN_UNDEFINED) { + if (!isQueueRunning()) { + if (_engine != nullptr) { + if (_engine->isDirPinBusy(_dirPin, _queue_num)) { + return AQE_DIR_PIN_IS_BUSY; + } + } + } + } else { + if (!cmd->count_up) { + return AQE_ERROR_NO_DIR_PIN_TO_TOGGLE; + } + } + + int res = AQE_OK; + if (_autoEnable) { + fasDisableInterrupts(); + uint16_t delay_counter = _auto_disable_delay_counter; + fasEnableInterrupts(); + if (delay_counter == 0) { + // outputs are disabled + if (!enableOutputs()) { + return AQE_WAIT_FOR_ENABLE_PIN_ACTIVE; + } + // if on delay is defined, fill queue if required amount of pauses before + // the first step + if (_on_delay_ticks > 0) { + uint32_t delay = _on_delay_ticks; + // this delay sets count_up appropriately. If this is shorter than + // dir_change_delay_ticks, then extend accordingly + if ((delay < _dir_change_delay_ticks) && + (q->queue_end.count_up != cmd->count_up)) { + delay = _dir_change_delay_ticks; + } + while (delay > 0) { + uint32_t ticks = delay >> 1; + uint16_t ticks_u16 = ticks; + if (ticks > 65535) { + ticks_u16 = 65535; + } else if (ticks < 32768) { + ticks_u16 = delay; + } + struct stepper_command_s start_cmd = { + .ticks = ticks_u16, .steps = 0, .count_up = cmd->count_up}; + q->addQueueEntry(&start_cmd, false); + delay -= ticks_u16; + } + res = q->addQueueEntry(nullptr, start); + if (res != AQE_OK) { + return res; + } + } + } + } + if (q->queue_end.count_up != cmd->count_up) { + // Change of direction has been detected. + if (_dirPin & PIN_EXTERNAL_FLAG) { + // for external pins, two pause commands need to be added. The first one + // with the dir pin change. The second one just a pause. + // The queue's addQueueEntry() will set repeat_entry for the command entry + if (q->queueEntries() > QUEUE_LEN - 2) { + // no space for two commands => do nothing and return QUEUE_FULL + return AQE_QUEUE_FULL; + } + struct stepper_command_s start_cmd = { + .ticks = US_TO_TICKS(500), .steps = 0, .count_up = cmd->count_up}; + res = q->addQueueEntry(&start_cmd, start); + if (res != AQE_OK) { + return res; + } + res = q->addQueueEntry(&start_cmd, start); + if (res != AQE_OK) { + return res; + } + } else if ((_dir_change_delay_ticks != 0) && (cmd->steps != 0)) { + // add pause command to delay dir pin change to first step + struct stepper_command_s start_cmd = {.ticks = _dir_change_delay_ticks, + .steps = 0, + .count_up = cmd->count_up}; + res = q->addQueueEntry(&start_cmd, start); + if (res != AQE_OK) { + return res; + } + } + } + res = q->addQueueEntry(cmd, start); + if (_autoEnable) { + if (res == AQE_OK) { + fasDisableInterrupts(); + _auto_disable_delay_counter = _off_delay_count; + fasEnableInterrupts(); + } + } + + return res; +} + +#ifdef SUPPORT_EXTERNAL_DIRECTION_PIN +bool FastAccelStepper::externalDirPinChangeCompletedIfNeeded() { + StepperQueue* q = &fas_queue[_queue_num]; + if ((_dirPin != PIN_UNDEFINED) && ((_dirPin & PIN_EXTERNAL_FLAG) != 0)) { + if (q->isOnRepeatingEntry()) { + if (_engine->_externalCallForPin) { + uint8_t state = q->dirPinState(); + bool newState = _engine->_externalCallForPin(_dirPin, state); + if (newState != state) { + return false; + } + q->clearRepeatingFlag(); + } + } + } + return true; +} +#endif + +//************************************************************************************************* +// fill_queue generates commands to the stepper for executing a ramp +// +// Plan is to fill the queue with commmands summing up to approx. 10 ms in the +// future (or more). For low speeds, this results in single stepping For high +// speeds (40kSteps/s) approx. 400 Steps to be created using 3 commands +// +//************************************************************************************************* + +void FastAccelStepper::fill_queue() { + // Check preconditions to be allowed to fill the queue + if (!_rg.isRampGeneratorActive()) { + return; + } + if (!_rg.hasValidConfig()) { +#ifdef TEST + assert(false); +#endif + return; + } + // check if addition of commands is suspended (due to forceStopAndNewPosition) + StepperQueue* q = &fas_queue[_queue_num]; + // if force stop has been called, then ignore_commands is true and ramp + // stopped. So the ramp generator will not create a new command, unless new + // move command has been given after forceStop..(). So we just clear the flag + q->ignore_commands = false; + + // preconditions are fulfilled, so create the command(s) + NextCommand cmd; + // 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 < _forward_planning_in_ticks) || + (q->queueEntries() <= 1)) && + _rg.isRampGeneratorActive()) { +#if (TEST_MEASURE_ISR_SINGLE_FILL == 1) + // For run time measurement + uint32_t runtime_us = micros(); +#endif + int8_t res = AQE_OK; + _rg.getNextCommand(&q->queue_end, &cmd); + if (cmd.command.ticks != 0) { + res = addQueueEntry(&cmd.command, !delayed_start); + } + if (res == AQE_OK) { + _rg.afterCommandEnqueued(&cmd); + need_delayed_start = delayed_start; + if (cmd.command.steps <= 1) { + ticksPrepared += cmd.command.ticks; + } else { + uint32_t tmp = cmd.command.ticks; + tmp *= cmd.command.steps; + ticksPrepared += tmp; + } + } + +#if (TEST_MEASURE_ISR_SINGLE_FILL == 1) + // For run time measurement + runtime_us = micros() - runtime_us; + max_micros = fas_max(max_micros, runtime_us); +#endif + if (cmd.command.ticks == 0) { + break; + } + if (res != AQE_OK) { + if (res > 0) { + // try later again + break; + } else { +#ifdef SIM_TEST_INPUT + Serial.println("Abort ramp due to queue error res="); + Serial.print(res); + Serial.print(" Steps="); + Serial.print(cmd.command.steps); + Serial.print(" ticks="); + Serial.print(cmd.command.ticks); + Serial.print(" min_cmd_ticks="); + Serial.println(MIN_CMD_TICKS); +#endif +#ifdef TEST + printf("ERROR: Abort ramp due to queue error (%d)\n", res); + printf("steps=%d ticks=%d limit=%ld state=%d\n", cmd.command.steps, + cmd.command.ticks, MIN_CMD_TICKS, cmd.rw.ramp_state); + assert(false); +#endif + _rg.stopRamp(); + delayed_start = false; + } + } + } + if (need_delayed_start) { + addQueueEntry(nullptr, true); + } +} + +void FastAccelStepper::updateAutoDisable() { + // FastAccelStepperEngine will call with interrupts disabled + // fasDisableInterrupts(); + if (_auto_disable_delay_counter > 1) { + if (!isRunning()) { + _auto_disable_delay_counter--; + } + } + // fasEnableInterrupts(); +} + +bool FastAccelStepper::agreeWithAutoDisable() { + bool agree = true; + // FastAccelStepperEngine will call with interrupts disabled + // fasDisableInterrupts(); + if (isRunning()) { + agree = false; + } + if (_auto_disable_delay_counter > 1) { + agree = false; + } + // fasEnableInterrupts(); + return agree; +} + +bool FastAccelStepper::needAutoDisable() { + bool need_disable = false; + // FastAccelStepperEngine will call with interrupts disabled + // fasDisableInterrupts(); + if (_auto_disable_delay_counter == 1) { + if (!isRunning()) { + need_disable = true; + } + } + // fasEnableInterrupts(); + return need_disable; +} + +bool FastAccelStepper::usesAutoEnablePin(uint8_t pin) const { + if (pin != PIN_UNDEFINED) { + if ((pin == _enablePinHighActive) || (pin == _enablePinLowActive)) { + return true; + } + } + return false; +} + +void FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num, + uint8_t step_pin) { +#if (TEST_MEASURE_ISR_SINGLE_FILL == 1) + // For run time measurement + max_micros = 0; +#endif + _engine = engine; + _autoEnable = false; + _dir_change_delay_ticks = 0; + _on_delay_ticks = 0; + _off_delay_count = 1; + _auto_disable_delay_counter = 0; + _stepPin = step_pin; + _dirHighCountsUp = true; + _dirPin = PIN_UNDEFINED; + _enablePinHighActive = PIN_UNDEFINED; + _enablePinLowActive = PIN_UNDEFINED; + _forward_planning_in_ticks = TICKS_PER_S / 50; + _rg.init(); + + _queue_num = num; + fas_queue[_queue_num].init(_queue_num, step_pin); +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5) + _attached_pulse_unit = nullptr; +#endif +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4) + _attached_pulse_cnt_unit = -1; +#endif +} +uint8_t FastAccelStepper::getStepPin() const { return _stepPin; } +void FastAccelStepper::setDirectionPin(uint8_t dirPin, bool dirHighCountsUp, + uint16_t dir_change_delay_us) { + _dirPin = dirPin; + _dirHighCountsUp = dirHighCountsUp; + if (_dirPin != PIN_UNDEFINED) { + if (_dirPin & PIN_EXTERNAL_FLAG) { + if (_engine->_externalCallForPin) { + _engine->_externalCallForPin(_dirPin, dirHighCountsUp ? HIGH : LOW); + } + } else { + PIN_OUTPUT(dirPin, dirHighCountsUp ? HIGH : LOW); + } + } + fas_queue[_queue_num].setDirPin(dirPin, dirHighCountsUp); + if (dir_change_delay_us != 0) { + if (dir_change_delay_us > MAX_DIR_DELAY_US) { + dir_change_delay_us = MAX_DIR_DELAY_US; + } + if (dir_change_delay_us < MIN_DIR_DELAY_US) { + dir_change_delay_us = MIN_DIR_DELAY_US; + } + _dir_change_delay_ticks = US_TO_TICKS(dir_change_delay_us); + } else { + _dir_change_delay_ticks = 0; + } +} +void FastAccelStepper::setEnablePin(uint8_t enablePin, + bool low_active_enables_stepper) { + if (low_active_enables_stepper) { + _enablePinLowActive = enablePin; + if (enablePin != PIN_UNDEFINED) { + if (enablePin & PIN_EXTERNAL_FLAG) { + if (_engine->_externalCallForPin) { + _engine->_externalCallForPin(enablePin, HIGH); + } + } else { + PIN_OUTPUT(enablePin, HIGH); + if (_enablePinHighActive == enablePin) { + _enablePinHighActive = PIN_UNDEFINED; + } + } + } + } else { + _enablePinHighActive = enablePin; + if (enablePin != PIN_UNDEFINED) { + if (enablePin & PIN_EXTERNAL_FLAG) { + if (_engine->_externalCallForPin) { + _engine->_externalCallForPin(enablePin, LOW); + } + } else { + PIN_OUTPUT(enablePin, LOW); + if (_enablePinLowActive == enablePin) { + _enablePinLowActive = PIN_UNDEFINED; + } + } + } + } +} +void FastAccelStepper::setAutoEnable(bool auto_enable) { + _autoEnable = auto_enable; + if (auto_enable && (_off_delay_count == 0)) { + _off_delay_count = 1; + } +} +int8_t FastAccelStepper::setDelayToEnable(uint32_t delay_us) { + uint32_t delay_ticks = US_TO_TICKS(delay_us); + if (delay_ticks > 0) { + if (delay_ticks < MIN_CMD_TICKS) { + return DELAY_TOO_LOW; + } + } + if (delay_ticks > MAX_ON_DELAY_TICKS) { + return DELAY_TOO_HIGH; + } + _on_delay_ticks = delay_ticks; + return DELAY_OK; +} +void FastAccelStepper::setDelayToDisable(uint16_t delay_ms) { + uint16_t delay_count = delay_ms / DELAY_MS_BASE; + if ((delay_ms > 0) && (delay_count < 2)) { + // ensure minimum time + delay_count = 2; + } + _off_delay_count = fas_max(delay_count, (uint16_t)1); +} +int8_t FastAccelStepper::runForward() { return _rg.startRun(true); } +int8_t FastAccelStepper::runBackward() { return _rg.startRun(false); } +int8_t FastAccelStepper::moveTo(int32_t position, bool blocking) { + int8_t res = _rg.moveTo(position, &fas_queue[_queue_num].queue_end); + if ((res == MOVE_OK) && blocking) { + while (isRunning()) { + noop_or_wait; + } + } + return res; +} +int8_t FastAccelStepper::move(int32_t move, bool blocking) { + if ((move < 0) && (_dirPin == PIN_UNDEFINED)) { + return MOVE_ERR_NO_DIRECTION_PIN; + } + int8_t res = _rg.move(move, &fas_queue[_queue_num].queue_end); + if ((res == MOVE_OK) && blocking) { + while (isRunning()) { + noop_or_wait; + } + } + return res; +} +void FastAccelStepper::keepRunning() { _rg.setKeepRunning(); } +void FastAccelStepper::stopMove() { _rg.initiateStop(); } +void FastAccelStepper::applySpeedAcceleration() { + _rg.applySpeedAcceleration(); +} +int8_t FastAccelStepper::moveByAcceleration(int32_t acceleration, + bool allow_reverse) { + int8_t res = MOVE_OK; + if (acceleration > 0) { + setAcceleration(acceleration); + res = runForward(); + } else if (acceleration < 0) { + setAcceleration(-acceleration); + if (allow_reverse && (_dirPin != PIN_UNDEFINED)) { + res = runBackward(); + } else { + applySpeedAcceleration(); + stopMove(); + } + } else { + uint32_t max_speed = _rg.getSpeedInTicks(); + setSpeedInTicks(getPeriodInTicksAfterCommandsCompleted()); + setAcceleration(1); // ensure increase, so the speed is kept + applySpeedAcceleration(); + setSpeedInTicks(max_speed); + } + return res; +} +void FastAccelStepper::forceStop() { + StepperQueue* q = &fas_queue[_queue_num]; + + // ensure no more commands are added to the queue + q->ignore_commands = true; + + // inform ramp generator to force stop + _rg.forceStop(); +} +void FastAccelStepper::forceStopAndNewPosition(int32_t new_pos) { + StepperQueue* q = &fas_queue[_queue_num]; + + // ensure no more commands are added to the queue + q->ignore_commands = true; + + // stop ramp generator + _rg.stopRamp(); + + // stop the stepper interrupt and empty the queue + q->forceStop(); + + // set the new position. This should be safe + q->queue_end.pos = new_pos; + _rg.setTargetPosition(new_pos); +} +bool FastAccelStepper::disableOutputs() { + if (isRunning() && _autoEnable) { + return false; + } + bool disabled = true; + if (_enablePinLowActive != PIN_UNDEFINED) { + if (_enablePinLowActive & PIN_EXTERNAL_FLAG) { + if (_engine->_externalCallForPin != nullptr) { + disabled &= + (_engine->_externalCallForPin(_enablePinLowActive, HIGH) == HIGH); + } + } else { + digitalWrite(_enablePinLowActive, HIGH); + } + } + if (_enablePinHighActive != PIN_UNDEFINED) { + if (_enablePinHighActive & PIN_EXTERNAL_FLAG) { + if (_engine->_externalCallForPin != nullptr) { + disabled &= + (_engine->_externalCallForPin(_enablePinHighActive, LOW) == LOW); + } + } else { + digitalWrite(_enablePinHighActive, LOW); + } + } + if (disabled) { + _auto_disable_delay_counter = 0; + } + return disabled; +} +bool FastAccelStepper::enableOutputs() { + bool enabled = true; + if (_enablePinLowActive != PIN_UNDEFINED) { + if (_enablePinLowActive & PIN_EXTERNAL_FLAG) { + if (_engine->_externalCallForPin != nullptr) { + enabled &= + (_engine->_externalCallForPin(_enablePinLowActive, LOW) == LOW); + } + } else { + digitalWrite(_enablePinLowActive, LOW); + } + } + if (_enablePinHighActive != PIN_UNDEFINED) { + if (_enablePinHighActive & PIN_EXTERNAL_FLAG) { + if (_engine->_externalCallForPin != nullptr) { + enabled &= + (_engine->_externalCallForPin(_enablePinHighActive, HIGH) == HIGH); + } + } else { + digitalWrite(_enablePinHighActive, HIGH); + } + } + return enabled; +} +int32_t FastAccelStepper::getPositionAfterCommandsCompleted() const { + return fas_queue[_queue_num].queue_end.pos; +} +uint32_t FastAccelStepper::getPeriodInTicksAfterCommandsCompleted() { + if (_rg.isRampGeneratorActive()) { + return _rg.getCurrentPeriodInTicks(); + } + return 0; +} +uint32_t FastAccelStepper::getPeriodInUsAfterCommandsCompleted() { + if (_rg.isRampGeneratorActive()) { + return _rg.getCurrentPeriodInUs(); + } + return 0; +} +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); + } else { + speed->ticks = 0; + } + } +} +int32_t FastAccelStepper::getCurrentSpeedInUs(bool realtime) { + struct actual_ticks_s 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; +} +int32_t FastAccelStepper::getCurrentSpeedInMilliHz(bool realtime) { + struct actual_ticks_s 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; + } + return 0; +} +uint16_t FastAccelStepper::getMaxSpeedInTicks() const { + return fas_queue[_queue_num].getMaxSpeedInTicks(); +} +uint16_t FastAccelStepper::getMaxSpeedInUs() const { + uint16_t ticks = getMaxSpeedInTicks(); + uint16_t speed_in_us = ticks / (TICKS_PER_S / 1000000); + return speed_in_us; +} +uint32_t FastAccelStepper::getMaxSpeedInHz() const { + uint16_t ticks = getMaxSpeedInTicks(); + uint32_t speed_in_hz = TICKS_PER_S / ticks; + return speed_in_hz; +} +uint32_t FastAccelStepper::getMaxSpeedInMilliHz() const { + uint16_t ticks = getMaxSpeedInTicks(); + 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) const { + 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; + } + if (min_step_ticks == TICKS_FOR_STOPPED_MOTOR) { + return -1; + } + _rg.setSpeedInTicks(min_step_ticks); + return 0; +} +int8_t FastAccelStepper::setSpeedInUs(uint32_t min_step_us) { + if (min_step_us >= TICKS_TO_US(0xffffffff)) { + return -1; + } + uint32_t min_step_ticks = US_TO_TICKS(min_step_us); + return setSpeedInTicks(min_step_ticks); +} +int8_t FastAccelStepper::setSpeedInHz(uint32_t speed_hz) { + if (speed_hz == 0) { + return -1; + } + uint32_t ticks = _rg.divForHz(speed_hz); + return setSpeedInTicks(ticks); +} +int8_t FastAccelStepper::setSpeedInMilliHz(uint32_t speed_mhz) { + if (speed_mhz <= (1000LL * TICKS_PER_S / 0xffffffff + 1)) { + return -1; + } + uint32_t ticks = _rg.divForMilliHz(speed_mhz); + return setSpeedInTicks(ticks); +} +void FastAccelStepper::setCurrentPosition(int32_t new_pos) { + int32_t delta = new_pos - getCurrentPosition(); + if (delta != 0) { + struct queue_end_s* queue_end = &fas_queue[_queue_num].queue_end; + fasDisableInterrupts(); + queue_end->pos = queue_end->pos + delta; + _rg.advanceTargetPosition(delta, queue_end); + fasEnableInterrupts(); + } +} +void FastAccelStepper::setPositionAfterCommandsCompleted(int32_t new_pos) { + struct queue_end_s* queue_end = &fas_queue[_queue_num].queue_end; + fasDisableInterrupts(); + int32_t delta = new_pos - fas_queue[_queue_num].queue_end.pos; + queue_end->pos = new_pos; + if (delta != 0) { + _rg.advanceTargetPosition(delta, queue_end); + } + fasEnableInterrupts(); +} +uint8_t FastAccelStepper::queueEntries() const { + return fas_queue[_queue_num].queueEntries(); +} +uint32_t FastAccelStepper::ticksInQueue() { + return fas_queue[_queue_num].ticksInQueue(); +} +bool FastAccelStepper::hasTicksInQueue(uint32_t min_ticks) const { + return fas_queue[_queue_num].hasTicksInQueue(min_ticks); +} +bool FastAccelStepper::isQueueFull() const { + return fas_queue[_queue_num].isQueueFull(); +} +bool FastAccelStepper::isQueueEmpty() const { + return fas_queue[_queue_num].isQueueEmpty(); +} +bool FastAccelStepper::isQueueRunning() const { + return fas_queue[_queue_num].isRunning(); +} +bool FastAccelStepper::isRunning() { + StepperQueue* q = &fas_queue[_queue_num]; + return q->isRunning() || _rg.isRampGeneratorActive() || !isQueueEmpty(); +} +void FastAccelStepper::performOneStep(bool count_up, bool blocking) { + if (!isRunning()) { + if (count_up || (_dirPin != PIN_UNDEFINED)) { + struct stepper_command_s cmd = { + .ticks = MIN_CMD_TICKS, .steps = 1, .count_up = count_up}; + addQueueEntry(&cmd); + if (blocking) { + while (isRunning()) { + } + } + } + } +} +void FastAccelStepper::forwardStep(bool blocking) { + performOneStep(true, blocking); +} +void FastAccelStepper::backwardStep(bool blocking) { + performOneStep(false, blocking); +} +int32_t FastAccelStepper::getCurrentPosition() const { + return fas_queue[_queue_num].getCurrentPosition(); +} +void FastAccelStepper::detachFromPin() const { fas_queue[_queue_num].disconnect(); } +void FastAccelStepper::reAttachToPin() const { fas_queue[_queue_num].connect(); } diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h new file mode 100644 index 0000000..cef3110 --- /dev/null +++ b/src/FastAccelStepper.h @@ -0,0 +1,770 @@ +#ifndef FASTACCELSTEPPER_H +#define FASTACCELSTEPPER_H +#include +#include "PoorManFloat.h" +#include "fas_arch/common.h" + +// # FastAccelStepper +// +// FastAccelStepper is a high speed alternative for the +// [AccelStepper library](http://www.airspayce.com/mikem/arduino/AccelStepper/). +// Supported are avr (ATmega 168/328/P, ATmega2560), esp32 and atmelsam due. +// +// Here is a basic example to run a stepper from position 0 to 1000 and back +// again to 0. +// ``` +// #include +// +// FastAccelStepperEngine engine = FastAccelStepperEngine(); +// FastAccelStepper *stepper = NULL; +// +// #define dirPinStepper 5 +// #define enablePinStepper 6 +// #define stepPinStepper 9 +// void setup() { +// engine.init(); +// stepper = engine.stepperConnectToPin(stepPinStepper); +// if (stepper) { +// stepper->setDirectionPin(dirPinStepper); +// stepper->setEnablePin(enablePinStepper); +// stepper->setAutoEnable(true); +// +// stepper->setSpeedInHz(500); +// stepper->setAcceleration(100); +// stepper->moveTo(1000, true); +// stepper->moveTo(0, true); +// } +// } +// +// void loop() {} +// ``` + +class FastAccelStepper; + +class FastAccelStepperEngine { + // + // ## FastAccelStepperEngine + // + // This engine - actually a factory - provides you with instances of steppers. + + public: + // ### Initialization + // + // The FastAccelStepperEngine is declared with FastAccelStepperEngine(). + // This is to occupy the needed memory. + // ```cpp + // FastAccelStepperEngine engine = FastAccelStepperEngine(); + // ``` + // But it still needs to be initialized. + // For this init shall be used: + // ```cpp + // void setup() { + // engine.init(); + // } + // ``` + + void init(); + +#if defined(SUPPORT_CPU_AFFINITY) + // In a multitasking and multicore system like ESP32, the steppers are + // controlled by a continuously running task. This task can be fixed to one + // CPU core with this modified init()-call. ESP32 implementation detail: For + // values 0 and 1, xTaskCreatePinnedToCore() is used, or else xTaskCreate() + void init(uint8_t cpu_core); +#endif + + // ### Creation of FastAccelStepper + // + // Using a call to `stepperConnectToPin()` a FastAccelStepper instance is + // created. This call tells the stepper, which step pin to use. As the + // 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 +#if defined(SUPPORT_SELECT_DRIVER_TYPE) +#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); +#endif + + // Comments to valid pins: + // + // clang-format off + // | Device | Comment | + // |:----------------|:--------------------------------------------------------------------------------------------------| + // | ESP32   | Every output capable GPIO can be used | + // | ESP32S2   | Every output capable GPIO can be used | + // | Atmega168/328/p | Only the pins connected to OC1A and OC1B are allowed | + // | Atmega2560 | Only the pins connected to OC4A, OC4B and OC4C are allowed. | + // | Atmega32u4 | Only the pins connected to OC1A, OC1B and OC1C are allowed | + // | Atmel SAM | This can be one of each group of pins: 34/67/74/35, 17/36/72/37/42, 40/64/69/41, 9, 8/44, 7/45, 6 | + // clang-format on + + // ## External Pins + // + // If the direction/enable pins are e.g. connected via external HW (shift + // registers), then an external callback function can be supplied. The + // supplied value is either LOW or HIGH. The return value shall be the status + // of the pin (false for LOW or true for HIGH). If returned value and supplied + // value do not match, the stepper does not continue, but calls this function + // again. + // + // This function is called from cyclic task/interrupt with 4ms rate, which + // creates the commands to put into the command queue. Thus the supplied + // function should take much less time than 4ms. Otherwise there is risk, that + // other running steppers are running out of commands in the queue. If this + // takes longer, then the function should be offloaded and return the new + // status, after the pin change has been successfully completed. + // + // The callback has to be called on the FastAccelStepperEngine. + // See examples/ExternalCall + // + // Stepperpins (enable or direction), which should use this external callback, + // need to be or'ed with PIN_EXTERNAL_FLAG ! FastAccelStepper uses this flag + // to determine, if a pin is external or internal. + void setExternalCallForPin(bool (*func)(uint8_t pin, uint8_t value)); + + // ### Debug LED + // + // If blinking of a LED is required to indicate, the stepper controller is + // still running, then the port. to which the LED is connected, can be told to + // the engine. The periodic task will let the associated LED blink with 1 Hz + static void setDebugLed(uint8_t ledPin); + + /* This should be only called from ISR or stepper task. So do not call it */ + void manageSteppers(); + + private: + bool isDirPinBusy(uint8_t dirPin, uint8_t except_stepper); + + uint8_t _stepper_cnt; + FastAccelStepper* _stepper[MAX_STEPPER]; + + static bool _isValidStepPin(uint8_t step_pin); + bool (*_externalCallForPin)(uint8_t pin, uint8_t value); + + friend class FastAccelStepper; +}; + +// ### Return codes of calls to `move()` and `moveTo()` +// +// The defined preprocessor macros are MOVE_xxx: +// MOVE_OK: All is OK: +// MOVE_ERR_NO_DIRECTION_PIN: Negative direction requested, but no direction pin +// MOVE_ERR_SPEED_IS_UNDEFINED: The maximum speed has not been set yet +// MOVE_ERR_ACCELERATION_IS_UNDEFINED: The acceleration to use has not been set +// yet + +// ### Return codes of `rampState()` +// +// The return value is an uint8_t, which consist of two fields: +// +// | Bit 7 | Bits 6-5 | Bits 4-0 | +// |:--------|:----------|:---------| +// |Always 0 | Direction | State | +// +// The bit mask for direction and state: +#define RAMP_DIRECTION_MASK (32 + 64) +#define RAMP_STATE_MASK (1 + 2 + 4 + 8 + 16) + +// The defined ramp states are: +#define RAMP_STATE_IDLE 0 +#define RAMP_STATE_COAST 1 +#define RAMP_STATE_ACCELERATE 2 +#define RAMP_STATE_DECELERATE 4 +#define RAMP_STATE_REVERSE (4 + 8) +#define RAMP_STATE_ACCELERATING_FLAG 2 +#define RAMP_STATE_DECELERATING_FLAG 4 + +// And the two directions of a move +#define RAMP_DIRECTION_COUNT_UP 32 +#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 +// control the direction flags + +#include "RampGenerator.h" + +// +// ## Timing values - Architecture dependent +// +// ### AVR +// |VARIABLE | Value | Unit | +// |:----------------|------------:|:------------------------| +// |TICKS_PER_S | 16_000_000 | [ticks/s] | +// |MIN_CMD_TICKS | 640 | [1/TICKS_PER_S seconds] | +// |MIN_DIR_DELAY_US | 40 | [µs] | +// |MAX_DIR_DELAY_US | 4095 | [µs] | +// +// ### ESP32 +// |VARIABLE | Value | Unit | +// |:----------------|------------:|:------------------------| +// |TICKS_PER_S | 16_000_000 | [ticks/s] | +// |MIN_CMD_TICKS | 3200 | [1/TICKS_PER_S seconds] | +// |MIN_DIR_DELAY_US | 200 | [µs] | +// |MAX_DIR_DELAY_US | 4095 | [µs] | +// +// ### SAM DUE +// |VARIABLE | Value | Unit | +// |:----------------|------------:|:------------------------| +// |TICKS_PER_S | 21_000_000 | [ticks/s] | +// |MIN_CMD_TICKS | 4200 | [1/TICKS_PER_S seconds] | +// |MIN_DIR_DELAY_US | 200 | [µs] | +// |MAX_DIR_DELAY_US | 3120 | [µs] | +// +// # FastAccelStepper + +#define MAX_ON_DELAY_TICKS ((uint32_t)(65535 * (QUEUE_LEN - 1))) + +#define PIN_UNDEFINED 255 +#define PIN_EXTERNAL_FLAG 128 + +class FastAccelStepper { +#ifdef TEST + public: +#else + private: +#endif + void init(FastAccelStepperEngine* engine, uint8_t num, uint8_t step_pin); + + public: + // ## Step Pin + // step pin is defined at creation. Here can retrieve the pin + uint8_t getStepPin() const; + + // ## Direction Pin + // if direction pin is connected, call this function. + // + // If the pin number is >= 128, then the direction pin is assumed to be + // external and the external callback function (set by + // `setExternalCallForPin()`) is used to set the pin. For direction pin, this + // is implemented for esp32 and its supported derivates, and avr and its + // derivates except atmega32u4 + // + // For slow driver hardware the first step after any polarity change of the + // direction pin can be delayed by the value dir_change_delay_us. The allowed + // range is MIN_DIR_DELAY_US and MAX_DIR_DELAY_US. The special value of 0 + // means, that no delay is added. Values 1 up to MIN_DIR_DELAY_US will be + // clamped to MIN_DIR_DELAY_US. Values above MAX_DIR_DELAY_US will be clamped + // to MAX_DIR_DELAY_US. For external pins, dir_change_delay_us is ignored, + // because the mechanism applied for external pins provides already pause + // in the range of ms or more. + void setDirectionPin(uint8_t dirPin, bool dirHighCountsUp = true, + uint16_t dir_change_delay_us = 0); + inline uint8_t getDirectionPin() { return _dirPin; } + inline bool directionPinHighCountsUp() { return _dirHighCountsUp; } + + // ## Enable Pin + // if enable pin is connected, then use this function. + // + // If the pin number is >= 128, then the enable pin is assumed to be + // external and the external callback function (set by + // `setExternalCallForPin()`) is used to set the pin. + // + // In case there are two enable pins: one low and one high active, then + // these calls are valid and both pins will be operated: + // setEnablePin(pin1, true); + // setEnablePin(pin2, false); + // If pin1 and pin2 are same, then the last call will be used. + void setEnablePin(uint8_t enablePin, bool low_active_enables_stepper = true); + inline uint8_t getEnablePinHighActive() { return _enablePinHighActive; } + inline uint8_t getEnablePinLowActive() { return _enablePinLowActive; } + + // using enableOutputs/disableOutputs the stepper can be enabled and disabled + // For a running motor with autoEnable set, disableOutputs() will return false + bool enableOutputs(); // returns true, if enabled + bool disableOutputs(); // returns true, if disabled + + // In auto enable mode, the stepper is enabled before stepping and disabled + // afterwards. The delay from stepper enabled till first step and from + // last step to stepper disabled can be separately adjusted. + // The delay from enable to first step is done in ticks and as such is limited + // to MAX_ON_DELAY_TICKS, which translates approximately to 120ms for + // esp32 and 60ms for avr at 16 MHz). The delay till disable is done in period + // interrupt/task with 4 or 10 ms repetition rate and as such is with several + // ms jitter. + void setAutoEnable(bool auto_enable); + int8_t setDelayToEnable(uint32_t delay_us); + void setDelayToDisable(uint16_t delay_ms); +#define DELAY_OK 0 +#define DELAY_TOO_LOW -1 +#define DELAY_TOO_HIGH -2 + + // ## Stepper Position + // Retrieve the current position of the stepper + // + // Comment for esp32 with rmt module: + // The actual position may be off by the number of steps in the ongoing + // command. If precise real time position is needed, attaching a pulse counter + // may be of help. + int32_t getCurrentPosition() const; + + // Set the current position of the stepper - either in standstill or while + // moving. + // for esp32: the implementation uses getCurrentPosition(), which does not + // consider the steps of the current command + // => recommend to use only in standstill + void setCurrentPosition(int32_t new_pos); + + // ## Stepper running status + // is true while the stepper is running or ramp generation is active + bool isRunning(); + + // ## Speed + // For stepper movement control by FastAccelStepper's ramp generator + // + // Speed can be defined in four different units: + // - In Hz: This means steps/s + // - In millHz: This means in steps/1000s + // - In us: This means in us/step + // + // For the device's maximum allowed speed, the following calls can be used. + uint16_t getMaxSpeedInUs() const; + uint16_t getMaxSpeedInTicks() const; + uint32_t getMaxSpeedInHz() const; + uint32_t getMaxSpeedInMilliHz() const; + + // 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) const; +#endif + + // Setting the speed can be done with the four `setSpeed...()` calls. + // The new value will be used only after call of these functions: + // + // - `move()` + // - `moveTo()` + // - `runForward()` + // - `runBackward()` + // - `applySpeedAcceleration()` + // - `moveByAcceleration()` + // + // Note: no update on `stopMove()` + // + // Returns 0 on success, or -1 on invalid value. + // Invalid is faster than MaxSpeed or slower than ~250 Mio ticks/step. + int8_t setSpeedInUs(uint32_t min_step_us); + int8_t setSpeedInTicks(uint32_t min_step_ticks); + int8_t setSpeedInHz(uint32_t speed_hz); + int8_t setSpeedInMilliHz(uint32_t speed_mhz); + + // To retrieve current set speed. This means, while accelerating and/or + // decelerating, this is NOT the actual speed ! + inline uint32_t getSpeedInUs() { return _rg.getSpeedInUs(); } + inline uint32_t getSpeedInTicks() { return _rg.getSpeedInTicks(); } + inline uint32_t getSpeedInMilliHz() { return _rg.getSpeedInMilliHz(); } + + // If the current speed is needed, then use `getCurrentSpeed...()`. This + // retrieves the actual speed. + // + // | value | description | + // |:-----:|:-----------------------------| + // | = 0 | while not moving | + // | > 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. + int32_t getCurrentSpeedInUs(bool realtime = true); + int32_t getCurrentSpeedInMilliHz(bool realtime = true); + + // ## Acceleration + // setAcceleration() expects as parameter the change of speed + // as step/s². + // If for example the speed should ramp up from 0 to 10000 steps/s within + // 10s, then the acceleration is 10000 steps/s / 10s = 1000 steps/s² + // + // New value will be used after call to + // move/moveTo/runForward/runBackward/applySpeedAcceleration/moveByAcceleration + // + // note: no update on stopMove() + // + // Returns 0 on success, or -1 on invalid value (<=0) + inline int8_t setAcceleration(int32_t step_s_s) { + return _rg.setAcceleration(step_s_s); + } + inline uint32_t getAcceleration() { return _rg.getAcceleration(); } + + // getCurrentAcceleration() retrieves the actual acceleration. + // = 0 while idle or coasting + // > 0 while speed is changing towards positive values + // < 0 while speed is changeing towards negative values + inline int32_t getCurrentAcceleration() { + return _rg.getCurrentAcceleration(); + } + + // ## Linear Acceleration + // setLinearAcceleration expects as parameter the number of steps, + // where the acceleration is increased linearly from standstill up to the + // configured acceleration value. If this parameter is 0, then there will be + // no linear acceleration phase + // + // If for example the acceleration should ramp up from 0 to 10000 steps/s^2 + // within 100 steps, then call setLinearAcceleration(100) + // + // The speed at which linear acceleration turns into constant acceleration + // can be calculated from the parameter linear_acceleration_steps. + // Let's call this parameter `s_h` for handover steps. + // Then the speed is: + // `v_h = sqrt(1.5 * a * s_h)` + // + // New value will be used after call to + // move/moveTo/runForward/runBackward/applySpeedAcceleration/moveByAcceleration + // + // note: no update on stopMove() + inline void setLinearAcceleration(uint32_t linear_acceleration_steps) { + _rg.setLinearAcceleration(linear_acceleration_steps); + } + + // ## Jump Start + // setJumpStart expects as parameter the ramp step to start from standstill. + // + // The speed at which the stepper will start can be calculated like this: + // - If linear acceleration is not in use: + // start speed `v = sqrt(2 * a * jump_step)` + // - If linear acceleration is in use and `jump_step <= s_h`: + // start speed `v = sqrt(1.5*a)/s_h^(1/6) * jump_step^(2/3)` + // - If linear acceleration is in use and `jump_step > s_h`: + // start speed `v = sqrt(2 * a * (jump_step - s_h/4))` + // + // + // New value will be used after call to + // move/moveTo/runForward/runBackward + inline void setJumpStart(uint32_t jump_step) { _rg.setJumpStart(jump_step); } + + // ## Apply new speed/acceleration value + // This function applies new values for speed/acceleration. + // This is convenient especially, if the stepper is set to continuous running. + void applySpeedAcceleration(); + + // ## Move commands + // ### move() and moveTo() + // start/move the stepper for (move) steps or to an absolute position. + // + // If the stepper is already running, then the current running move will be + // updated together with any updated values of acceleration/speed. The move is + // relative to the target position of any ongoing move ! If the new + // move/moveTo for an ongoing command would reverse the direction, then the + // command is silently ignored. + // return values are the MOVE_... constants + int8_t move(int32_t move, bool blocking = false); + int8_t moveTo(int32_t position, bool blocking = false); + + // ### keepRunning() + // This command flags the stepper to keep run continuously into current + // direction. It can be stopped by stopMove. + // Be aware, if the motor is currently decelerating towards reversed + // direction, then keepRunning() will speed up again and not finish direction + // reversal first. + void keepRunning(); + bool isRunningContinuously() { return _rg.isRunningContinuously(); } + + // ### runForward() and runBackwards() + // These commands just let the motor run continuously in one direction. + // If the motor is running in the opposite direction, it will reverse + // return value as with move/moveTo + int8_t runForward(); + int8_t runBackward(); + + // ### forwardStep() and backwardStep() + // forwardStep()/backwardstep() can be called, while stepper is not moving + // If stepper is moving, this is a no-op. + // backwardStep() is a no-op, if no direction pin defined + // It will immediately let the stepper perform one single step. + // If blocking = true, then the routine will wait till isRunning() is false + void forwardStep(bool blocking = false); + void backwardStep(bool blocking = false); + + // ### moveByAcceleration() + // moveByAcceleration() can be called, if only the speed of the stepper + // is of interest and that speed to be controlled by acceleration. + // The maximum speed (in both directions) to be set by setSpeedInUs() before. + // The behaviour will be: + // acceleration > 0 => accelerate towards positive maximum speed + // acceleration = 0 => keep current speed + // acceleration < 0 + // => accelerate towards negative maximum speed if allow_reverse + // => decelerate towards motor stop if allow_reverse = false + // return value as with move/moveTo + int8_t moveByAcceleration(int32_t acceleration, bool allow_reverse = true); + + // ### stopMove() + // Stop the running stepper with normal deceleration. + // This only sets a flag and can be called from an interrupt ! + void stopMove(); + inline bool isStopping() { return _rg.isStopping(); } + + // ### stepsToStop() + // This returns the current step value of the ramp. + // This equals the number of steps for a motor to + // reach the current position and speed from standstill + // and to come to standstill with deceleration if stopped + // immediately. + // This value is valid with or without linear acceleration + // being used. + // Primary use is to forecast possible stop position. + // The stop position is: + // getCurrentPosition() + stepsToStop() + // in case of a motor running in positive direction. + uint32_t stepsToStop() { return _rg.stepsToStop(); } + + // ### forceStop() + // Abruptly stop the running stepper without deceleration. + // This can be called from an interrupt ! + // + // The stepper command queue will be processed, but no further commands are + // added. This means, that the stepper can be expected to stop within approx. + // 20ms. + void forceStop(); + + // abruptly stop the running stepper without deceleration. + // This can be called from an interrupt ! + // + // No further step will be issued. As this is aborting all commands in the + // queue, the actual stop position is lost (recovering this position cannot be + // done within an interrupt). So the new position after stop has to be + // provided and will be set as current position after stop. + void forceStopAndNewPosition(int32_t new_pos); + + // get the target position for the current move. + // As of now, this position is the view of the stepper task. + // This means, the value will stay unchanged after a move/moveTo until the + // stepper task is executed. + // In keep running mode, the targetPos() is not updated + inline int32_t targetPos() { return _rg.targetPosition(); } + + // ### Task planning + // 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. + // But the queue may run out of commands while executing addQueueEntry, + // so it is better to set start=true to automatically restart/continue + // a running queue. + // + // If the queue is not running, then the start parameter defines starting it + // or not. The latter case is of interest to first fill the queue and then + // start it. + // + // The call addQueueEntry(NULL, true) just starts the queue. This is intended + // to achieve a near synchronous start of several steppers. Consequently it + // should be called with interrupts disabled and return very fast. + // Actually this is necessary, too, in case the queue is full and not + // started. + int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start = true); + + // Return codes for addQueueEntry + // positive values mean, that caller should retry later +#define AQE_OK 0 +#define AQE_QUEUE_FULL 1 +#define AQE_DIR_PIN_IS_BUSY 2 +#define AQE_WAIT_FOR_ENABLE_PIN_ACTIVE 3 +#define AQE_DEVICE_NOT_READY 4 +#define AQE_ERROR_TICKS_TOO_LOW -1 +#define AQE_ERROR_EMPTY_QUEUE_TO_START -2 +#define AQE_ERROR_NO_DIR_PIN_TO_TOGGLE -3 + + // ### check functions for command queue being empty, full or running. + bool isQueueEmpty() const; + bool isQueueFull() const; + bool isQueueRunning() const; + + // ### functions to get the fill level of the queue + // + // To retrieve the forward planning time in the queue, ticksInQueue() + // can be used. It sums up all ticks of the not yet processed commands. + // For commands defining pauses, the summed up value is entry.ticks. + // For commands with steps, the summed up value is entry.steps*entry.ticks + uint32_t ticksInQueue(); + + // This function can be used to check, if the commands in the queue + // will last for ticks. This is again without the + // currently processed command. + bool hasTicksInQueue(uint32_t min_ticks) const; + + // This function allows to check the number of commands in the queue. + // This is including the currently processed command. + uint8_t queueEntries() const; + + // Get the future position of the stepper after all commands in queue are + // completed + int32_t getPositionAfterCommandsCompleted() const; + + // Get the future speed of the stepper after all commands in queue are + // completed. This is in µs. Returns 0 for stopped motor + // + // This value comes from the ramp generator and is not valid for raw command + // queue + // ==> Will be renamed in future release + uint32_t getPeriodInUsAfterCommandsCompleted(); + uint32_t getPeriodInTicksAfterCommandsCompleted(); + + // Set the future position of the stepper after all commands in queue are + // completed. This has immediate effect to getCurrentPosition(). + void setPositionAfterCommandsCompleted(int32_t new_pos); + + // This function provides info, in which state the high level stepper control + // is operating. The return value is an `or` of RAMP_STATE_... and + // RAMP_DIRECTION_... flags. Definitions are above + inline uint8_t rampState() { return _rg.rampState(); } + + // returns true, if the ramp generation is active + inline bool isRampGeneratorActive() { return _rg.isRampGeneratorActive(); } + + // These functions allow to detach and reAttach a step pin for other use. + // Pretty low level, use with care or not at all + void detachFromPin() const; + void reAttachToPin() const; + + // ## ESP32 only: Free pulse counter + // These four functions are only available on esp32. + // The first can attach any of the eight pulse counters to this stepper. + // The second then will read the current pulse counter value + // + // The user is responsible to not use a pulse counter, which is occupied by a + // stepper and by this render the stepper or even blow up the uC. + // + // Pulse counter 6 and 7 are not used by the stepper library and are judged as + // available. If only five steppers are defined, then 5 gets available. If + // four steppers are defined, then 4 is usable,too. + // + // These functions are intended primarily for testing, because the library + // should always output the correct amount of pulses. Possible application + // usage would be an immediate and interrupt friendly version for + // getCurrentPosition() + // + // The pulse counter counts up towards high_value. + // If the high_value is reached, then the pulse counter is reset to 0. + // Similarly, if direction pin is configured and set to count down, + // then the pulse counter counts towards low_value. When the low value is hit, + // the pulse counter is reset to 0. + // + // If low_value and high_value are set to zero, then the pulse counter is just + // counting like any int16_t counter: 0...32767,-32768,-32767,...,0 and + // backwards accordingly + // + // Possible application: + // Assume the stepper, to which the pulse counter attached to, needs 3200 + // steps/revolution. If now attachToPulseCounter is called with -3200 and 3200 + // for the low and high values respectively, then the momentary angle of the + // stepper (at exact this moment) can be retrieved just by reading the pulse + // counter. If the value is negative, then just add 3200. + // + // Update for idf5 version: + // The pcnt_unit value is not used, because the available units are managed + // by the system. The parameter is kept for compatibility. + // +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5) + bool attachToPulseCounter(uint8_t unused_pcnt_unit = 0, + int16_t low_value = -16384, + int16_t high_value = 16384); + int16_t readPulseCounter(); + void clearPulseCounter(); + inline bool pulseCounterAttached() { return _attached_pulse_unit != NULL; } +#endif +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4) + bool attachToPulseCounter(uint8_t pcnt_unit, int16_t low_value = -16384, + int16_t high_value = 16384); + int16_t readPulseCounter(); + void clearPulseCounter(); + inline bool pulseCounterAttached() { return _attached_pulse_cnt_unit >= 0; } +#endif + + private: + void performOneStep(bool count_up, bool blocking = false); +#ifdef SUPPORT_EXTERNAL_DIRECTION_PIN + bool externalDirPinChangeCompletedIfNeeded(); +#endif + void fill_queue(); + void updateAutoDisable(); + void blockingWaitForForceStopComplete(); + bool needAutoDisable(); + bool agreeWithAutoDisable(); + bool usesAutoEnablePin(uint8_t pin) const; + void getCurrentSpeedInTicks(struct actual_ticks_s* speed, bool realtime); + + FastAccelStepperEngine* _engine; + RampGenerator _rg; + uint8_t _stepPin; + uint8_t _dirPin; + bool _dirHighCountsUp; + bool _autoEnable; + uint8_t _enablePinLowActive; + uint8_t _enablePinHighActive; + uint8_t _queue_num; + + uint16_t _dir_change_delay_ticks; + uint32_t _on_delay_ticks; + uint16_t _off_delay_count; + uint16_t _auto_disable_delay_counter; + + uint32_t _forward_planning_in_ticks; + +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5) + pcnt_unit_handle_t _attached_pulse_unit; +#endif +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4) + int16_t _attached_pulse_cnt_unit; +#endif +#if (TEST_MEASURE_ISR_SINGLE_FILL == 1) + uint32_t max_micros; +#endif + + friend class FastAccelStepperEngine; + friend class FastAccelStepperTest; +}; + +#endif diff --git a/src/FastAccelStepper_idf4_esp32_pcnt.cpp b/src/FastAccelStepper_idf4_esp32_pcnt.cpp new file mode 100644 index 0000000..f66f3a7 --- /dev/null +++ b/src/FastAccelStepper_idf4_esp32_pcnt.cpp @@ -0,0 +1,95 @@ +#include "StepperISR.h" +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4) + +uint32_t sig_idx[SUPPORT_ESP32_PULSE_COUNTER] = { + PCNT_SIG_CH0_IN0_IDX, PCNT_SIG_CH0_IN1_IDX, + PCNT_SIG_CH0_IN2_IDX, PCNT_SIG_CH0_IN3_IDX, +#if SUPPORT_ESP32_PULSE_COUNTER == 8 + PCNT_SIG_CH0_IN4_IDX, PCNT_SIG_CH0_IN5_IDX, + PCNT_SIG_CH0_IN6_IDX, PCNT_SIG_CH0_IN7_IDX, +#endif +}; +uint32_t ctrl_idx[SUPPORT_ESP32_PULSE_COUNTER] = { + PCNT_CTRL_CH0_IN0_IDX, PCNT_CTRL_CH0_IN1_IDX, PCNT_CTRL_CH0_IN2_IDX, + PCNT_CTRL_CH0_IN3_IDX, +#if SUPPORT_ESP32_PULSE_COUNTER == 8 + PCNT_CTRL_CH0_IN4_IDX, PCNT_CTRL_CH0_IN5_IDX, PCNT_CTRL_CH0_IN6_IDX, + PCNT_CTRL_CH0_IN7_IDX +#endif +}; + +bool FastAccelStepper::attachToPulseCounter(uint8_t pcnt_unit, + int16_t low_value, + int16_t high_value) { + if (pcnt_unit >= SUPPORT_ESP32_PULSE_COUNTER) { + return false; + } + + pcnt_config_t cfg; + uint8_t dir_pin = getDirectionPin(); + uint8_t step_pin = getStepPin(); + cfg.pulse_gpio_num = PCNT_PIN_NOT_USED; + if (dir_pin == PIN_UNDEFINED) { + cfg.ctrl_gpio_num = PCNT_PIN_NOT_USED; + cfg.hctrl_mode = PCNT_MODE_KEEP; + cfg.lctrl_mode = PCNT_MODE_KEEP; + } else { + cfg.ctrl_gpio_num = dir_pin; + if (directionPinHighCountsUp()) { + cfg.lctrl_mode = PCNT_MODE_REVERSE; + cfg.hctrl_mode = PCNT_MODE_KEEP; + } else { + cfg.lctrl_mode = PCNT_MODE_KEEP; + cfg.hctrl_mode = PCNT_MODE_REVERSE; + } + } + cfg.pos_mode = PCNT_COUNT_INC; // increment on rising edge + cfg.neg_mode = PCNT_COUNT_DIS; // ignore falling edge + cfg.counter_h_lim = high_value; + cfg.counter_l_lim = low_value; + cfg.unit = (pcnt_unit_t)pcnt_unit; + cfg.channel = PCNT_CHANNEL_0; + pcnt_unit_config(&cfg); + +#ifndef HAVE_ESP32S3_PULSE_COUNTER + PCNT.conf_unit[cfg.unit].conf0.thr_h_lim_en = 0; + PCNT.conf_unit[cfg.unit].conf0.thr_l_lim_en = 0; +#else + PCNT.conf_unit[cfg.unit].conf0.thr_h_lim_en_un = 0; + PCNT.conf_unit[cfg.unit].conf0.thr_l_lim_en_un = 0; +#endif + + // detachFromPin(); + // reAttachToPin(); + gpio_matrix_in(step_pin, sig_idx[pcnt_unit], 0); + gpio_iomux_in(step_pin, + sig_idx[pcnt_unit]); // test failure without this call + if (dir_pin != PIN_UNDEFINED) { + pinMode(dir_pin, OUTPUT); + gpio_matrix_out(dir_pin, 0x100, false, false); + gpio_matrix_in(dir_pin, ctrl_idx[pcnt_unit], 0); + gpio_iomux_in(dir_pin, ctrl_idx[pcnt_unit]); + } + + pcnt_counter_clear(cfg.unit); + pcnt_counter_resume(cfg.unit); + + _attached_pulse_cnt_unit = pcnt_unit; + return true; +} +void FastAccelStepper::clearPulseCounter() { + if (pulseCounterAttached()) { + pcnt_counter_clear((pcnt_unit_t)_attached_pulse_cnt_unit); + } +} +int16_t FastAccelStepper::readPulseCounter() { + if (pulseCounterAttached()) { +#ifndef HAVE_ESP32S3_PULSE_COUNTER + return PCNT.cnt_unit[(pcnt_unit_t)_attached_pulse_cnt_unit].cnt_val; +#else + return PCNT.cnt_unit[(pcnt_unit_t)_attached_pulse_cnt_unit].pulse_cnt_un; +#endif + } + return 0; +} +#endif diff --git a/src/FastAccelStepper_idf5_esp32_pcnt.cpp b/src/FastAccelStepper_idf5_esp32_pcnt.cpp new file mode 100644 index 0000000..1912c3c --- /dev/null +++ b/src/FastAccelStepper_idf5_esp32_pcnt.cpp @@ -0,0 +1,137 @@ +#include "StepperISR.h" +#if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5) + +// Why the hell, does espressif think, that the unit and channel id are not +// needed ? Without unit/channel ID, the needed parameter for +// gpio_matrix_in/gpio_iomux_in cannot be derived. +// +// Here we declare the private pcnt_chan_t structure, which is not save. +struct pcnt_unit_t { + /*pcnt_group_t*/ void *group; + portMUX_TYPE spinlock; + int unit_id; + // remainder of struct not needed +}; +struct pcnt_chan_t { + pcnt_unit_t *unit; + int channel_id; + // remainder of struct not needed +}; + +bool FastAccelStepper::attachToPulseCounter(uint8_t unused_pcnt_unit, + int16_t low_value, + int16_t high_value) { + pcnt_unit_config_t config = {.low_limit = low_value, + .high_limit = high_value, + .intr_priority = 0, + .flags = {.accum_count = 0}}; + pcnt_unit_handle_t punit = NULL; + esp_err_t rc; + rc = pcnt_new_unit(&config, &punit); + if (rc != ESP_OK) { + return false; + } + + pcnt_chan_config_t chan_config = {.edge_gpio_num = -1, + .level_gpio_num = -1, + .flags = { + .invert_edge_input = 0, + .invert_level_input = 0, + .virt_edge_io_level = 0, + .virt_level_io_level = 0, + .io_loop_back = 0, + }}; + + pcnt_channel_level_action_t level_high = PCNT_CHANNEL_LEVEL_ACTION_KEEP; + pcnt_channel_level_action_t level_low = PCNT_CHANNEL_LEVEL_ACTION_KEEP; + uint8_t dir_pin = getDirectionPin(); + if (dir_pin != PIN_UNDEFINED) { + chan_config.level_gpio_num = dir_pin; + if (directionPinHighCountsUp()) { + level_low = PCNT_CHANNEL_LEVEL_ACTION_INVERSE; + } else { + level_high = PCNT_CHANNEL_LEVEL_ACTION_INVERSE; + } + } + + pcnt_channel_handle_t pcnt_chan = NULL; + rc = pcnt_new_channel(punit, &chan_config, &pcnt_chan); + if (rc != ESP_OK) { + pcnt_del_unit(punit); + return false; + } + + int unit_id = punit->unit_id; + int channel_id = pcnt_chan->channel_id; + if ((unit_id < 0) || (unit_id >= SUPPORT_ESP32_PULSE_COUNTER)) { + // perhaps the pcnt_chan_t-structure is changed !? + pcnt_del_channel(pcnt_chan); + pcnt_del_unit(punit); + return false; + } + + rc = + pcnt_channel_set_edge_action(pcnt_chan, PCNT_CHANNEL_EDGE_ACTION_INCREASE, + PCNT_CHANNEL_EDGE_ACTION_HOLD); + if (rc != ESP_OK) { + return false; + } + rc = pcnt_channel_set_level_action(pcnt_chan, level_high, level_low); + if (rc != ESP_OK) { + return false; + } + + rc = pcnt_unit_enable(punit); + if (rc != ESP_OK) { + return false; + } + rc = pcnt_unit_clear_count(punit); + if (rc != ESP_OK) { + return false; + } + rc = pcnt_unit_start(punit); + if (rc != ESP_OK) { + return false; + } + + uint8_t step_pin = getStepPin(); +#ifdef TRACE + printf("pins = %d/%d unit_id=%d channel_id=%d\n", step_pin, dir_pin, unit_id, + channel_id); +#endif + int signal = pcnt_periph_signals.groups[0] + .units[unit_id] + .channels[channel_id] + .pulse_sig; + gpio_matrix_in(step_pin, signal, 0); + gpio_iomux_in(step_pin, signal); + if (dir_pin != PIN_UNDEFINED) { + pinMode(dir_pin, OUTPUT); + int control = pcnt_periph_signals.groups[0] + .units[unit_id] + .channels[channel_id] + .control_sig; + gpio_iomux_out(dir_pin, 0x100, false); + gpio_matrix_in(dir_pin, control, 0); + gpio_iomux_in(dir_pin, control); + } + + _attached_pulse_unit = punit; + + return true; +} + +void FastAccelStepper::clearPulseCounter() { + if (pulseCounterAttached()) { + pcnt_unit_clear_count(_attached_pulse_unit); + } +} +int16_t FastAccelStepper::readPulseCounter() { + int value = 0; + if (pulseCounterAttached()) { + pcnt_unit_get_count(_attached_pulse_unit, &value); + } + return value; +} + +#endif diff --git a/src/PoorManFloat.cpp b/src/PoorManFloat.cpp new file mode 100644 index 0000000..7822c2a --- /dev/null +++ b/src/PoorManFloat.cpp @@ -0,0 +1,302 @@ +#include +#if defined(ARDUINO_ARCH_AVR) +#include +#else +#define PROGMEM +#define pgm_read_byte_near(x) (*(x)) +#endif +#include "PoorManFloat.h" +#ifdef TEST +#include +#endif + +// In FastAccelStepper there is seldom the need for adding or +// subtracting floats, but multiplication/division/square and power +// are often in use. Negative numbers and even zero are not needed. +// Consequently, a purely logarithmic representation is completely +// sufficient and the necessary range can be achieved by 16 bit signed integers. +// The interpretation of a signed integer xi is: +// +// x = 2^(xi/512) +// +// The signed integer range xi (-32768..32767) is mapped to the +// range (5e-20, 1.8e19). Please note: zero is not included. +// The greatest constant in use is 2.2e14. So the range is sufficient. +// +// In order to map x to xi, log2(x) needs to be calculated. +// For this we rewrite x to: +// x = (1 + r) * 2^e +// +// With r being in the range 0 <= r < 1. +// This means e is the largest integer value with 2^e < x. +// Consequently, e can be derived by counting the leading numbers in +// the integer x. +// +// So xi = 512*log2(x) = 512 * (e + log(1+r)) +// +// The first 7 bits is plain e with sign and the lower 9 bits is log(1+r)*512 +// xi = eeee_eeem_mmmm_mmmm +// +// Example for 16 bit integer: +// x = 15373 = 0b0011_1100_0000_1101 +// +// log2(15373)*512 = 7121 (rounded) +// = 0b0001_1011_1101_0001 +// +// two leading zeros => e = 15 - 2 = 13 = 0b1101 +// 2^e = 8192 +// +// => x = 8192 * 1.1_1100_0000_1101 +// r is the decimal part without the leading 1 +// r = 1_1100_0000 / 512 +// log2(1+r) * 512 = 464 = 0b1_1101_0001 +// +// xi = eeee_eeem_mmmm_mmmm +// = 0001_1011_1101_0001 +// = 0x1bd1 +// +// The remaining task is to calculate log2(1+f). +// +// log2(1+r) is at the corners identical to r: +// log2(1+0) = 0 and log2(1+1) = 1 +// So it is interesting to look at function +// f(1+r) = log2(1+r) - r +// +// This function is 0 for r at 0 and 1, positive over the range inbetween +// and reaches max value of 0.08607 at r = 0.442695. +// This max value is at r = 1/ln(2)-1 +// +// As we need actually 512*log2(1+r), then the max value is 44. +// This allows to multiply with up to 8 to improve the resolution. +// Here we use factor 4, so two table values can be summed up without overflow. +// +// So the log2 can be calculated for e.g.: +// x = 0001_mmmm_mmmm_mmmm three leading zeros +// +// Shift left by leading zeros+1 (removing leading 1) and then right by 5: +// x = 0001_mmmm_mmmm_mmmm three leading zeros +// mmmm_mmmm_mmmm_m000 shift left (3+1) +// 0000_0mmm_mmmm_mmmm shift right (5) +// if ninth bit is 0: +// + ttt_tttt0 add table value for mmmm_mmmm shifted one +// else: +// + 0ttt_tttt add table value for mmmm_mmmm +// + 0ttt_tttt add table value for mmmm_mmmm+1 +// + 0000_0001 round +// result: mmmm_mmmm_m +// final: 000e_eeem_mmmm_mmmm For up to 32bits (e = 31) +// +// We are using a table of length 256, so first 8 bits are the index. +// In order to improve the resolution, the ninth bit is used to interpolate +// with the next table entry. +// +// Using python3 this can be calculated by: +// [round(math.log2(i/256) * 256 - (i-256)) for i in range(256,512)] +// +// For better precision y_yyyy is shifted by 2 and can be calculated as: +// [round((math.log2(i/256) * 256 - (i-256))*4) for i in range(256,512)] +// +const PROGMEM uint8_t log2_minus_x_plus_one_shifted_by_2[256] = { + 0, 2, 3, 5, 7, 9, 10, 12, 13, 15, 17, 18, 20, 21, 23, 24, 26, 27, 28, + 30, 31, 32, 34, 35, 36, 38, 39, 40, 41, 43, 44, 45, 46, 47, 48, 49, 50, 51, + 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 64, 65, 66, 67, 68, 68, + 69, 70, 70, 71, 72, 72, 73, 74, 74, 75, 75, 76, 77, 77, 78, 78, 79, 79, 80, + 80, 80, 81, 81, 82, 82, 83, 83, 83, 84, 84, 84, 84, 85, 85, 85, 86, 86, 86, + 86, 86, 87, 87, 87, 87, 87, 87, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, + 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 87, 87, 87, 87, 87, 87, 86, 86, + 86, 86, 86, 85, 85, 85, 85, 84, 84, 84, 84, 83, 83, 83, 82, 82, 82, 81, 81, + 81, 80, 80, 79, 79, 79, 78, 78, 77, 77, 76, 76, 75, 75, 74, 74, 73, 73, 72, + 72, 71, 71, 70, 70, 69, 68, 68, 67, 67, 66, 65, 65, 64, 63, 63, 62, 61, 61, + 60, 59, 59, 58, 57, 57, 56, 55, 54, 54, 53, 52, 51, 51, 50, 49, 48, 47, 47, + 46, 45, 44, 43, 42, 42, 41, 40, 39, 38, 37, 36, 35, 34, 34, 33, 32, 31, 30, + 29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, + 10, 9, 8, 7, 6, 4, 3, 2, 1}; + +// For the inverse pow(2,x) needs to be calculated. Similarly it makes sense to +// evaluate instead +// g(x) = x - pow(2,x-1) +// +// This function equals 0 for x at 1 and 2, with extremum 0.08607 at x +// = 1.528766. This max. value is at x = 1 - ln(ln(2))/ln(2) +// +// Noteworthy the max values of log2(x) - x + 1 and x - pow(2, x-1) are +// identical, calculated by (-1 + ln(2) - ln(ln(2)))/ln(2) +// +// Using python3 this can be calculated by: +// [round(i - math.pow(2,i/256-1)*256) for i in range(256,512)] +// +// Similarly shifted by two bits: +// [round((i - math.pow(2,i/256-1)*256)*4) for i in range(256,512)] +// +const PROGMEM uint8_t x_minus_pow2_of_x_minus_one_shifted_by_2[256] = { + 0, 1, 2, 4, 5, 6, 7, 8, 10, 11, 12, 13, 14, 15, 16, 18, 19, 20, 21, + 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, + 41, 42, 43, 44, 45, 46, 46, 47, 48, 49, 50, 51, 52, 52, 53, 54, 55, 56, 56, + 57, 58, 59, 59, 60, 61, 62, 62, 63, 64, 64, 65, 66, 66, 67, 68, 68, 69, 69, + 70, 71, 71, 72, 72, 73, 73, 74, 74, 75, 76, 76, 76, 77, 77, 78, 78, 79, 79, + 80, 80, 80, 81, 81, 82, 82, 82, 83, 83, 83, 84, 84, 84, 84, 85, 85, 85, 85, + 86, 86, 86, 86, 87, 87, 87, 87, 87, 87, 87, 88, 88, 88, 88, 88, 88, 88, 88, + 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 88, 87, 87, 87, 87, 87, + 87, 86, 86, 86, 86, 86, 85, 85, 85, 84, 84, 84, 84, 83, 83, 83, 82, 82, 81, + 81, 81, 80, 80, 79, 79, 78, 78, 77, 77, 76, 76, 75, 75, 74, 74, 73, 72, 72, + 71, 71, 70, 69, 68, 68, 67, 66, 66, 65, 64, 63, 63, 62, 61, 60, 59, 58, 58, + 57, 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39, + 38, 36, 35, 34, 33, 32, 30, 29, 28, 27, 25, 24, 23, 22, 20, 19, 17, 16, 15, + 13, 12, 10, 9, 8, 6, 5, 3, 2}; + +uint8_t leading_zeros(uint8_t x) { + uint8_t res; + if ((x & 0xf0) == 0) { + x <<= 4; + res = 4; + } else { + res = 0; + } + if ((x & 0xc0) == 0) { + x <<= 2; + res += 2; + } + if ((x & 0x80) == 0) { + res += 1; + if (x == 0) { + res += 1; + } + } + return res; +} + +pmf_logarithmic pmfl_from(uint8_t x) { + // calling with x == 0 is considered an error. + // + // In a first step convert to + // 0000_0eee_mmmm_mmmm + // + // Hereby mmmm_mmmm are the lower bits right from the first 1 in x + // An example with only four valid mantissa bits: + // x = 0001_mmmm => 0000_0100_mmmm_0000 + // + // eee is the exponent + // + // The second convert this to the logarithm of x + // 1. Use mmmm_mmmm as index in the log2_minus_x_plus_one_shifted_by_1 + // table + // 2. shift left 0000_0eee_mmmm_mmmm by 1 + // 3. add the value from the log2_minus_x_plus_one_shifted_by_1 table + uint8_t leading = leading_zeros(x); + if (leading == 8) { + return PMF_CONST_INVALID; + } + x <<= leading + 1; + uint8_t e = 7 - leading; + uint8_t offset = pgm_read_byte_near(&log2_minus_x_plus_one_shifted_by_2[x]); + uint16_t res = (((uint16_t)e) << 8) | x; + res <<= 1; + offset += 1; + res += offset >> 1; + return res; +} + +pmf_logarithmic pmfl_from(uint16_t x) { + uint8_t leading = leading_zeros(x >> 8); + if (leading == 8) { + return pmfl_from((uint8_t)x); + } + // shift msb out + x <<= leading + 1; + uint8_t exponent = 15 - leading; + x >>= 5; + uint8_t index = x >> 3; + uint8_t offset = + pgm_read_byte_near(&log2_minus_x_plus_one_shifted_by_2[index]); + // only with x & 7 > 2, the calculated constants are correct... + if ((x & 7) > 2) { + index++; // overflow to 0 is ok. index is an uint8_t + offset += pgm_read_byte_near(&log2_minus_x_plus_one_shifted_by_2[index]); + offset += 1; + } else { + offset <<= 1; + // offset += 1; + } + x += offset; + x >>= 2; + x += ((uint16_t)exponent) << 9; + return x; +} +pmf_logarithmic pmfl_from(uint32_t x) { + int16_t exp_offset; + uint16_t w; + if ((x & 0xff000000) == 0) { + if ((x & 0x00ff0000) == 0) { + w = (uint16_t)x; + exp_offset = 0; + } else if ((x & 0x00f00000) == 0) { + w = x >> 4; + exp_offset = 0x0800; + } else { + w = x >> 8; + exp_offset = 0x1000; + } + } else if ((x & 0xf0000000) == 0) { + w = x >> 12; + exp_offset = 0x1800; + } else { + w = x >> 16; + exp_offset = 0x2000; + } + return pmfl_from(w) + exp_offset; +} +uint16_t pmfl_to_u16(pmf_logarithmic x) { + if (x < 0) { + return 0; + } + if (x >= PMF_CONST_UINT16_MAX) { + return __UINT16_MAX__; + } + uint8_t exponent = ((uint16_t)x) >> 9; + x &= 0x01ff; + x += 0x200; + uint8_t index = ((uint16_t)x) >> 1; + x <<= 1; + uint8_t offset = + pgm_read_byte_near(&x_minus_pow2_of_x_minus_one_shifted_by_2[index]); + if ((x & 2) != 0) { + index++; // overflow to 0 is ok. index is an uint8_t + offset += + pgm_read_byte_near(&x_minus_pow2_of_x_minus_one_shifted_by_2[index]); + offset >>= 1; + } + x -= offset; + if (exponent > 10) { + x <<= exponent - 10; + } else if (exponent < 10) { + x += (exponent != 9) ? 2 : 1; + x >>= 10 - exponent; + } + return x; +} +uint32_t pmfl_to_u32(pmf_logarithmic x) { + if (x < 0) { + return 0; + } + if (x >= PMF_CONST_UINT32_MAX) { + return __UINT32_MAX__; + } + uint8_t exponent = ((uint16_t)x) >> 9; + if (exponent < 0x10) { + return pmfl_to_u16(x); + } + uint8_t shift = exponent - 0x0f; + x = pmfl_shr(x, shift); + uint32_t res = pmfl_to_u16(x); + res <<= shift; + return res; +} +pmf_logarithmic pmfl_square(pmf_logarithmic x) { + if (x > 0x4000) { + return PMF_CONST_MAX; + } + if (x <= -0x4000) { + return PMF_CONST_MIN; + } + return x + x; +} diff --git a/src/PoorManFloat.h b/src/PoorManFloat.h new file mode 100644 index 0000000..66f33d3 --- /dev/null +++ b/src/PoorManFloat.h @@ -0,0 +1,45 @@ +#ifndef POORMANFLOAT_H +#define POORMANFLOAT_H +#include + +#include + +typedef int16_t pmf_logarithmic; + +#define PMF_CONST_INVALID ((pmf_logarithmic)0x8000) +#define PMF_CONST_MAX ((pmf_logarithmic)0x7fff) +#define PMF_CONST_MIN ((pmf_logarithmic)0x8001) + +pmf_logarithmic pmfl_from(uint8_t x); +pmf_logarithmic pmfl_from(uint16_t x); +pmf_logarithmic pmfl_from(uint32_t x); + +uint16_t pmfl_to_u16(pmf_logarithmic x); +uint32_t pmfl_to_u32(pmf_logarithmic x); + +#define pmfl_shl(x, n) ((x) + (((int16_t)(n)) << 9)) +#define pmfl_shr(x, n) ((x) - (((int16_t)(n)) << 9)) + +#define pmfl_multiply(x, y) ((x) + (y)) +#define pmfl_divide(x, y) ((x) - (y)) +#define pmfl_reciprocal(x) (-(x)) +#define pmfl_sqrt(x) ((x) / 2) +#define pmfl_rsqrt(x) (-(x) / 2) +#define pmfl_rsquare(x) pmfl_reciprocal(pmfl_square(x)) + +inline pmf_logarithmic pmfl_pow_div_3(pmf_logarithmic x) { + // 1/3 ~ (1/4+1/16+1/64+1/256+1/1024+1/4096+1/16384) + x /= 2; // x/2 + x += x / 4; // x/2 + x/8 + x += x / 16; // x/2 + x/8 + x/32 + x/128 + x += x / 256; // x/2 + x/8 + x/32 + x/128 + x/512 + x/2048 + x/8192 + x/32768 + x += 1; + return x / 2; +} +#define pmfl_pow_2_div_3(x) ((x) - pmfl_pow_div_3(x)) +#define pmfl_pow_3_div_2(x) ((x) + (x) / 2) + +pmf_logarithmic pmfl_square(pmf_logarithmic x); + +uint8_t leading_zeros(uint8_t x); +#endif diff --git a/src/PoorManFloatConst.h b/src/PoorManFloatConst.h new file mode 100644 index 0000000..637851e --- /dev/null +++ b/src/PoorManFloatConst.h @@ -0,0 +1,73 @@ +// Autogenerated by extras/gen_pmf_const/main +// DO NOT EDIT +#ifndef POORMANFLOATCONST_H +#define POORMANFLOATCONST_H + +#include + +// PMF_CONST_1 = 0.000000 0 = 0x0000 +#define PMF_CONST_1 ((pmf_logarithmic)0x0000) +// => converted back => 1.00 + +// PMF_CONST_3_DIV_2 = 299.500793 300 = 0x012c +#define PMF_CONST_3_DIV_2 ((pmf_logarithmic)0x012c) +// => converted back => 1.50 + +// PMF_CONST_128E12 = 23993.925781 23994 = 0x5dba +#define PMF_CONST_128E12 ((pmf_logarithmic)0x5dba) +// => converted back => 128012783714304.00 + +// PMF_CONST_16E6 = 12252.962891 12253 = 0x2fdd +#define PMF_CONST_16E6 ((pmf_logarithmic)0x2fdd) +// => converted back => 16000799.00 + +// PMF_CONST_500 = 4590.481445 4590 = 0x11ee +#define PMF_CONST_500 ((pmf_logarithmic)0x11ee) +// => converted back => 499.67 + +// PMF_CONST_1000 = 5102.481445 5102 = 0x13ee +#define PMF_CONST_1000 ((pmf_logarithmic)0x13ee) +// => converted back => 999.35 + +// PMF_CONST_2000 = 5614.481445 5614 = 0x15ee +#define PMF_CONST_2000 ((pmf_logarithmic)0x15ee) +// => converted back => 1998.70 + +// PMF_CONST_32000 = 7662.481445 7662 = 0x1dee +#define PMF_CONST_32000 ((pmf_logarithmic)0x1dee) +// => converted back => 31979.14 + +// PMF_CONST_16E6_DIV_SQRT_OF_2 = 11996.962891 11997 = 0x2edd +#define PMF_CONST_16E6_DIV_SQRT_OF_2 ((pmf_logarithmic)0x2edd) +// => converted back => 11314274.00 + +// PMF_CONST_21E6 = 12453.830078 12454 = 0x30a6 +#define PMF_CONST_21E6 ((pmf_logarithmic)0x30a6) +// => converted back => 21004844.00 + +// PMF_CONST_42000 = 7863.348145 7863 = 0x1eb7 +#define PMF_CONST_42000 ((pmf_logarithmic)0x1eb7) +// => converted back => 41980.21 + +// PMF_CONST_21E6_DIV_SQRT_OF_2 = 12197.830078 12198 = 0x2fa6 +#define PMF_CONST_21E6_DIV_SQRT_OF_2 ((pmf_logarithmic)0x2fa6) +// => converted back => 14852668.00 + +// PMF_CONST_2205E11 = 24395.660156 24396 = 0x5f4c +#define PMF_CONST_2205E11 ((pmf_logarithmic)0x5f4c) +// => converted back => 220601734135808.00 + +// PMF_CONST_UINT32_MAX = 16384.000000 16384 = 0x4000 +#define PMF_CONST_UINT32_MAX ((pmf_logarithmic)0x4000) +// => converted back => 4294967296.00 + +// PMF_CONST_UINT16_MAX = 8191.988770 8192 = 0x2000 +#define PMF_CONST_UINT16_MAX ((pmf_logarithmic)0x2000) +// => converted back => 65536.00 + +// used in PoorManFloat.cpp as example +// +// PMF_CONST_15373 = 7120.953125 7121 = 0x1bd1 +#define PMF_CONST_15373 ((pmf_logarithmic)0x1bd1) +// => converted back => 15373.98 +#endif diff --git a/src/RampCalculator.cpp b/src/RampCalculator.cpp new file mode 100644 index 0000000..d2c9b22 --- /dev/null +++ b/src/RampCalculator.cpp @@ -0,0 +1,181 @@ +#include "RampCalculator.h" + +#include + +#include "StepperISR.h" + +#ifdef TEST_TIMING +// This module has only one purpose: +// +// - To calculate for a given step the corresponding speed. +// +// With constant acceleration the relation between the steps s. acceleration a, +// time t and speed v will fulfill this equation during acceleration and +// deceleration: +// +// s = 0.5 * a * t² = 0.5 * v² / a +// +// Acceleration is just counting up the steps and deceleration counting down +// towards 0. +// +// From this equation the actual speed v at a given step can be calculated by: +// +// v = sqrt(2*s*a) +// +// For the pwm, the speed needs to be translated into time ticks T = n * +// timeticks = 1/v: +// +// n * timeticks = 1 / sqrt(2*s*a) +// +// If the µC would be fast, then the solution would be in floating point: +uint32_t calculate_ticks_v1(uint32_t steps, float acceleration) { + float n = TICKS_PER_S / sqrt(2.0 * steps * acceleration); + return n; +} +// Just this takes approx 92-4 = 88 us. Which is pretty slow. +// +// Little optimization improves to 87-4 = 83 us +uint32_t calculate_ticks_v2(uint32_t steps, float acceleration) { + float n = TICKS_PER_S / sqrt(2 * steps * acceleration); + return n; +} +// +// Precalculate TICKS_PER_S / sqrt(2 * acceleration) +// results in 80-4 = 76us +uint32_t calculate_ticks_v3(uint32_t steps, float pre_calc) { + float n = pre_calc / sqrt(steps); + return n; +} +// +// Using pmf_logarithmic improves to 22-4 = 18us, but less precision +uint32_t calculate_ticks_v4(uint32_t steps, uint32_t acceleration) { + pmf_logarithmic pmfl_a = pmfl_from(acceleration); + pmf_logarithmic pmfl_s = pmfl_from(steps); + pmf_logarithmic pmfl_res = + pmfl_divide(PMF_TICKS_PER_S, pmfl_sqrt(pmfl_multiply(pmfl_s, pmfl_a))); + uint32_t res = pmfl_to_u32(pmfl_res); + return res; +} +// +// Precalculating the acceleration related constant improves further to 12-4 = +// 8us. This measurement was actually done with ump_float. +// So this routine does not fit to that measurement anymore +// +uint32_t calculate_ticks_v5(uint32_t steps, pmf_logarithmic pre_calc) { + pmf_logarithmic pmfl_s = pmfl_from(steps); + pmf_logarithmic pmfl_res = pmfl_divide(pre_calc, pmfl_sqrt(pmfl_s)); + uint32_t res = pmfl_to_u32(pmfl_res); + return res; +} +// +// using the combined function yields actually no measureable improvement +uint32_t calculate_ticks_v6(uint32_t steps, pmf_logarithmic pre_calc) { + // pmf_logarithmic pmfl_s = pmfl_from(steps); + // pmf_logarithmic pmfl_res = pmfl_sqrt_after_divide(pre_calc, pmfl_s); + // uint32_t res = pmfl_to_u32(pmfl_res); + // return res; + return calculate_ticks_v5(steps, pre_calc); +} +// +// In order to increase the precision to more than 8 bits and avoid +// the penalty of slow float operations different approach will be used. +// +// in order to avoid division+sqrt operation, the following equation will be +// rearranged: +// +// n * timeticks = 1 / sqrt(2*s*a) +// +// into: +// +// n² * s = 1 / sqrt(2*a) / timeticks² +// +// with the right part being constant for a given acceleration a +// +// n² * s = const(a) +// +// This can be solved iteratively +// +// n_0 = 0 +// n_i +// n_[i+1] = { with n_i² * s <= const(a) < (n_i +// + mask_i)² * s +// n_i + mask_i +// +// mask_[i+1] = mask_i >> 1 +// +// In order to avoid the square calculation in each step several temporary +// values need to be calculated +// n_i² * s = f_i +// f_[i+1] = n_[i+1]² * s = { with same condition +// as before +// (n_i + mask_i)² * s +// +// For the lower case the calculation continues: +// +// = n_i² * s + 2 * n_i * mask_i * s + mask_i² * s +// +// = f_i + g_i + h_i +// +// With g_i and h_i defined like this: +// +// g_0 = 0 +// +// g_[i+1] = 2 * n_[i+1] * mask_[i+1] * s +// +// 2 * n_i * (mask_i >> 1) * s = g_i >> 1 +// = { +// 2 * (n_i + mask_i) * (mask_i >> 1) * s = +//(g_i +//>> 1) + (mask_i² * s) +// = (g_i >> 1) + h_i +// +// h_0 = mask_0 ² * s +// +// h_[i+1] = mask_[i+1]² * s +// = (mask_i >> 1)² * s +// = h_i >> 2 +// +// Actually this is a dead end, because below routines already needs 35us, +// while several variables below ought to be 32bit +uint32_t calculate_ticks_v7(uint32_t steps, pmf_logarithmic pre_calc) { + // initial values for i = 0 + uint16_t mask_i = 0x800; + uint16_t n_i = 0; + uint16_t f_i = 0; + uint16_t g_i = 0; + uint16_t h_i = steps; + uint16_t const_a = pre_calc; + + while (mask_i) { + uint16_t f_x = f_i + g_i + h_i; + // Serial.println(f_x); + if (f_x < const_a) { + n_i += mask_i; + f_i = f_x; + g_i >>= 1; + g_i += h_i; + h_i >>= 2; + mask_i >>= 1; + } else { + g_i >>= 1; + h_i >>= 2; + mask_i >>= 1; + } + } + return n_i; +} + +// This is used and requires 9.5us. +// +// Measure with: +// cd extras/tests/simavr_based +// make -C off_test_timing +// From the results subtract port B from port A measurement +uint32_t calculate_ticks_v8(uint32_t steps, pmf_logarithmic pre_calc) { + pmf_logarithmic pmfl_steps = pmfl_from(steps); + pmf_logarithmic pmfl_sqrt_steps = pmfl_sqrt(pmfl_steps); + pmf_logarithmic pmfl_res = pmfl_divide(pre_calc, pmfl_sqrt_steps); + uint32_t res = pmfl_to_u32(pmfl_res); + return res; +} +#endif diff --git a/src/RampCalculator.h b/src/RampCalculator.h new file mode 100644 index 0000000..b173456 --- /dev/null +++ b/src/RampCalculator.h @@ -0,0 +1,235 @@ +#ifndef RAMP_CALCULATOR_H +#define RAMP_CALCULATOR_H + +#include + +#include "PoorManFloat.h" +#include "fas_arch/common.h" + +#if (TICKS_PER_S == 16000000L) +#define PMF_TICKS_PER_S PMF_CONST_16E6 +#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 PMF_CONST_16E6_DIV_SQRT_OF_2 +#define PMF_ACCEL_FACTOR PMF_CONST_128E12 +#define US_TO_TICKS(u32) ((u32) * 16) +#define TICKS_TO_US(u32) ((u32) / 16) +#elif (TICKS_PER_S == 21000000L) +#define PMF_TICKS_PER_S PMF_CONST_21E6 +#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 PMF_CONST_21E6_DIV_SQRT_OF_2 +#define PMF_ACCEL_FACTOR PMF_CONST_2205E11 +#define US_TO_TICKS(u32) ((u32) * 21) +#define TICKS_TO_US(u32) ((u32) / 21) +#else +#define SUPPORT_PMF_TIMER_FREQ_VARIABLES +#define PMF_TICKS_PER_S pmfl_timer_freq +#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 pmfl_timer_freq_div_sqrt_of_2 +#define PMF_ACCEL_FACTOR pmfl_timer_freq_square_div_2 +// This overflows for approx. 1s at 40 MHz, only +#define US_TO_TICKS(u32) \ + ((uint32_t)((((uint32_t)((u32) * (TICKS_PER_S / 10000L))) / 100L))) + +// This calculation needs more work +#define TICKS_TO_US(u32) \ + ((uint32_t)((((uint32_t)((u32) / (TICKS_PER_S / 1000000L))) / 1L))) + +#endif + +#ifdef TEST_TIMING +uint32_t calculate_ticks_v1(uint32_t steps, float acceleration); +uint32_t calculate_ticks_v2(uint32_t steps, float acceleration); +uint32_t calculate_ticks_v3(uint32_t steps, float pre_calc); +uint32_t calculate_ticks_v4(uint32_t steps, uint32_t acceleration); +uint32_t calculate_ticks_v5(uint32_t steps, pmf_logarithmic pre_calc); +uint32_t calculate_ticks_v6(uint32_t steps, pmf_logarithmic pre_calc); +uint32_t calculate_ticks_v7(uint32_t steps, pmf_logarithmic pre_calc); +uint32_t calculate_ticks_v8(uint32_t steps, pmf_logarithmic pre_calc); +#endif + +struct ramp_parameters_s { + int32_t move_value; + uint32_t min_travel_ticks; + uint32_t s_h; + uint32_t s_jump; + pmf_logarithmic pmfl_accel; + bool apply : 1; // clear on read by stepper task. Triggers read ! + bool any_change : 1; // clear on read by stepper task + bool recalc_ramp_steps : 1; // clear on read by stepper task + bool valid_acceleration : 1; + bool valid_speed : 1; + bool move_absolute : 1; + bool keep_running : 1; + bool keep_running_count_up : 1; + + void init() { + move_value = 0; + valid_acceleration = false; + valid_speed = false; + apply = false; + any_change = false; + recalc_ramp_steps = false; + move_absolute = true; + keep_running = false; + keep_running_count_up = true; + s_h = 0; + s_jump = 0; + min_travel_ticks = 0; + } + inline void applyParameters() { + if (any_change) { + fasDisableInterrupts(); + apply = true; + fasEnableInterrupts(); + } + } + inline void setRunning(bool count_up) { + fasDisableInterrupts(); + keep_running = true; + keep_running_count_up = count_up; + any_change = true; + apply = true; + fasEnableInterrupts(); + } + inline void setTargetPosition(int32_t pos) { + fasDisableInterrupts(); + move_value = pos; + move_absolute = true; + keep_running = false; + keep_running_count_up = true; + any_change = true; + apply = true; + fasEnableInterrupts(); + } + inline void setTargetRelativePosition(int32_t move) { + fasDisableInterrupts(); + if (any_change && !move_absolute) { + move_value += move; + } else { + move_value = move; + } + move_absolute = false; + keep_running = false; + keep_running_count_up = true; + any_change = true; + apply = true; + fasEnableInterrupts(); + } + inline void setCubicAccelerationSteps(uint32_t s_cubic_steps) { + if (s_h != s_cubic_steps) { + fasDisableInterrupts(); + s_h = s_cubic_steps; + recalc_ramp_steps = true; + any_change = true; + fasEnableInterrupts(); + } + } + inline void setSpeedInTicks(uint32_t min_step_ticks) { + if (!valid_speed || (min_travel_ticks != min_step_ticks)) { + fasDisableInterrupts(); + min_travel_ticks = min_step_ticks; + valid_speed = true; + any_change = true; + fasEnableInterrupts(); + } + } + inline void setAcceleration(int32_t accel) { + pmf_logarithmic new_pmfl_accel = pmfl_from((uint32_t)accel); + if (!valid_acceleration || (pmfl_accel != new_pmfl_accel)) { + fasDisableInterrupts(); + valid_acceleration = true; + any_change = true; + recalc_ramp_steps = true; + pmfl_accel = new_pmfl_accel; + fasEnableInterrupts(); + } + } + inline void setJumpStart(uint32_t jump_step) { s_jump = jump_step; } + inline int8_t checkValidConfig() const { + if (!valid_speed) { + return MOVE_ERR_SPEED_IS_UNDEFINED; + } + if (!valid_acceleration) { + return MOVE_ERR_ACCELERATION_IS_UNDEFINED; + } + return MOVE_OK; + } +}; + +struct ramp_config_s { + struct ramp_parameters_s parameters; + + // These three variables are derived + uint32_t max_ramp_up_steps; + pmf_logarithmic pmfl_ticks_h; + pmf_logarithmic cubic; + + void init() { parameters.init(); } + inline void update() { + if (parameters.s_h > 0) { + pmf_logarithmic pmfl_s_h = pmfl_from(parameters.s_h); + // 1/cubic = sqrt(3/2 * a) / s_h^(1/6) / TICKS_PER_S + // = sqrt(3/2 * a / s_h^(1/3)) / TICKS_PER_S + // cubic = TICKS_PER_S / sqrt(s_h^(1/3) / (3/2 * a)) + cubic = pmfl_multiply(PMF_CONST_3_DIV_2, parameters.pmfl_accel); + cubic = pmfl_sqrt(pmfl_divide(pmfl_pow_div_3(pmfl_s_h), cubic)); + cubic = pmfl_multiply(PMF_TICKS_PER_S, cubic); + + // calculate_ticks(s_h) + pmfl_ticks_h = pmfl_divide(cubic, pmfl_pow_2_div_3(pmfl_s_h)); + } else { + pmfl_ticks_h = PMF_CONST_MAX; + } + max_ramp_up_steps = calculate_ramp_steps(parameters.min_travel_ticks); + if (max_ramp_up_steps == 0) { + max_ramp_up_steps = 1; + } +#ifdef TEST + printf("MAX_RAMP_UP_STEPS=%d from %d ticks\n", max_ramp_up_steps, + parameters.min_travel_ticks); +#endif + } + + uint32_t calculate_ticks(uint32_t steps) const { + // s = 1/2 * a * t^2 + // 2*a*s = (a*t)^2 = v^2 = (TICKS_PER_S/ticks)^2 + // ticks = TICKS_PER_S / sqrt(2*a*s) + // ticks = TICKS_PER_S/sqrt(2) / sqrt(a*s) + if (steps >= parameters.s_h) { + steps -= (parameters.s_h + 2) >> 2; + pmf_logarithmic pmfl_steps = pmfl_from(steps); + pmf_logarithmic pmfl_steps_mul_accel = + pmfl_multiply(pmfl_steps, parameters.pmfl_accel); + pmf_logarithmic pmfl_sqrt_steps_mul_accel = + pmfl_sqrt(pmfl_steps_mul_accel); + pmf_logarithmic pmfl_res = + pmfl_divide(PMF_TICKS_PER_S_DIV_SQRT_OF_2, pmfl_sqrt_steps_mul_accel); + uint32_t res = pmfl_to_u32(pmfl_res); + return res; + } + // ticks = cubic / s^(2/3) + pmf_logarithmic pmfl_steps = pmfl_from(steps); + pmf_logarithmic pmfl_res = pmfl_divide(cubic, pmfl_pow_2_div_3(pmfl_steps)); + uint32_t res = pmfl_to_u32(pmfl_res); + return res; + } + uint32_t calculate_ramp_steps(uint32_t ticks) const { + // pmfl is in range -64..<64 due to shift by 1 + // pmfl_ticks is in range 0..<32 + // pmfl_accel is in range 0..<32 + // PMF_ACCEL_FACTOR is approx. 47 for 16 Mticks/s + // pmfl_ticks squared is in range 0..<64 + pmf_logarithmic pmfl_ticks = pmfl_from(ticks); + if (pmfl_ticks <= pmfl_ticks_h) { + pmf_logarithmic pmfl_inv_accel2 = + pmfl_divide(PMF_ACCEL_FACTOR, parameters.pmfl_accel); + uint32_t steps = + pmfl_to_u32(pmfl_divide(pmfl_inv_accel2, pmfl_square(pmfl_ticks))); + steps += (parameters.s_h + 2) >> 2; + return steps; + } + // s = (cubic/ticks)^(3/2) + pmf_logarithmic pmfl_res = pmfl_divide(cubic, pmfl_ticks); + pmfl_res = pmfl_pow_3_div_2(pmfl_res); + uint32_t steps = pmfl_to_u32(pmfl_res); + return steps; + } +}; +#endif diff --git a/src/RampConstAcceleration.cpp b/src/RampConstAcceleration.cpp new file mode 100644 index 0000000..7bc7266 --- /dev/null +++ b/src/RampConstAcceleration.cpp @@ -0,0 +1,529 @@ +#include + +#include "FastAccelStepper.h" +#include "StepperISR.h" + +#include "RampConstAcceleration.h" +#include "fas_arch/common.h" + +#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES +static pmf_logarithmic pmfl_timer_freq; +static pmf_logarithmic pmfl_timer_freq_div_sqrt_of_2; +static pmf_logarithmic pmfl_timer_freq_square_div_2; +#endif + +void init_ramp_module() { +#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES + pmfl_timer_freq = pmfl_from((uint32_t)TICKS_PER_S); + pmfl_timer_freq_div_sqrt_of_2 = + pmfl_shr(pmfl_multiply(pmfl_timer_freq, pmfl_timer_freq)); + pmfl_timer_freq_square_div_2 = pmfl_shr(pmfl_square(pmfl_timer_freq)); +#endif +} + +//************************************************************************************************* + +// #define TRACE +#ifdef TRACE +#define TRACE_OUTPUT(x) Serial.print(x) +#else +#define TRACE_OUTPUT(x) +#endif + +#ifdef TEST +void print_ramp_state(uint8_t this_state) { + switch (this_state & RAMP_DIRECTION_MASK) { + case RAMP_DIRECTION_COUNT_UP: + printf("+"); + break; + case RAMP_DIRECTION_COUNT_DOWN: + printf("-"); + break; + } + switch (this_state & RAMP_STATE_MASK) { + case RAMP_STATE_COAST: + printf("COAST"); + break; + case RAMP_STATE_ACCELERATE: + printf("ACC"); + break; + case RAMP_STATE_DECELERATE: + printf("DEC"); + break; + case RAMP_STATE_REVERSE: + printf("REVERSE"); + break; + } +} +#endif + +//************************************************************************************************* +void _getNextCommand(const struct ramp_ro_s *ramp, const struct ramp_rw_s *rw, + const struct queue_end_s *queue_end, + NextCommand *command) { + { + // If there is a pause from last step, then just output a pause + uint32_t pause_ticks = rw->pause_ticks_left; + if (pause_ticks > 0) { + if (pause_ticks > 65535) { + pause_ticks >>= 1; + pause_ticks = fas_min(pause_ticks, 65535); + } + command->command.ticks = pause_ticks; + command->command.steps = 0; + command->command.count_up = queue_end->count_up; + command->rw = *rw; + command->rw.pause_ticks_left -= pause_ticks; +#ifdef TEST + printf("add command pause ticks = %d remaining pause = %d\n", + pause_ticks, command->rw.pause_ticks_left); +#endif + return; + } + } + + bool count_up = queue_end->count_up; + + // check state for acceleration/deceleration or deceleration to stop + uint8_t this_state; + uint32_t remaining_steps; + bool need_count_up; + if (ramp->config.parameters.keep_running) { + need_count_up = ramp->config.parameters.keep_running_count_up; + remaining_steps = 0xfffffff; + } else { + // this can overflow, which is legal + int32_t delta = ramp->target_pos - queue_end->pos; + + if (delta == 0) { + // this case can happen on overshoot. So reverse current direction + need_count_up = !count_up; + } else { + need_count_up = delta > 0; + } + remaining_steps = fas_abs(delta); + } + +#ifdef TRACE + if (remaining_steps == performed_ramp_up_steps) { + Serial.print('='); + } else if (remaining_steps > performed_ramp_up_steps) { + uint32_t dx = remaining_steps - performed_ramp_up_steps; + if (dx < 10) { + char ch = '0' + dx; + Serial.print(ch); + Serial.print('x'); + } + } else { + uint32_t dx = performed_ramp_up_steps - remaining_steps; + if (dx < 10) { + char ch = '0' + dx; + Serial.print(ch); + Serial.print('y'); + } + } +#endif + + // If not moving, then use requested direction + uint32_t performed_ramp_up_steps = rw->performed_ramp_up_steps; + if (performed_ramp_up_steps == 0) { + count_up = need_count_up; + } + + if ((remaining_steps == 0) && (performed_ramp_up_steps <= 1)) { + command->command.ticks = 0; + command->rw.pause_ticks_left = 0; + command->rw.ramp_state = RAMP_STATE_IDLE; + command->rw.curr_ticks = TICKS_FOR_STOPPED_MOTOR; +#ifdef TEST + puts("ramp complete"); +#endif + return; + } + + uint32_t curr_ticks = rw->curr_ticks; + + // Forward planning of 2ms or more on slow speed. + uint16_t planning_steps; + if (curr_ticks < TICKS_PER_S / 1000) { + uint16_t ps = TICKS_PER_S / 500; + ps /= (uint16_t)curr_ticks; + planning_steps = ps; + } else { + planning_steps = 1; + } + uint16_t orig_planning_steps = planning_steps; + + // In case of force stop just run down the ramp + if (count_up != need_count_up) { + // On direction change, do reversing + this_state = RAMP_STATE_REVERSE; + remaining_steps = performed_ramp_up_steps; + } else { + // If come here, then direction is same as current movement + if (remaining_steps == performed_ramp_up_steps) { + this_state = RAMP_STATE_DECELERATE; + // remaining_steps = performed_ramp_up_steps; + } else if (remaining_steps < performed_ramp_up_steps) { + // We will overshoot + TRACE_OUTPUT('O'); + this_state = RAMP_STATE_REVERSE; + remaining_steps = performed_ramp_up_steps; + } else if (ramp->config.parameters.min_travel_ticks < rw->curr_ticks) { + this_state = RAMP_STATE_ACCELERATE; + if (rw->curr_ticks < 2 * MIN_CMD_TICKS) { + // special consideration needed, that invalid commands are not generated + // + // possible coast steps is divided by 4: 1 part acc, 2 part coast, 1 + // part dec + uint32_t possible_coast_steps = + (remaining_steps - performed_ramp_up_steps) >> 2; + if (possible_coast_steps > 0) { + // curr_ticks is not necessarily correct due to speed increase + uint32_t coast_time = possible_coast_steps * rw->curr_ticks; + if (coast_time < 2 * MIN_CMD_TICKS) { + TRACE_OUTPUT('l'); + this_state = RAMP_STATE_COAST; +#ifdef TEST + printf("high speed coast %d %d\n", possible_coast_steps, + remaining_steps - performed_ramp_up_steps); +#endif + } + } + if (planning_steps > remaining_steps - performed_ramp_up_steps) { + this_state = RAMP_STATE_DECELERATE; + } + } else if (remaining_steps - performed_ramp_up_steps < + 2 * planning_steps) { + if (curr_ticks != TICKS_FOR_STOPPED_MOTOR) { + this_state = RAMP_STATE_COAST; + planning_steps = remaining_steps - performed_ramp_up_steps; + } + } + } else if (ramp->config.parameters.min_travel_ticks > rw->curr_ticks) { + TRACE_OUTPUT('d'); + this_state = RAMP_STATE_DECELERATE; + if (performed_ramp_up_steps <= planning_steps) { + if (performed_ramp_up_steps > 0) { + planning_steps = performed_ramp_up_steps; + } else { + planning_steps = 1; + } + } + } else { + TRACE_OUTPUT('c'); + this_state = RAMP_STATE_COAST; + uint32_t possible_coast_steps = remaining_steps - performed_ramp_up_steps; + if (possible_coast_steps < 2 * planning_steps) { + planning_steps = possible_coast_steps; + if (curr_ticks < MIN_CMD_TICKS) { + uint32_t cmd_ticks = curr_ticks * planning_steps; + if (cmd_ticks < MIN_CMD_TICKS) { + this_state = RAMP_STATE_DECELERATE; + } + } + } + } + } + if (remaining_steps == 0) { // This implies performed_ramp_up_steps == 0 + command->command.ticks = 0; + command->rw.pause_ticks_left = 0; + command->rw.ramp_state = RAMP_STATE_IDLE; + command->rw.curr_ticks = TICKS_FOR_STOPPED_MOTOR; +#ifdef TEST + puts("ramp complete"); +#endif + return; + } + + // Guarantee here: + // remaining_steps > 0 + // remaining_steps >= performed_ramp_up_steps + // remaining_steps > performed_ramp_up_steps, in COAST + // performed_ramp_up_steps can be 0 + // planning_steps >= 1 +#ifdef TEST + assert(remaining_steps > 0); + assert(remaining_steps >= performed_ramp_up_steps); + assert((remaining_steps > performed_ramp_up_steps) || + (this_state != RAMP_STATE_COAST)); + assert(planning_steps > 0); +#endif + +#ifdef TEST + printf("prus=%d planning_steps=%d remaining_steps=%d force_stop=%d\n", + performed_ramp_up_steps, planning_steps, remaining_steps, + ramp->force_stop); +#endif + uint32_t d_ticks_new; + { + if (this_state & RAMP_STATE_ACCELERATING_FLAG) { + TRACE_OUTPUT('A'); + // do not overshoot ramp down start + // + // seems to be not necessary, as consideration already done above + uint32_t dec_steps = remaining_steps - performed_ramp_up_steps; + if (dec_steps < 512) { + // Only allow half, cause the steps accelerating need to decelerate, too + auto dec_steps_u16 = (uint16_t)dec_steps; + dec_steps_u16 /= 2; + // Perhaps it would be better to coast instead + // consideration has been done above already + if (dec_steps_u16 < orig_planning_steps) { + planning_steps = dec_steps_u16; + if (planning_steps == 0) { + planning_steps = 1; + } +#ifdef TEST + printf("Change planning_steps=%u\n", planning_steps); +#endif + } + } + + uint32_t rs = performed_ramp_up_steps + planning_steps; + d_ticks_new = ramp->config.calculate_ticks(rs); +#ifdef TEST + printf("Calculate d_ticks_new=%u from ramp steps=%u\n", d_ticks_new, rs); +#endif + + // if acceleration is very high, then d_ticks_new can be lower than + // min_travel_ticks + if (d_ticks_new < ramp->config.parameters.min_travel_ticks) { + d_ticks_new = ramp->config.parameters.min_travel_ticks; + } + } else if (this_state & RAMP_STATE_DECELERATING_FLAG) { + TRACE_OUTPUT('D'); + if (performed_ramp_up_steps == 1) { + d_ticks_new = ramp->config.parameters.min_travel_ticks; +#ifdef TEST + printf("Set d_ticks_new=%u to min_travel_ticks\n", d_ticks_new); +#endif + } else { + uint32_t rs; + if (performed_ramp_up_steps <= planning_steps) { + rs = planning_steps; + } else { + rs = performed_ramp_up_steps - planning_steps; + } + d_ticks_new = ramp->config.calculate_ticks(rs); +#ifdef TEST + printf("Calculate d_ticks_new=%d from ramp steps=%d for deceleration\n", + d_ticks_new, rs); +#endif + // If the ramp generator cannot decelerate by going down the ramp, + // then we need to clip the new d_ticks to the min travel ticks + // This is for issue #150 + uint32_t min_travel_ticks = ramp->config.parameters.min_travel_ticks; + if ((rs == 1) && (min_travel_ticks > d_ticks_new)) { + d_ticks_new = min_travel_ticks; +#ifdef TEST + printf("Clip d_ticks_new=%d for deceleration\n", d_ticks_new); +#endif + } + } + } else { + TRACE_OUTPUT('C'); + d_ticks_new = rw->curr_ticks; + // do not overshoot ramp down start + uint32_t coast_steps = remaining_steps - performed_ramp_up_steps; + if (coast_steps < 256) { + uint16_t coast_steps_u16 = coast_steps; + if (coast_steps_u16 < 2 * orig_planning_steps) { + planning_steps = coast_steps_u16; + } + } +#ifdef TEST + printf("planning steps=%d remaining steps=%d\n", planning_steps, + remaining_steps); +#endif + } + } + + // The above plannings_steps evaluation uses curr_ticks, + // but new_ticks can be lower and so the command time not sufficient + if (d_ticks_new < MIN_CMD_TICKS) { + uint32_t cmd_ticks = d_ticks_new * planning_steps; + if (cmd_ticks < MIN_CMD_TICKS) { + // using planning_steps and d_ticks_new would create invalid commands + + uint16_t steps = MIN_CMD_TICKS + d_ticks_new - 1; + steps /= ((uint16_t)d_ticks_new); +#ifdef TEST + printf("new steps=%d d_ticks_new=%d\n", steps, d_ticks_new); +#endif + if (steps >= remaining_steps) { +#ifdef TEST + printf( + "command time too low, with increased steps will reach ramp end\n"); +#endif + planning_steps = remaining_steps; + this_state = RAMP_STATE_DECELERATE; + } + + // if we are at ramp end, then reduce speed + steps = fas_max(planning_steps, steps); + if (2 * steps >= remaining_steps) { + d_ticks_new = MIN_CMD_TICKS + remaining_steps - 1; + d_ticks_new /= remaining_steps; + planning_steps = remaining_steps; + this_state = RAMP_STATE_DECELERATE; +#ifdef TEST + printf("command time too low, so reduce speed to %d ticks\n", + d_ticks_new); +#endif + } else { +#ifdef TEST + printf("Increase planning steps %d => %d due to command time\n", + planning_steps, steps); +#endif + planning_steps = steps; + + // do we need to decelerate in order to not overshoot ? + if (remaining_steps < performed_ramp_up_steps + planning_steps) { + this_state = RAMP_STATE_DECELERATE; + + // and now the speed is actually too high.... + } + } + } + } + + // perform clipping with current ticks + uint32_t next_ticks = d_ticks_new; + if (curr_ticks != TICKS_FOR_STOPPED_MOTOR) { + if (this_state & RAMP_STATE_ACCELERATING_FLAG) { + next_ticks = fas_min(next_ticks, curr_ticks); + } else if (this_state & RAMP_STATE_DECELERATING_FLAG) { + // CLIPPING: avoid reduction unless curr_ticks indicates stopped motor + // Issue #25: root cause is, that curr_ticks can be + // TICKS_FOR_STOPPED_MOTOR for the case, that queue is emptied before + // the next command is issued + next_ticks = fas_max(next_ticks, curr_ticks); + // if (this_state != RAMP_STATE_DECELERATE) { + // next_ticks = fas_max(next_ticks, ramp->config.min_travel_ticks); + // } + } + } +#ifdef TEST + assert(next_ticks > 0); +#endif +#ifdef TEST + if (next_ticks != d_ticks_new) { + printf( + "Clipping result d_ticks_new=%d => next_ticks=%d with curr_ticks=%d " + "state=%d\n", + d_ticks_new, next_ticks, curr_ticks, this_state); + } +#endif + +#ifdef TEST + printf("planning steps=%d remaining steps=%d prus=%d\n", planning_steps, + remaining_steps, performed_ramp_up_steps); +#endif + // Number of steps to execute with limitation to min 1 and max remaining steps + uint16_t steps = planning_steps; + steps = fas_min(steps, remaining_steps); // This could be problematic + steps = fas_max(steps, 1); + steps = fas_min(255, steps); + + // Check if pauses need to be added. If yes, reduce next_ticks and calculate + // pause_ticks_left + uint32_t pause_ticks_left; + if (next_ticks > 65535) { + steps = 1; + pause_ticks_left = next_ticks; + next_ticks >>= 1; + next_ticks = fas_min(next_ticks, 65535); + pause_ticks_left -= next_ticks; + } else { + pause_ticks_left = 0; + } + + // determine performed_ramp_up_steps after command enqueued + if (this_state & RAMP_STATE_ACCELERATING_FLAG) { + performed_ramp_up_steps += steps; + } else if (this_state & RAMP_STATE_DECELERATING_FLAG) { + if (performed_ramp_up_steps < steps) { + // This can occur with performed_ramp_up_steps = 0 and steps = 1 +#ifdef TEST + printf("prus=%d steps=%d\n", performed_ramp_up_steps, steps); +// assert((performed_ramp_up_steps == 0) && (steps == 1)); +#endif + // based on above assumption actually obsolete + performed_ramp_up_steps = 0; + } else { + uint32_t max_ramp_up_steps = ramp->config.max_ramp_up_steps; +#ifdef TEST + printf("prus=%d steps=%d max_prus=%d\n", performed_ramp_up_steps, steps, + max_ramp_up_steps); +#endif + if (performed_ramp_up_steps > max_ramp_up_steps) { +#ifdef TEST + printf("reduce prus=%d by %d\n", performed_ramp_up_steps, steps); +#endif + performed_ramp_up_steps -= steps; + } else if ((performed_ramp_up_steps >= max_ramp_up_steps) && + (max_ramp_up_steps + steps <= remaining_steps) && + (performed_ramp_up_steps - steps < max_ramp_up_steps)) { + // Speed was too high. So we need to ensure to not overshoot + // deceleration +#ifdef TEST + printf("clip prus=%d to %d\n", performed_ramp_up_steps, + max_ramp_up_steps); +#endif + performed_ramp_up_steps = max_ramp_up_steps; + next_ticks = ramp->config.parameters.min_travel_ticks; + } else { + if (remaining_steps > performed_ramp_up_steps) { + if (remaining_steps - performed_ramp_up_steps < steps) { + performed_ramp_up_steps = remaining_steps - steps; +#ifdef TEST + printf("set prus to remaining steps %d minus %d steps\n", + remaining_steps, steps); +#endif + } + } else { + performed_ramp_up_steps -= steps; +#ifdef TEST + printf("reduce prus by %d steps\n", steps); +#endif + } + } + } + } + + if (count_up) { + this_state |= RAMP_DIRECTION_COUNT_UP; + } else { + this_state |= RAMP_DIRECTION_COUNT_DOWN; + } + + command->command.ticks = next_ticks; + command->command.steps = steps; + command->command.count_up = count_up; + + command->rw.ramp_state = this_state; + command->rw.performed_ramp_up_steps = performed_ramp_up_steps; + command->rw.pause_ticks_left = pause_ticks_left; + command->rw.curr_ticks = pause_ticks_left + next_ticks; + +#ifdef TEST + printf( + "pos@queue_end=%d remaining=%u prus=%u planning steps=%d " + "last_ticks=%u travel_ticks=%u ", + queue_end->pos, remaining_steps, performed_ramp_up_steps, planning_steps, + rw->curr_ticks, ramp->config.parameters.min_travel_ticks); + print_ramp_state(this_state); + printf("\n"); + printf( + "add command Steps=%u ticks=%u Target pos=%u " + "Remaining steps=%u, planning_steps=%u, " + "d_ticks_new=%u, pause_left=%u\n", + steps, next_ticks, ramp->target_pos, remaining_steps, planning_steps, + d_ticks_new, pause_ticks_left); + if ((this_state & RAMP_STATE_MASK) == RAMP_STATE_ACCELERATE) { + assert(pause_ticks_left + next_ticks >= + ramp->config.parameters.min_travel_ticks); + } +#endif +} diff --git a/src/RampConstAcceleration.h b/src/RampConstAcceleration.h new file mode 100644 index 0000000..4870d79 --- /dev/null +++ b/src/RampConstAcceleration.h @@ -0,0 +1,79 @@ +#ifndef RAMP_CONST_ACCELERATION_H +#define RAMP_CONST_ACCELERATION_H + +#include "fas_arch/common.h" + +struct ramp_ro_s { + struct ramp_config_s config; + uint32_t target_pos; + bool force_stop : 1; + bool force_immediate_stop : 1; + bool incomplete_immediate_stop : 1; + inline void init() { + config.init(); + force_stop = false; + force_immediate_stop = false; + } + inline int32_t targetPosition() { return target_pos; } + inline void setTargetPosition(int32_t pos) { target_pos = pos; } + inline void advanceTargetPositionWithinInterruptDisabledScope(int32_t delta) { + target_pos += delta; + } + inline void immediateStop() { force_immediate_stop = true; } + inline bool isImmediateStopInitiated() { return force_immediate_stop; } + inline void clearImmediateStop() { force_immediate_stop = false; } + inline void initiateStop() { force_stop = true; } + inline bool isStopInitiated() { return force_stop; } + inline void setKeepRunning() { config.parameters.keep_running = true; } + inline bool isRunningContinuously() { return config.parameters.keep_running; } +}; + +struct ramp_rw_s { + volatile uint8_t ramp_state; + // the speed is linked on both ramp slopes to this variable as per (if no + // cubic ramp) + // s = v²/2a => v = sqrt(2*a*s) + uint32_t performed_ramp_up_steps; + // Are the ticks stored of the last previous step, if pulse time requires + // more than one command + uint32_t pause_ticks_left; + // Current ticks for ongoing step + uint32_t curr_ticks; + inline void stopRamp() { + ramp_state = RAMP_STATE_IDLE; // this prevents fill_queue to be executed + pause_ticks_left = 0; + performed_ramp_up_steps = 0; + curr_ticks = TICKS_FOR_STOPPED_MOTOR; +#ifdef TEST + printf("stopRamp() called\n"); +#endif + } + inline void init() { stopRamp(); } + inline uint8_t rampState() { + // reading one byte is atomic + return ramp_state; + } + inline void startRampIfNotRunning(uint32_t s_jump) { +#ifdef TEST + printf("startRampIfNotRunning(%d) called\n", s_jump); +#endif + // called with interrupts disabled + if (ramp_state == RAMP_STATE_IDLE) { + curr_ticks = TICKS_FOR_STOPPED_MOTOR; + // ramp_state value is significant to start the ramp generator. + // so initialize curr_ticks before + ramp_state = RAMP_STATE_ACCELERATE; + } + } +}; + +class NextCommand { + public: + struct stepper_command_s command; + struct ramp_rw_s rw; // new _rw, if command has been queued +}; + +void init_ramp_module(); +void _getNextCommand(const struct ramp_ro_s *ramp, const struct ramp_rw_s *rw, + const struct queue_end_s *queue_end, NextCommand *command); +#endif diff --git a/src/RampGenerator.cpp b/src/RampGenerator.cpp new file mode 100644 index 0000000..fe51a2b --- /dev/null +++ b/src/RampGenerator.cpp @@ -0,0 +1,274 @@ +#include + +#include "FastAccelStepper.h" +#include "RampGenerator.h" +#include "StepperISR.h" + +// This define in order to not shoot myself. +#ifndef TEST +#define printf DO_NOT_USE_PRINTF +#define puts DO_NOT_USE_PRINTF +#endif + +void RampGenerator::init() { + _parameters.init(); + _ro.init(); + _rw.init(); + init_ramp_module(); +} +int8_t RampGenerator::setAcceleration(int32_t accel) { + if (accel <= 0) { + return -1; + } + acceleration = (uint32_t)accel; + _parameters.setAcceleration(accel); + return 0; +} +void RampGenerator::applySpeedAcceleration() { + if (!_ro.isImmediateStopInitiated()) { + _parameters.applyParameters(); + } +} +int8_t RampGenerator::startRun(bool countUp) { + uint8_t res = _parameters.checkValidConfig(); + if (res != MOVE_OK) { + return res; + } + _ro.force_stop = false; + _parameters.setRunning(countUp); + _rw.startRampIfNotRunning(_parameters.s_jump); +#ifdef DEBUG + char buf[256]; + sprintf(buf, "Ramp data: curr_ticks = %lu travel_ticks = %lu\n", + _rw.curr_ticks, _parameters.min_travel_ticks); + Serial.println(buf); +#endif + return MOVE_OK; +} + +void RampGenerator::_startMove(bool position_changed) { + _ro.force_stop = false; + + if (position_changed) { + // Only start the ramp generator, if the target position is different + _rw.startRampIfNotRunning(_parameters.s_jump); + } + +#ifdef TEST + printf("Ramp data: go to %s %d curr_ticks = %u travel_ticks = %u prus=%u\n", + _parameters.move_absolute ? "ABS" : "REL", _parameters.move_value, + _rw.curr_ticks, _ro.config.parameters.min_travel_ticks, + _rw.performed_ramp_up_steps); +#endif +#ifdef DEBUG + char buf[256]; + sprintf(buf, + "Ramp data: go to = %s %ld curr_ticks = %lu travel_ticks = %lu " + "prus=%lu\n", + _parameters.move_absolute ? "ABS" : "REL", _parameters.move_value, + _rw.curr_ticks, _ro.config.parameters.min_travel_ticks, + _rw.performed_ramp_up_steps); + Serial.println(buf); +#endif +} + +int8_t RampGenerator::moveTo(int32_t position, + const struct queue_end_s *queue_end) { + uint8_t res = _parameters.checkValidConfig(); + if (res != MOVE_OK) { + return res; + } + int32_t curr_target; + if (isRampGeneratorActive() && !_ro.config.parameters.keep_running) { + curr_target = _ro.target_pos; + } else { + curr_target = queue_end->pos; + } + inject_fill_interrupt(1); + _parameters.setTargetPosition(position); + _startMove(curr_target != position); + inject_fill_interrupt(2); + return MOVE_OK; +} +int8_t RampGenerator::move(int32_t move, const struct queue_end_s *queue_end) { + uint8_t res = _parameters.checkValidConfig(); + if (res != MOVE_OK) { + return res; + } + _parameters.setTargetRelativePosition(move); + _startMove(move != 0); + return MOVE_OK; +} +void RampGenerator::advanceTargetPosition(int32_t delta, + const struct queue_end_s *queue) { + // called with interrupts disabled + _ro.target_pos += delta; +} + +void RampGenerator::afterCommandEnqueued(NextCommand *command) { +#ifdef TEST + printf( + "after Command Enqueued: performed ramp up steps = %u, pause left = %u, " + "curr_ticks = %u\n", + command->rw.performed_ramp_up_steps, command->rw.pause_ticks_left, + command->rw.curr_ticks); +#endif + _rw = command->rw; +} +void RampGenerator::getNextCommand(const struct queue_end_s *queue_end, + NextCommand *command) { + // we are running in higher priority than the application + // so we can just read the config without disable interrupts + // copy consistent ramp state + bool was_keep_running = _ro.config.parameters.keep_running; + if (_parameters.apply) { + _ro.config.parameters = _parameters; + _parameters.apply = false; + _parameters.any_change = false; + _parameters.move_value = 0; + _parameters.move_absolute = false; + _parameters.recalc_ramp_steps = false; + _ro.config.update(); + // if new move command,then reset any immediate stop flag + if (_ro.isImmediateStopInitiated()) { + if (_ro.config.parameters.move_absolute) { + if (_ro.target_pos != (uint32_t)_ro.config.parameters.move_value) { + _ro.clearImmediateStop(); + } + } else if (_ro.config.parameters.move_value != 0) { + _ro.clearImmediateStop(); + } + } +#ifdef DEBUG + Serial.print("new command: move="); + if (_ro.config.parameters.keep_running) { + Serial.print(_ro.config.parameters.keep_running_count_up ? "forward" + : "backward"); + } else { + Serial.print(_ro.config.parameters.move_absolute ? '@' : 'r'); + Serial.print(_ro.config.parameters.move_value); + } + if (_ro.config.parameters.valid_speed) { + Serial.print(" v="); + Serial.print(_ro.config.parameters.min_travel_ticks); + } + if (_ro.config.parameters.valid_acceleration) { + Serial.print(" a="); + Serial.print(pmfl_to_u32(_ro.config.parameters.pmfl_accel)); + } + if (_ro.config.parameters.recalc_ramp_steps) { + Serial.print(" recalc"); + } + Serial.println(); +#endif + } + + fasDisableInterrupts(); + struct queue_end_s qe = *queue_end; + fasEnableInterrupts(); + + uint32_t curr_ticks = _rw.curr_ticks; + if (curr_ticks == TICKS_FOR_STOPPED_MOTOR) { + // just started + uint32_t s_jump = _ro.config.parameters.s_jump; + if (s_jump != 0) { + uint32_t ticks = _ro.config.calculate_ticks(s_jump); + if (ticks < _ro.config.parameters.min_travel_ticks) { + s_jump = _ro.config.calculate_ramp_steps( + _ro.config.parameters.min_travel_ticks); + } + } + _rw.performed_ramp_up_steps = s_jump; + _ro.config.parameters.recalc_ramp_steps = false; + } + + // If the acceleration has changed, recalculate the ramp up/down steps, + // which is the equivalent to the current speed. + // Even if the acceleration value is constant, the calculated value + // can deviate due to precision or clipping effect + if (_ro.config.parameters.recalc_ramp_steps) { + uint32_t performed_ramp_up_steps = + _ro.config.calculate_ramp_steps(curr_ticks); +#ifdef TEST + printf("Recalculate performed_ramp_up_steps from %d to %d from %d ticks\n", + _rw.performed_ramp_up_steps, performed_ramp_up_steps, curr_ticks); +#endif +#ifdef DEBUG + char buf[100]; + sprintf( + buf, + "Recalculate performed_ramp_up_steps from %lu to %lu from %lu ticks\n", + _rw.performed_ramp_up_steps, performed_ramp_up_steps, curr_ticks); + Serial.print(buf); +#endif + _rw.performed_ramp_up_steps = performed_ramp_up_steps; + } + + if (_ro.force_stop) { + _ro.config.parameters.keep_running = false; + uint32_t target_pos = qe.pos; + if (qe.count_up) { + target_pos += _rw.performed_ramp_up_steps; + } else { + target_pos -= _rw.performed_ramp_up_steps; + } +#ifdef TEST + printf("Force stop: adjust target position from %d to %d\n", _ro.target_pos, + target_pos); +#endif +#ifdef DEBUG + char buf[100]; + sprintf(buf, "Force stop: adjust target position from %ld to %ld\n", + _ro.target_pos, target_pos); + Serial.print(buf); +#endif + _ro.target_pos = target_pos; + } else if (_ro.config.parameters.any_change) { + // calculate new target position + _ro.config.parameters.any_change = false; + if (_ro.config.parameters.keep_running) { + } else if (_ro.config.parameters.move_absolute) { + _ro.target_pos = _ro.config.parameters.move_value; + } else { + uint32_t target_pos = _ro.target_pos; + if (was_keep_running) { + // target_pos is not valid for keep running + target_pos = qe.pos; + } + _ro.target_pos = target_pos + _ro.config.parameters.move_value; + } +#ifdef DEBUG + Serial.print("target pos="); + Serial.println(_ro.target_pos); +#endif + } + + // This line is the root cause for the failure in issue #280 + //_ro.force_stop = false; + + // clear recalc flag + _ro.config.parameters.recalc_ramp_steps = false; + + if (_ro.isImmediateStopInitiated()) { + // no more commands + command->command.ticks = 0; + _ro.clearImmediateStop(); + _ro.target_pos = qe.pos; + command->rw.stopRamp(); + return; + } + _getNextCommand(&_ro, &_rw, &qe, command); +} +int32_t RampGenerator::getCurrentAcceleration() { + switch (_rw.rampState() & + (RAMP_STATE_ACCELERATING_FLAG | RAMP_STATE_DECELERATING_FLAG | + RAMP_DIRECTION_MASK)) { + case RAMP_STATE_ACCELERATING_FLAG | RAMP_DIRECTION_COUNT_UP: + case RAMP_STATE_DECELERATING_FLAG | RAMP_DIRECTION_COUNT_DOWN: + return acceleration; + case RAMP_STATE_DECELERATING_FLAG | RAMP_DIRECTION_COUNT_UP: + case RAMP_STATE_ACCELERATING_FLAG | RAMP_DIRECTION_COUNT_DOWN: + return -acceleration; + } + return 0; +} diff --git a/src/RampGenerator.h b/src/RampGenerator.h new file mode 100644 index 0000000..5ebaf3a --- /dev/null +++ b/src/RampGenerator.h @@ -0,0 +1,118 @@ +#ifndef RAMP_GENERATOR_H +#define RAMP_GENERATOR_H + +#include "FastAccelStepper.h" +#include "RampCalculator.h" +#include "RampConstAcceleration.h" +#include "fas_arch/common.h" + +class FastAccelStepper; + +#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES +extern pmf_logarithmic pmfl_timer_freq; +extern pmf_logarithmic pmfl_timer_freq_div_sqrt_of_2; +extern pmf_logarithmic pmfl_timer_freq_square_div_2; +#endif + +class RampGenerator { + private: + struct ramp_parameters_s _parameters; + + // The ro variables are those, which are only read from _getNextCommand(). + // The rw variables are only read and written by _getNextCommand() and + // commandEnqueued() Reading ro variables is safe in application + struct ramp_ro_s _ro; + struct ramp_rw_s _rw; + + public: + uint32_t acceleration; + inline uint8_t rampState() { return _rw.rampState(); } + void init(); + inline int32_t targetPosition() { return _ro.targetPosition(); } + inline void setTargetPosition(int32_t pos) { _ro.setTargetPosition(pos); } + void advanceTargetPosition(int32_t delta, const struct queue_end_s *queue); + inline void setSpeedInTicks(uint32_t min_step_ticks) { + _parameters.setSpeedInTicks(min_step_ticks); + } + inline uint32_t getSpeedInUs() { + return _parameters.min_travel_ticks / (TICKS_PER_S / 1000000); + } + inline uint32_t getSpeedInTicks() { return _parameters.min_travel_ticks; } + uint32_t divForMilliHz(uint32_t f) { + uint32_t base = (uint32_t)250 * TICKS_PER_S; + uint32_t res = base / f; + base -= res * f; + base <<= 2; + res <<= 2; + base += f / 2; // add rounding + res += base / f; + return res; + } + uint32_t divForHz(uint32_t f) { + uint32_t base = TICKS_PER_S; + base += f / 2; // add rounding + uint32_t res = base / f; + return res; + } + inline uint32_t getSpeedInMilliHz() { + if (_parameters.min_travel_ticks == 0) { + return 0; + } + return divForMilliHz(getSpeedInTicks()); + } + int8_t setAcceleration(int32_t accel); + inline uint32_t getAcceleration() { return acceleration; } + inline void setLinearAcceleration(uint32_t linear_acceleration_steps) { + _parameters.setCubicAccelerationSteps(linear_acceleration_steps); + } + inline void setJumpStart(uint32_t jump_step) { + _parameters.setJumpStart(jump_step); + } + int32_t getCurrentAcceleration(); + inline bool hasValidConfig() { + return _parameters.checkValidConfig() == MOVE_OK; + } + void applySpeedAcceleration(); + int8_t move(int32_t move, const struct queue_end_s *queue); + int8_t moveTo(int32_t position, const struct queue_end_s *queue); + int8_t startRun(bool countUp); + inline void forceStop() { _ro.immediateStop(); } + inline void initiateStop() { _ro.initiateStop(); } + inline bool isStopping() { + return _ro.isStopInitiated() && isRampGeneratorActive(); + } + inline bool isRampGeneratorActive() { return rampState() != RAMP_STATE_IDLE; } + inline uint32_t stepsToStop() { + fasDisableInterrupts(); + uint32_t v = _rw.performed_ramp_up_steps; + fasEnableInterrupts(); + return v; + } + inline void stopRamp() { _rw.stopRamp(); } + inline void setKeepRunning() { _ro.setKeepRunning(); } + inline bool isRunningContinuously() { return _ro.isRunningContinuously(); } + 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; + fasEnableInterrupts(); + return ticks; + } + inline uint32_t getCurrentPeriodInUs() { + return TICKS_TO_US(getCurrentPeriodInTicks()); + } + + private: + void _startMove(bool position_changed); +}; + +#endif diff --git a/src/StepperISR.cpp b/src/StepperISR.cpp new file mode 100644 index 0000000..c7baba1 --- /dev/null +++ b/src/StepperISR.cpp @@ -0,0 +1,349 @@ +#include + +#include "StepperISR.h" + +int8_t StepperQueue::addQueueEntry(const struct stepper_command_s* cmd, + bool start) { + // Just to check if, if the struct has the correct size + // if (sizeof(entry) != 6 * QUEUE_LEN) { + // return -1; + //} + if (!isReadyForCommands()) { + return AQE_DEVICE_NOT_READY; + } + if (cmd == NULL) { + if (start && !isRunning()) { + if (next_write_idx == read_idx) { + return AQE_ERROR_EMPTY_QUEUE_TO_START; + } + startQueue(); + } + return AQE_OK; + } + if (isQueueFull()) { + return AQE_QUEUE_FULL; + } + uint16_t period = cmd->ticks; + uint8_t steps = cmd->steps; + // generation discrepancy: pc vs target + // after Command Enqueued: performed ramp up steps = 1, pause left = 0, + // curr_ticks = 11320 after Command Enqueued: performed ramp up steps = 3, + // pause left = 0, curr_ticks = 6536 after Command Enqueued: performed ramp up + // steps = 7, pause left = 0, curr_ticks = 4276 after Command Enqueued: + // performed ramp up steps = 14, pause left = 0, curr_ticks = 3024 after + // Command Enqueued: performed ramp up steps = 24, pause left = 0, curr_ticks + // = 2312 after Command Enqueued: performed ramp up steps = 17, pause left = + // 0, curr_ticks = 3412 after Command Enqueued: performed ramp up steps = 8, + // pause left = 0, curr_ticks = 4004 after Command Enqueued: performed ramp up + // steps = 1, pause left = 0, curr_ticks = 11320 + // + // esp32 mcpwm and rmt: + // 0/268435455us/4294967295:1:11320X:2:6536X:4:4276X:7:3024X:10:2312X:13:3412X:9:4004X:7:11320X:1:11320X + // + // + // Failed 10steps: + // :0:40000X:0:40000X:1:56608X:0:56608X:1:40032X:0:40032X:1:65344X:1:56608X:1:50624X:1:56608X:1:65344X:1:40032X:0:40032X:1:56608X:0:56608X:1:56608X:0:56608X + // + // Failed seq_07 + // :0:40000X:0:40000X:1:56608X:0:56608X:1:40032X:0:40032X:1:65344X:1:56608X:1:50624X + // :1:46208X:1:42784X:1:40032X:1:37728X:1:35808X:1:34112X:1:32672X:1:31376X:1:30256X:1:29216X:1:28304X:1:27440X:1:26672X:1:25968X:1:25312X:1:24688X:1:24128X:1:23616X:1:23104X:1:22640X:1:22192X:1:21776X:1:21392X:1:21024X:1:20656X:1:20320X:1:20016X:1:19696X:1:19408X:1:19152X:1:18864X:1:18608X:1:18352X:1:18144X:1:17904X:1:17680X:1:17472X:1:17280X:1:17056X:1:16880X:1:16704X:1:16512X:1:16336X:1:16160X:1:16016X:1:15840X:2:15544X:2:15272X:2:14984X:2:14744X:2:14488X:2:14256X:2:14040X:2:13840X:2:13632X + // :2:13432X:2:13248X:2:13072X:2:12896X:2:12736X:2:12584X:2:12432X:2:12280X:2:12128X:2:12000X:2:11872X:2:11744X:2:11616X:2:11488X:2:11384X:2:11264X:2:11152X:2:11048X:2:10944X:2:10840X:2:10736X:2:10656X:3:10512X:3:10384X:3:10248X:3:10120X:3:10008X:3:9888X:3:9784X:3:9680X:3:9576X:3:9472X:3:9368X:3:9280X:3:9176X:3:9096X:3:9008X:3:8928X:3:8840X:3:8760X:3:8688X:3:8600X:3:8528X:3:8464X:3:8392X:3:8328X:3:8256X:3:8192X:3:8124X:3:8060X:3:8008X:3:7944X:4:7864X:4:7792X:4:7720X:4:7648X:4:7584X:4:7512X:4:7452X:4:7384X:4:7324X + // :4:7264X:4:7204X:4:7148X:4:7088X:4:7040X:4:6984X:4:6928X + +// #define TRACE +#ifdef TRACE + Serial.print(':'); + Serial.print(start ? "START" : "PUSH"); + Serial.print(':'); + Serial.print(cmd->count_up ? 'U' : 'D'); + Serial.print(':'); + Serial.print(steps); + Serial.print(':'); + Serial.print(period); + Serial.print('X'); +#endif + + uint32_t command_rate_ticks = period; + if (steps > 1) { + command_rate_ticks *= steps; + } + if (command_rate_ticks < MIN_CMD_TICKS) { + return AQE_ERROR_TICKS_TOO_LOW; + } + + uint8_t wp = next_write_idx; + struct queue_entry* e = &entry[wp & QUEUE_LEN_MASK]; + bool dir = (cmd->count_up == dirHighCountsUp); + bool toggle_dir = false; +#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN) + bool repeat_entry = false; +#endif + if (dirPin != PIN_UNDEFINED) { + if ((isQueueEmpty() && !isRunning()) && + ((dirPin & PIN_EXTERNAL_FLAG) == 0)) { + // set the dirPin here. Necessary with shared direction pins + digitalWrite(dirPin, dir); +#ifdef ARDUINO_ARCH_SAM + delayMicroseconds(30); // Make sure the driver has enough time to see + // the dir pin change +#endif + queue_end.dir = dir; + } else { + toggle_dir = (dir != queue_end.dir); +#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN) + if (toggle_dir && (dirPin & PIN_EXTERNAL_FLAG)) { + repeat_entry = toggle_dir; + toggle_dir = false; + } +#endif + } + } + e->steps = steps; +#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN) + e->repeat_entry = repeat_entry; + e->dirPinState = dir; +#endif + e->toggle_dir = toggle_dir; + e->countUp = cmd->count_up ? 1 : 0; + e->moreThanOneStep = steps > 1 ? 1 : 0; + e->hasSteps = steps > 0 ? 1 : 0; + e->ticks = period; + struct queue_end_s next_queue_end = queue_end; +#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16) + e->start_pos_last16 = (uint32_t)next_queue_end.pos & 0xffff; +#endif + next_queue_end.pos = next_queue_end.pos + (cmd->count_up ? steps : -steps); +#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16) + e->end_pos_last16 = (uint32_t)next_queue_end.pos & 0xffff; +#endif + next_queue_end.dir = dir; + next_queue_end.count_up = cmd->count_up; + + // Advance write pointer + fasDisableInterrupts(); + if (!ignore_commands) { + if (isReadyForCommands()) { + next_write_idx = next_write_idx + 1; + queue_end = next_queue_end; + } else { + fasEnableInterrupts(); + return AQE_DEVICE_NOT_READY; + } + } + fasEnableInterrupts(); + + if (!isRunning() && start) { + // stepper is not yet running and start is requested +#ifdef TRACE + Serial.print('S'); +#endif + startQueue(); + } +#ifdef TRACE + else { + // WHY IS start 0 in seq_01c + Serial.print(isRunning() ? 'R' : 'T'); + Serial.print(start ? '1' : '0'); + Serial.println('N'); + } +#endif + return AQE_OK; +} + +int32_t StepperQueue::getCurrentPosition() { + fasDisableInterrupts(); + uint32_t pos = (uint32_t)queue_end.pos; + uint8_t rp = read_idx; + bool is_empty = (rp == next_write_idx); + struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK]; +#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16) + uint16_t pos_last16 = e->end_pos_last16; +#endif +#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16) + uint16_t pos_last16 = e->start_pos_last16; +#endif + uint8_t steps = e->steps; +#if defined(SUPPORT_ESP32) + // pulse counter should go max up to 255 with perhaps few pulses overrun, so + // this conversion is safe + int16_t done_p = (int16_t)_getPerformedPulses(); +#endif + fasEnableInterrupts(); +#if defined(SUPPORT_ESP32) + if (done_p == 0) { + // fix for possible race condition described in issue #68 + fasDisableInterrupts(); + rp = read_idx; + is_empty = (rp == next_write_idx); + e = &entry[rp & QUEUE_LEN_MASK]; + pos_last16 = e->start_pos_last16; + steps = e->steps; + done_p = (int16_t)_getPerformedPulses(); + fasEnableInterrupts(); + } +#endif + if (!is_empty) { + int16_t adjust = 0; + + uint16_t pos16 = pos & 0xffff; + uint8_t transition = ((pos16 >> 12) & 0x0c) | (pos_last16 >> 14); + switch (transition) { + case 0: // 00 00 + case 5: // 01 01 + case 10: // 10 10 + case 15: // 11 11 + break; + case 1: // 00 01 + case 6: // 01 10 + case 11: // 10 11 + case 12: // 11 00 + pos += 0x4000; + break; + case 4: // 01 00 + case 9: // 10 01 + case 14: // 11 10 + case 3: // 00 11 + pos -= 0x4000; + break; + case 2: // 00 10 + case 7: // 01 11 + case 8: // 10 00 + case 13: // 11 01 + break; // TODO: ERROR + } + pos = (int32_t)((pos & 0xffff0000) | pos_last16); + + if (steps != 0) { + if (e->countUp) { +#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16) + adjust = -steps; +#endif +#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16) + adjust = done_p; +#endif + } else { +#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16) + adjust = steps; +#endif +#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16) + adjust = -done_p; +#endif + } + pos += adjust; + } + } + return pos; +} + +uint32_t StepperQueue::ticksInQueue() { + fasDisableInterrupts(); + uint8_t rp = read_idx; + uint8_t wp = next_write_idx; + fasEnableInterrupts(); + if (wp == rp) { + return 0; + } + uint32_t ticks = 0; + rp++; // ignore currently processed entry + while (wp != rp) { + struct queue_entry* e = &entry[rp++ & QUEUE_LEN_MASK]; + ticks += e->ticks; + uint8_t steps = e->steps; + if (steps > 1) { + uint32_t tmp = e->ticks; + tmp *= steps - 1; + ticks += tmp; + } + } + return ticks; +} + +bool StepperQueue::hasTicksInQueue(uint32_t min_ticks) { + fasDisableInterrupts(); + uint8_t rp = read_idx; + uint8_t wp = next_write_idx; + fasEnableInterrupts(); + if (wp == rp) { + return false; + } + rp++; // ignore currently processed entry + while (wp != rp) { + struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK]; + uint32_t tmp = e->ticks; + uint8_t steps = fas_max(e->steps, (uint8_t)1); + tmp *= steps; + if (tmp >= min_ticks) { + return true; + } + min_ticks -= tmp; + rp++; + } + return false; +} + +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) { + 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 true; + } + if (wp != ++rp) { + if (entry[rp & QUEUE_LEN_MASK].hasSteps) { + return true; + } + } + } + return false; +} + +void StepperQueue::_initVars() { + dirPin = PIN_UNDEFINED; +#ifndef TEST + max_speed_in_ticks = TICKS_PER_S / 1000; // use a default value 1_000 steps/s +#else + max_speed_in_ticks = + TICKS_PER_S / 50000; // use a default value 50_000 steps/s +#endif + ignore_commands = false; + read_idx = 0; + next_write_idx = 0; + queue_end.dir = true; + queue_end.count_up = true; + queue_end.pos = 0; + dirHighCountsUp = true; +#if defined(ARDUINO_ARCH_AVR) + _isRunning = false; + _noMoreCommands = false; +#endif +#if defined(SUPPORT_ESP32) + _isRunning = false; + _nextCommandIsPrepared = false; +#endif +#if defined(SUPPORT_ESP32_RMT) + _rmtStopped = true; +#endif +#if defined(ARDUINO_ARCH_SAM) + _hasISRactive = false; + // we cannot clear the PWM interrupt when switching to a pause, but we'll + // get a double interrupt if we do nothing. So this tells us that on a + // transition from a pulse to a pause to skip the next interrupt. + _pauseCommanded = false; + timePWMInterruptEnabled = 0; +#endif +#if defined(TEST) + _isRunning = false; +#endif +} diff --git a/src/StepperISR.h b/src/StepperISR.h new file mode 100644 index 0000000..087e40f --- /dev/null +++ b/src/StepperISR.h @@ -0,0 +1,202 @@ +#include + +#include "FastAccelStepper.h" +#include "fas_arch/common.h" + +// Here are the global variables to interface with the interrupts + +// These variables control the stepper timing behaviour +#define QUEUE_LEN_MASK (QUEUE_LEN - 1) + +struct queue_entry { + uint8_t steps; // if 0, then the command only adds a delay + uint8_t toggle_dir : 1; + uint8_t countUp : 1; + uint8_t moreThanOneStep : 1; + uint8_t hasSteps : 1; +#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN) + // if repeat_entry==1, then this entry shall be repeated. + // This mechanism only works for pauses (steps == 0) + // Used for external direction pin + uint8_t repeat_entry : 1; + uint8_t dirPinState : 1; +#endif + uint16_t ticks; +#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16) + uint16_t end_pos_last16; +#endif +#if defined(SUPPORT_QUEUE_ENTRY_START_POS_U16) + uint16_t start_pos_last16; +#endif +}; + +class StepperQueue { + public: + struct queue_entry entry[QUEUE_LEN]; + + // In case of forceStopAndNewPosition() the adding of commands has to be + // temporarily suspended + volatile bool ignore_commands; + volatile uint8_t read_idx; // ISR stops if readptr == next_writeptr + volatile uint8_t next_write_idx; + bool dirHighCountsUp; + uint8_t dirPin; + + // a word to isRunning(): + // if isRunning() is false, then the _QUEUE_ is not running. + // + // For esp32 this does NOT mean, that the HW is finished. + // The timer is still counting down to zero until it stops at 0. + // But there will be no interrupt to process another command. + // So the queue requires startQueue() again + // + // Due to the rmt version of esp32, there has been the needed to + // provide information, that device is not yet ready for new commands. + // This has been called isReadyForCommands(). + // + +#if defined(SUPPORT_ESP32) + volatile bool _isRunning; + bool _nextCommandIsPrepared; + inline bool isRunning() { return _isRunning; } + bool isReadyForCommands() const; + bool use_rmt; + uint8_t _step_pin; + uint16_t _getPerformedPulses() const; +#endif +#ifdef SUPPORT_ESP32_MCPWM_PCNT + const void* driver_data; +#endif +#ifdef SUPPORT_ESP32_RMT + RMT_CHANNEL_T channel; + bool _rmtStopped; +#if ESP_IDF_VERSION_MAJOR == 4 + bool bufferContainsSteps[2]; +#else + bool lastChunkContainsSteps; + rmt_encoder_handle_t _tx_encoder; +#endif +#endif +#if defined(SUPPORT_DIR_PIN_MASK) + 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 _noMoreCommands; + volatile bool _isRunning; + inline bool isRunning() { return _isRunning; } + inline bool isReadyForCommands() { return true; } + enum channels channel; +#endif +#if defined(SUPPORT_SAM) + uint8_t _step_pin; + uint8_t _queue_num; + void* driver_data; + volatile bool _hasISRactive; + bool isRunning(); + bool _connected; + inline bool isReadyForCommands() { return true; } + volatile bool _pauseCommanded; + volatile uint32_t timePWMInterruptEnabled; +#endif +#if defined(TEST) + volatile bool _isRunning; + inline bool isReadyForCommands() { return true; } + inline bool isRunning() { return _isRunning; } +#endif + + struct queue_end_s queue_end; + uint16_t max_speed_in_ticks; + + void init(uint8_t queue_num, uint8_t step_pin); + inline uint8_t queueEntries() { + fasDisableInterrupts(); + uint8_t rp = read_idx; + uint8_t wp = next_write_idx; + fasEnableInterrupts(); + inject_fill_interrupt(0); + return (uint8_t)(wp - rp); + } + inline bool isQueueFull() { return queueEntries() == QUEUE_LEN; } + inline bool isQueueEmpty() { return queueEntries() == 0; } +#if defined(SUPPORT_EXTERNAL_DIRECTION_PIN) + inline bool isOnRepeatingEntry() { + return entry[read_idx & QUEUE_LEN_MASK].repeat_entry == 1; + } + inline uint8_t dirPinState() { + return entry[read_idx & QUEUE_LEN_MASK].dirPinState; + } + inline void clearRepeatingFlag() { + entry[read_idx & QUEUE_LEN_MASK].repeat_entry = 0; + } +#endif + + int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start); + int32_t getCurrentPosition(); + uint32_t ticksInQueue(); + bool hasTicksInQueue(uint32_t min_ticks); + bool getActualTicksWithDirection(struct actual_ticks_s* speed); + + inline uint16_t getMaxSpeedInTicks() { return max_speed_in_ticks; } + + // startQueue is always called + void startQueue(); + void forceStop(); + void _initVars(); + void connect(); + void disconnect(); + +#ifdef SUPPORT_ESP32_MCPWM_PCNT + bool isReadyForCommands_mcpwm_pcnt(); + void init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin); + void startQueue_mcpwm_pcnt(); + void forceStop_mcpwm_pcnt(); + uint16_t _getPerformedPulses_mcpwm_pcnt(); + void connect_mcpwm_pcnt(); + void disconnect_mcpwm_pcnt(); +#endif +#ifdef SUPPORT_ESP32_RMT + bool isReadyForCommands_rmt() const; + void init_rmt(uint8_t channel_num, uint8_t step_pin); + void startQueue_rmt(); +#if ESP_IDF_VERSION_MAJOR == 4 + void stop_rmt(bool both); +#else + bool _channel_enabled; +#endif + void forceStop_rmt(); + static uint16_t _getPerformedPulses_rmt(); + void connect_rmt(); + void disconnect_rmt(); +#endif + void setDirPin(uint8_t dir_pin, bool _dirHighCountsUp) { + dirPin = dir_pin; + dirHighCountsUp = _dirHighCountsUp; +#if defined(SUPPORT_DIR_PIN_MASK) + if ((dir_pin != PIN_UNDEFINED) && ((dir_pin & PIN_EXTERNAL_FLAG) == 0)) { + _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 + } +#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); +}; + +extern StepperQueue fas_queue[NUM_QUEUES]; + +void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core); diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp new file mode 100644 index 0000000..c8c8d50 --- /dev/null +++ b/src/StepperISR_avr.cpp @@ -0,0 +1,359 @@ +#if defined(ARDUINO_ARCH_AVR) +#include "AVRStepperPins.h" +#include "StepperISR.h" + +// T is the timer module number 0,1,2,3... +// X is the Channel name A or B +// +// BV1 Bv0 +// Output one is 1 1 +// Output zero is 1 0 +// Toggle is 0 1 +// Disconnect is 0 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 +#ifdef DISABLE +#define Stepper_Toggle(T, X) \ + 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)) +#define Stepper_IsOne(T, X) \ + ((TCCR##T##A & (_BV(COM##T##X##0) | _BV(COM##T##X##1))) == \ + (_BV(COM##T##X##0) | _BV(COM##T##X##1))) +#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); \ + } + +#ifdef SIMAVR_TIME_MEASUREMENT +#define prepareISRtimeMeasurement() DDRB |= 0x18 +#define enterStepperISR() PORTB |= 0x08 +#define exitStepperISR() PORTB ^= 0x08 +#define enterFillQueueISR() PORTB |= 0x10 +#define exitFillQueueISR() PORTB ^= 0x10 +#elif defined(SIMAVR_TIME_MEASUREMENT_QUEUE) +#define prepareISRtimeMeasurement() DDRB |= 0x10 +#define enterStepperISR() \ + {} +#define exitStepperISR() \ + {} +#define enterFillQueueISR() PORTB |= 0x10 +#define exitFillQueueISR() PORTB ^= 0x10 +#else +#define prepareISRtimeMeasurement() \ + {} +#define enterStepperISR() \ + {} +#define exitStepperISR() \ + {} +#define enterFillQueueISR() \ + {} +#define exitFillQueueISR() \ + {} +#endif + +#ifdef SUPPORT_EXTERNAL_DIRECTION_PIN +#define TEST_NOT_REPEATING_ENTRY (e->repeat_entry == 0) +#else +#define TEST_NOT_REPEATING_ENTRY (0 == 0) +#endif + +#define ForceCompare(T, X) TCCR##T##C = _BV(FOC##T##X) +#define DisableCompareInterrupt(T, X) TIMSK##T &= ~_BV(OCIE##T##X) +#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) \ + { \ + /* Set WGMn3:0 to all zero => Normal mode */ \ + TCCR##T##A &= ~(_BV(WGM##T##1) | _BV(WGM##T##0)); \ + TCCR##T##B &= ~(_BV(WGM##T##3) | _BV(WGM##T##2)); \ + /* Set prescaler to 1 */ \ + TCCR##T##B = \ + (TCCR##T##B & ~(_BV(CS##T##2) | _BV(CS##T##1) | _BV(CS##T##0))) | \ + _BV(CS##T##0); \ + } +#define EnableOverflowInterrupt(T) TIMSK##T |= _BV(TOIE##T) +#define DisableOverflowInterrupt(T) TIMSK##T &= ~_BV(TOIE##T) + +// this is needed to give the background task isr access to engine +static FastAccelStepperEngine* fas_engine = NULL; + +// Here are the global variables to interface with the interrupts +StepperQueue fas_queue[NUM_QUEUES]; + +#define AVR_INIT(T, CHANNEL) \ + { \ + /* Disconnect stepper on next compare event */ \ + Stepper_Disconnect(T, CHANNEL); \ + /* disable compare A interrupt */ \ + DisableCompareInterrupt(T, CHANNEL); \ + /* force compare to ensure disconnect */ \ + ForceCompare(T, CHANNEL); \ + /* Initialize timer for correct time base */ \ + ConfigureTimer(T); \ + /* ensure cyclic interrupt is running */ \ + EnableOverflowInterrupt(T); \ + } +void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { + prepareISRtimeMeasurement(); + _initVars(); + digitalWrite(step_pin, LOW); + pinMode(step_pin, OUTPUT); + if (step_pin == stepPinStepperA) { + channel = channelA; + AVR_INIT(FAS_TIMER_MODULE, A) + } + if (step_pin == stepPinStepperB) { + channel = channelB; + AVR_INIT(FAS_TIMER_MODULE, B) + } +#ifdef stepPinStepperC + if (step_pin == stepPinStepperC) { + channel = channelC; + AVR_INIT(FAS_TIMER_MODULE, C) + } +#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. +// +// 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_GEN(T, CHANNEL) AVR_STEPPER_ISR(T, CHANNEL) +AVR_STEPPER_ISR_GEN(FAS_TIMER_MODULE, A) +AVR_STEPPER_ISR_GEN(FAS_TIMER_MODULE, B) +#ifdef stepPinStepperC +AVR_STEPPER_ISR_GEN(FAS_TIMER_MODULE, C) +#endif + +// this is for cyclic task +#define AVR_CYCLIC_ISR(T) \ + ISR(TIMER##T##_OVF_vect) { \ + enterFillQueueISR(); \ + \ + /* disable OVF interrupt to avoid nesting */ \ + DisableOverflowInterrupt(T); \ + \ + /* enable interrupts for nesting */ \ + sei(); \ + \ + /* manage steppers */ \ + fas_engine->manageSteppers(); \ + \ + /* disable interrupts for exist ISR routine */ \ + cli(); \ + \ + /* enable OVF interrupt again */ \ + EnableOverflowInterrupt(T); \ + \ + exitFillQueueISR(); \ + } +#define AVR_CYCLIC_ISR_GEN(T) AVR_CYCLIC_ISR(T) +AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) + +#define GET_ENTRY_PTR(T, CHANNEL) \ + rp = fas_queue_##CHANNEL.read_idx; \ + e = &fas_queue_##CHANNEL.entry[rp & QUEUE_LEN_MASK]; + +#define AVR_START_QUEUE(T, CHANNEL) \ + /* 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); \ + } \ + /* clear interrupt flag */ \ + ClearInterruptFlag(T, CHANNEL); \ + /* enable compare interrupt */ \ + EnableCompareInterrupt(T, CHANNEL); \ + /* start */ \ + noInterrupts(); \ + SetTimerCompareRelative(T, CHANNEL, 10); \ + interrupts(); + +void StepperQueue::startQueue() { + uint8_t rp; + struct queue_entry* e; + + _isRunning = true; + switch (channel) { + case channelA: + GET_ENTRY_PTR(FAS_TIMER_MODULE, A) + PREPARE_DIRECTION_PIN(A) + AVR_START_QUEUE(FAS_TIMER_MODULE, A) + break; + case channelB: + GET_ENTRY_PTR(FAS_TIMER_MODULE, B) + PREPARE_DIRECTION_PIN(B) + AVR_START_QUEUE(FAS_TIMER_MODULE, B) + break; +#ifdef stepPinStepperC + case channelC: + GET_ENTRY_PTR(FAS_TIMER_MODULE, C) + PREPARE_DIRECTION_PIN(C) + AVR_START_QUEUE(FAS_TIMER_MODULE, C) + break; +#endif + } +} + +#define FORCE_STOP(T, CHANNEL) \ + { \ + /* disable compare interrupt */ \ + DisableCompareInterrupt(T, CHANNEL); \ + /* set to disconnect */ \ + Stepper_Disconnect(T, CHANNEL); \ + /* force compare to ensure disconnect */ \ + ForceCompare(T, CHANNEL); \ + } +void StepperQueue::forceStop() { + switch (channel) { + case channelA: + FORCE_STOP(FAS_TIMER_MODULE, A) + break; + case channelB: + FORCE_STOP(FAS_TIMER_MODULE, B) + break; +#ifdef stepPinStepperC + case channelC: + FORCE_STOP(FAS_TIMER_MODULE, C) + break; +#endif + } + _isRunning = false; + + // empty the queue + read_idx = next_write_idx; +} +void StepperQueue::connect() {} +void StepperQueue::disconnect() {} +bool StepperQueue::isValidStepPin(uint8_t step_pin) { + if (step_pin == stepPinStepperA) { + return true; + } + if (step_pin == stepPinStepperB) { + return true; + } +#ifdef stepPinStepperC + if (step_pin == stepPinStepperC) { + return true; + } +#endif + return false; +} + +int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { + if (step_pin == stepPinStepperA) { + return 0; + } + if (step_pin == stepPinStepperB) { + return 1; + } +#ifdef stepPinStepperC + if (step_pin == stepPinStepperC) { + return 2; + } +#endif + return -1; +} +void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { + // commit 9577e9bfd4b9a6cf1ad830901c00c8b129a62aee fails + // test_sd_04_timing_2560 as timer 3 reaches 40us. + // This includes port set/clear for timer measurement. + // So choose 20kHz + // + // check if Issue_152.ino, the interrupt need 14us. + // So 70000 Steps/s is too high. + if (steppers == 1) { + max_speed_in_ticks = TICKS_PER_S / 50000; + } else if (steppers == 2) { + max_speed_in_ticks = 426; + } else { + max_speed_in_ticks = TICKS_PER_S / 20000; + } +} + +void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core) { + fas_engine = engine; +} +#endif diff --git a/src/StepperISR_due.cpp b/src/StepperISR_due.cpp new file mode 100644 index 0000000..5fc6943 --- /dev/null +++ b/src/StepperISR_due.cpp @@ -0,0 +1,719 @@ +#include "StepperISR.h" +// #ifndef ARDUINO_ARCH_SAM +// #define ARDUINO_ARCH_SAM +// #endif +// #define KEEP_SCORE +#if defined(ARDUINO_ARCH_SAM) +const bool enabled = false; +uint32_t junk = 0; +/* +I went through a ton of iterations of trying to make this work sd desired. +It seems quite simple, set the pulse period of the PWM generator, start it, +stop it after e->ticks number of ticks. Except for that its never easy. + +We start with how to detect that e->ticks has happened. I found that +regaurdless of output mode, the PIO device is always connected and receiving +inputs! This means I can use it to trigger on a pulse being sent! But this +means I will get an interrupt for every single pulse. Thats not ideal, and I +wasn't sure this would be able to operate fast enough in this mode. I +actually did what I could to get into the PWM shutdown as fast as possible. +However, it seems like it would still send one more pulse every time. It +wouldn't register a full rise on my scope, and it probably....wouldn't have +triggered a pulse on a stepper driver...but probably isn't good enough. No +matter what I did, I wasn't fast enough. So I found that using the PIO to +disconnect the output of the pulse generator was nearly instantaneous. So +fast that to keep the interrupt on a rising edge, I need to delay a few +microseconds to keep from chopping off the pulse! In fact, my driver says +it needs 5 microsecond pulses. I'm currently sending 10 microsencond pulses. +With a 5 microsecond delay before shutting off, the pulse is only 7 +microseconds in length! The performance of the code seems quite good! So +not only is the overhead of the interrupts low enough to chop pulses off, +its good enough to easily manage at least one stepper. (I need to make code +more generic and make ISRs for the other ports and test with more). + +The last "trick" that was necessary was a delay in starting the pulse generator +after setting the direction pin. Currently my test just spins the motor 1 +revolution then spins it the other way. So every stop needs a delay. The +delay is 100% necessary for setting the direction pin, so it cannot simply be +removed! + +With that, I've spun this motor to about 1600 RPM. No matter how slow the +acceleration, it seems to stop somewhere just above that. So the code can do +more than my nema23 test motor :) + +Last "trick". I didn't think about the fact that at 21MHz with a 16 bit +counter, the longest the system can delay is around 3ms. Fortunately, this was +not lost on the original author. the hasSteps or steps==0 case is when you +want to pause for a time (specified in the ticks member). To pause, I can +disconnect the PWM peripheral, and generate PWM interrupts instead of using +the PIO interrupts. It may seem like a good idea to just use the PWM +interrupts. However, this arrangement actually eliminates a branch, and keeps +pause code seprate from pulse code. On top of that, I get a true count of +pulses output, rather than pulses attempted to be generated even if it was +disconnected. Its actually more accurate than my scope, as if I am in +process of disconnecting the PWM peripheral, I still get a PIO interrupt, but +on the scope, I see only a small .8-.9V pulse. Its possible that pulse could +be detected, though not likely. Better to eliminate it if possible (which it +was!) The branch is eliminated by not needing to check if it was the cmpr +register interrupt, or the period update interrupt. The PWM interrupt is +always the period update, and the PIO is always the cmp. One branch isn't +much, but its something! + + +I think I'm going about this all wrong. I think I can set the AB reg to the PWM +generator, but set the output to 0 and still get interrupts on the PIO when a +pulse would have happened if we hadn't overridden the output to 0. Look into +this... +*/ +inline void disconnectPWMPeriphal(Pio* port, uint8_t pin, uint32_t channelMask); + +typedef struct _PWMCHANNELMAPPING { + uint8_t pin; + uint32_t channel; + Pio* port; + uint32_t channelMask; +} PWMCHANNELMAPPING; + +#define NUM_PWM_CHANNELS 8 +uint8_t TimerChannel_Map[NUM_PWM_CHANNELS]; + +uint8_t numChannels = 0; + +// this is needed to give the background task isr access to engine +static FastAccelStepperEngine* fas_engine = NULL; + +// Here are the global variables to interface with the interrupts +#if defined(KEEP_SCORE) // If you want to count how many pulses we detect... +volatile uint32_t totalPulsesDetected[NUM_QUEUES]; +volatile uint32_t totalSteps[NUM_QUEUES]; +inline void IncrementQueue(int queue) { totalPulsesDetected[queue]++; } +inline void AddToTotalSteps(int queue, uint32_t steps) { + totalSteps[queue] += steps; +} +#else +#define IncrementQueue(Q) +#define AddToTotalSteps(Q, N) +#endif +bool channelsUsed[8] = {false, false, false, false, false, false, false, false}; +StepperQueue fas_queue[NUM_QUEUES]; +PWMCHANNELMAPPING gChannelMap[NUM_QUEUES]; + +void TC5_Handler() { + uint32_t SR0 = TC1->TC_CHANNEL[2].TC_SR; + if (SR0 & TC_SR_CPCS) { + // We're going to use TC5 as its not connected to any pins on the DUE to run + // our engine... We only get in here on an RC compare, so if we're in here, + // run manage steppers... + fas_engine->manageSteppers(); + TC1->TC_CHANNEL[2].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN; // CLKEN and SWTRG + } +} +// Handy function to strobe a pin, and will do so conditionally based on an int +// and a desired int. I was using PWM channels to decide what to strobe, thus +// the variable names. If you want to just strobe a pin leave the ints blank +void strobePin(uint32_t pin, uint32_t desiredChannel = 0, + uint32_t channel = 0) { + if (channel == desiredChannel) { + digitalWrite(pin, HIGH); + digitalWrite(pin, LOW); + } +} +void PWM_Handler(void) { + // The datasheet is INCREDIBLY confusing here again. It writes of interrupts + // on comparison registers, and tells us that each channel has comparison + // registers, and proceeds to tell us that interrupts are only for synchronous + // channels. This is contradictory at best, useless at worst! The truth is + // that Atmel's tech writers are atrocious. The PWM unit containts two almost + // distinct peripherals. Theres the PWM generator with its 8 channels and + // individual register and clock selection. In addition to this, attached to + // channel 0 only is the "comparison" peripheral. Even though the main PWM + // channel registers will compare values against the clock, do not call them + // comparison registers. They are Period and Duty cycle registers. Thats all + // they can be used for, so don't think of them as "comparison", especially + // since there are "comparison" registers which are only good for comparison + // attached to channel 0. The comparison unit is used to generate "special" + // interrupts, all controleld by the IXR2 registers (where X can be E, D, M, + // or S to make up the entire register set). The fact they are tied to + // channel 0 is why there is the confusion mention of "synchronous" in the + // datasheet, despite synchronous being used to mean tying main PWM output + // channels to the same clock so that either their left sides are aligned or + // their centers are aligned (depending on a register setting). + // + // The IXR1 (again E,D,M,and S) registers are for the main PWM channels + // only, and do not make use of the comparison unit. They have 2 possible + // sources, channel fault, and "clock event" was used because there are two + // possible modes for the clock to be run in, but in the description, this + // isn't explained. It really ought to be called "Period Reset". Effectively + // this is what it is whether in left aligned or center aligned modes. In + // the timing diagrams on page 980 of the data sheet, its clear that it + // responds directly to the period register, not the clock itself. The clock + // also responds to the period register, so naming of this interrupt/event + // would make *FAR* more sense to be tied to the register than effects it the + // most! So without checking the datasheet, its an interrupt when the period + // expires. In center aligned the counter counts up until it matches the + // period register, then back to 0 to make "one full period". When it hits 0 + // the "Counter Event" interrupt for the channel "CHIDx" is fired, again, + // think of it as Period Reset interrupt. In left Aligned the counter counts + // up until it reaches the period register and then resets to 0, and fires + // the "CHIDx" interrupt" (period reset interrupt). That was way more + // description than should have been necessary, but I wanted to clear up the + // nearly worthless datasheet. It is supposed to be a reference, you're not + // supposed to need to read the entire thing, correleate all discrepencies, + // determine the truth, then apply, it should be a reference meaning each + // section should be able to be read/understood individually. Intel does a + // fantastic job of this with IA32 Software Developer's Manual! Atmel could + // take a lesson! Even AMD with Radeon GPU documenation was better than this! + + // Anyhow, this ISR without the complexity of IXR2 registers is actually quite + // simple. Figure out which channel had a period reset event, and service it. + // Since we have to read the entire ISR1 register, we need to make sure we + // don't have multiple period update events. It should be rare, but it can + // happen! Then, because we only use this for pausing, and each pause has one + // period cycle, we simply advance rp, and decide what to do from there (pause + // again or switch to outputting steps). Pretty easy :) + + uint32_t sr = PWM_INTERFACE->PWM_ISR1; + // uint32_t sr2 = PWM_INTERFACE->PWM_ISR2; + uint8_t leading_zeros; + sr = sr & 0xFF; // Ignore the faults, they will screw this up, and we + // shouldn't be getting faults... + while ((leading_zeros = __CLZ(sr)) < 32) { + uint8_t channel = 32 - leading_zeros - 1; + uint32_t mask = (1 << channel); + sr = sr & (~mask); + uint8_t queue_num = TimerChannel_Map[channel]; + if (channelsUsed[channel] == false) continue; + StepperQueue* q = &fas_queue[queue_num]; + // if (q->_skipNextPWMInterrupt) + // The idea of just blindly telling the microcontroller to skip an + // interrupt is not possible. It should be totally deterministic to the + // code, but it isn't. Something else is going on under the hood masked + // from "userland" code. For some reason, one channel or the other + // generates a spurious interrupt, but rarely do both. This makes no + // sense. They are supposed to be independent. So, rather than just + // saying, this is my first time re-enabling the interrupt, and its going + // to fire, so skip it, I instead need to do some math on execution time. + // If we're really close to when the interrupt was enabled, we should skip. + // I've "guessed" at 100 microseconds. This is irrelevant to high rate + // pulses, so we're only concered with pauses, which are going to be + // measured in milliseconds, so 100 microseconds ought to be really close! + // but 1/10th of a millisecond shouldn't warrant a delay. Seems like a + // good balance. Hopefully this strategy proves correct for all cases... + uint32_t t = micros(); + uint32_t timeElapsed = micros() - q->timePWMInterruptEnabled; + if (t < q->timePWMInterruptEnabled) // Timer wrapped... + { + timeElapsed = 0xFFFFFFFF - q->timePWMInterruptEnabled; + timeElapsed += t; + } + if (timeElapsed < 100) { + // This is unfortunately still entirely necessary. I had thought I + // could gate this in other ways. Nope, the ISR is low lag enough we + // easily get into this ISR immediatly after enabling it for the pulse + // that was just handled by the PIO ISR, so we still need to skip one. + // Being too fast is a good thing :) + continue; + } + if (!q->_pauseCommanded) { + // In the immortal words of Richard Gray aka levelord, + //"YOU'RE NOT SUPPOSED TO BE HERE!" + + // try disabling the interrupt again...I saw on the logic analyzer + // we got in here a good 5 times extra! Seems like its not always + // disabling when we tell it to. Based on the quality of the datasheet + // I'm not altogether surprised! Handle it and move on... + PWM_INTERFACE->PWM_IDR1 = mask; + continue; + } + PWMCHANNELMAPPING* mapping = &gChannelMap[queue_num]; + q->driver_data = (void*)mapping; + + Pio* port = mapping->port; + + // Now with the queue, we can get the current entry, and see if we need to + uint8_t rp = q->read_idx; + if (rp == q->next_write_idx) { + // I believe this is solved by the gating of _delayCommanded and + // hasISRActive, but its not a bad idea to double check! + // Double interrupt case, bail before we destroy the read index... + // We're going to write rp and wp out as pulses onto pin32. + continue; + } + rp = ++q->read_idx; + struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK]; + if (rp == q->next_write_idx) { + q->_hasISRactive = false; + // Since we arent sure about more pauses, we need to say no we aren't + // pausing anymore... + q->_pauseCommanded = false; + // We're disconnected already, so we don't need to worry about that + // disconnect the interrupt though. + PWM_INTERFACE->PWM_IDR1 = mask; + PWM_INTERFACE->PWM_DIS = mask; + disconnectPWMPeriphal(port, mapping->pin, mapping->channelMask); + PWM_INTERFACE->PWM_IDR1 = mask; + q->read_idx = rp; + pinMode(mapping->pin, OUTPUT); + PIO_SetOutput(mapping->port, g_APinDescription[mapping->pin].ulPin, 0, 0, + 0); + + continue; // We're done apparently + } + if (e->steps > 0) { + // stop the interrupt on the PWM generator, set the period, + // re-attach the PIO handler, disconnect the PWM generator + // interrupt, and send it on its way...One problem, we have to use + // this ordering, but that means we miss an interrupt. So + // decrement the ticks here. + + // This delay is absolutely necessary. Our interrupt latency is + // quite low compared to the period of the PWM output. So we're able + // to get in here, determine our pause is done, switch the pin back to + // the PWM output, and output the pulse before it goes low, LONG before + // it goes low. + // So we add a 10 microsecond delay knowing the delay will be longer + // than 10 microseconds to ensure we have passed the point where the + // pulse is high! We won't be far off, so we aren't killing the processor + // with ISR time. If there were 2+ cores, I'd have another thread that + // would do the waiting so the ISR could be as short as possible, but in + // reality,10 microseonds would probably be shorter than the time it + // would take to setup a timer channel, enable its interrupt, and get into + // the ISR, so we'd likely be *less* efficient than just delaying. + delayMicroseconds(10); + PWM_INTERFACE->PWM_CH_NUM[channel].PWM_CPRDUPD = + e->ticks; // Change the period first! + PWM_INTERFACE->PWM_IDR1 = mask; + uint32_t dwSR = port->PIO_ABSR; + port->PIO_ABSR = (g_APinDescription[mapping->pin].ulPin | dwSR); + // Remove the pins from under the control of PIO + port->PIO_ODR |= g_APinDescription[mapping->pin].ulPin; + port->PIO_PDR = g_APinDescription[mapping->pin].ulPin; + port->PIO_IER = g_APinDescription[mapping->pin].ulPin; + q->_pauseCommanded = false; + } else { + // We needed to pause longer, so set the period, and wait... + // set the period, reconnect, restart, then disconnect PWM interrupt, so + // the next interrupt goes to the PIO interrupt handler and works like + // normal + PWM_INTERFACE->PWM_CH_NUM[channel].PWM_CPRDUPD = e->ticks; + } + q->read_idx = rp; + } +} + +inline void disconnectPWMPeriphal(Pio* port, uint8_t pin, + uint32_t channelMask) { + // This function has become quite minimal. Its still somewhat nice to have + // it seperated out, but I may decided it needs to just be inline...2 lines + // saved and function call overhead added :-/ + PWM_INTERFACE->PWM_DIS = channelMask; + PIO_SetOutput(port, g_APinDescription[pin].ulPin, 0, 0, 0); +} + +// gin66: removed the obsolete clearISR flag +inline void attachPWMPeripheral(Pio* port, uint8_t pin, uint32_t channelMask) { + PWM_INTERFACE->PWM_IDR1 = (channelMask); + uint32_t dwSR = port->PIO_ABSR; + port->PIO_ABSR = (g_APinDescription[pin].ulPin | dwSR); + // Remove the pins from under the control of PIO + port->PIO_ODR |= g_APinDescription[pin].ulPin; + port->PIO_PDR |= g_APinDescription[pin].ulPin; + port->PIO_IER |= g_APinDescription[pin].ulPin; + PWM_INTERFACE->PWM_ENA |= channelMask; +} +#define ISRQueueName(Q) Queue##Q##ISR +#define ISRQueueNameStr(Q) "Queue" #Q "ISR" + +#define DUE_STEPPER_ISR(Q) \ + void Queue##Q##ISR() { \ + /*These need only be looked up once rather than multiple times in the \ + * isr*/ \ + /*One would hope the compiler would do this, but how is it supposed to*/ \ + /*know if these values change or not? const requires setting it at */ \ + /*instantiation or casting...this is the next best thing :) */ \ + static StepperQueue* const q = &fas_queue[Q]; \ + const PWMCHANNELMAPPING* mapping = \ + (const PWMCHANNELMAPPING*)fas_queue[Q].driver_data; \ + static uint32_t channel = mapping->channel; \ + static uint32_t samPin = g_APinDescription[mapping->pin].ulPin; \ + static Pio* port = mapping->port; \ + if (!q->_hasISRactive || q->_pauseCommanded) { \ + /*In the immortal words of Richard Gray aka levelord, */ \ + /*"YOU'RE NOT SUPPOSED TO BE HERE!" */ \ + return; \ + } \ + IncrementQueue(Q); \ + uint8_t rp = q->read_idx; \ + /*This case should hopefully be elimiated....Unfortunately, it is not :(*/ \ + /*It is quite rare though.*/ \ + if (rp == q->next_write_idx) { \ + /*Double interrupt - bail before we destroy the read index!*/ \ + return; \ + } \ + struct queue_entry* e = &q->entry[rp & QUEUE_LEN_MASK]; \ + uint8_t s = --e->steps; \ + /*Obvious case, if we have done all of the steps in this queue entry....*/ \ + if (s == 0) { \ + /*Setup for the next move...The PWM Peripheral is pretty smart. We set \ + the period "update" register, and it will take effect at the end of \ + the current period. Since we arent changing the clock, there is no \ + need to change the duty cycle, just the period... \ + Check if we need to toggle direction....If so, we will also need to \ + delay...Does this ever happen? I think the dirPinBusy function \ + prevents this...*/ \ + if (e->toggle_dir) { \ + e->toggle_dir = 0; \ + *q->_dirPinPort ^= q->_dirPinMask; \ + } \ + /*Check immediatly if we need to turn off the pulse generator.Time is \ + critical! */ \ + rp++; \ + if (rp == q->next_write_idx) { \ + delayMicroseconds(10); \ + disconnectPWMPeriphal(mapping->port, mapping->pin, \ + mapping->channelMask); \ + q->_hasISRactive = false; \ + q->_connected = false; \ + /*Update the queue pointer...setup for the next move.*/ \ + q->read_idx = rp; \ + return; \ + } \ + /*Since we're done with e, e is now e_next...*/ \ + e = &q->entry[rp & QUEUE_LEN_MASK]; \ + q->read_idx = rp; \ + /*We need to look for queue entries with some sort of command \ + in them..*/ \ + if (e->steps == 0 || !e->hasSteps) { \ + delayMicroseconds(7); \ + q->_pauseCommanded = true; \ + q->timePWMInterruptEnabled = micros(); \ + PWM_INTERFACE->PWM_IER1 = mapping->channelMask; \ + port->PIO_CODR = samPin; \ + port->PIO_OER = samPin; \ + port->PIO_PER = samPin; \ + port->PIO_IDR = samPin; \ + PWM_INTERFACE->PWM_CH_NUM[channel].PWM_CPRDUPD = e->ticks; \ + return; \ + } \ + /*That should be it...We should start the next command on the next \ + cycle */ \ + AddToTotalSteps(Q, e->steps); \ + PWM_INTERFACE->PWM_CH_NUM[channel].PWM_CPRDUPD = e->ticks; \ + if (e->toggle_dir) { \ + e->toggle_dir = 0; \ + *q->_dirPinPort ^= q->_dirPinMask; \ + delayMicroseconds(30); \ + } \ + return; \ + } \ + } + +DUE_STEPPER_ISR(0) +DUE_STEPPER_ISR(1) +DUE_STEPPER_ISR(2) +DUE_STEPPER_ISR(3) +DUE_STEPPER_ISR(4) +DUE_STEPPER_ISR(5) + +inline uint32_t pinToChannel(uint32_t pin) { + switch (pin) { + case 34: + case 67: + case 73: + case 35: + return 0; + case 17: + case 36: + case 72: + case 37: + case 42: + return 1; + case 38: + case 43: + case 63: + case 39: + return 2; + case 40: + case 64: + case 69: + case 41: + return 3; + case 9: + return 4; + case 8: + case 44: + return 5; + case 7: + case 45: + return 6; + case 6: + return 7; + } + return 0xFFFFFFFF; +} + +void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { + _queue_num = queue_num; + driver_data = (void*)&gChannelMap[queue_num]; + _initVars(); + _step_pin = step_pin; + channelsUsed[pinToChannel(step_pin)] = true; + numChannels++; +#if defined(KEEP_SCORE) + totalPulsesDetected[queue_num] = 0; + totalSteps[queue_num] = 0; +#endif + gChannelMap[queue_num].pin = step_pin; + gChannelMap[queue_num].channel = pinToChannel(step_pin); + TimerChannel_Map[gChannelMap[queue_num].channel] = queue_num; + gChannelMap[queue_num].port = g_APinDescription[step_pin].pPort; + gChannelMap[queue_num].channelMask = 1 << gChannelMap[queue_num].channel; + static bool isPWMEnabled = false; + if (!isPWMEnabled) { + pmc_enable_periph_clk(PWM_INTERFACE_ID); + // ESP32 operates at 3x the speed as the due, which is reasonable to say the + // esp32 should be 3x as capable. In the code for the esp, the comment + // says the clock is 160/5, but the prescaler is set to 5-1? so its at + // 40MHz. We'll pick a slower clock rate for for this 255 for the prescaler, + // and a mod clock of 4 for 84,000,000/4=21,000,000/255 is 82352.9....52 + // might be the better number...This puts us at roughly 1/2 the performance + // of the esp32...Will have to see how many channels it can tolerate... + // remain compatible with due - We'll use MCK or clk B as necessary. + // PWMC_ConfigureClocks(PWM_FREQUENCY * PWM_MAX_DUTY_CYCLE, VARIANT_MCK / 4, + // VARIANT_MCK); + isPWMEnabled = true; + } + static bool isTCEnabled = false; + if (!isTCEnabled) { + // Lets set it up on a 2ms timer. That should keep the queue full... + // Enable the peripheral + // gin66: Better to remove any Serial.println() cause an application may + // choose to not use Serial + // Yes, agreed. I had meant to do that a long time ago! + // Serial.println("Enabling Timer Channel 5"); + pmc_enable_periph_clk(ID_TC5); + TC1->TC_CHANNEL[2].TC_CCR = 1; + TC1->TC_CHANNEL[2].TC_CMR = 0; + TC1->TC_CHANNEL[2].TC_CMR = 0x80 | 0x40; // Up counter reset on C + // comparison + // We want 2ms, we're using MCK/2 - 42MHz, so we need a count of 84000 + // Set A B and C registers so we guarantee a hit :) + TC1->TC_CHANNEL[2].TC_RA = 84000; + TC1->TC_CHANNEL[2].TC_RB = 84000; + TC1->TC_CHANNEL[2].TC_RC = 84000; + TC1->TC_CHANNEL[2].TC_IER = 0; + TC1->TC_CHANNEL[2].TC_IER = TC_IER_CPCS; + TC1->TC_CHANNEL[2].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN; + NVIC_SetPriority(TC5_IRQn, 1); + NVIC_EnableIRQ(TC5_IRQn); + } + + digitalWrite(step_pin, LOW); + pinMode(step_pin, OUTPUT); + NVIC_SetPriority(PIOA_IRQn, 0); + NVIC_SetPriority(PIOB_IRQn, 0); + NVIC_SetPriority(PIOC_IRQn, 0); + NVIC_SetPriority(PIOD_IRQn, 0); + PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F; + PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F; + NVIC_EnableIRQ(PWM_IRQn); + PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F; + PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F; + + connect(); +} + +void StepperQueue::connect() { + PIO_Configure(g_APinDescription[_step_pin].pPort, + g_APinDescription[_step_pin].ulPinType, + g_APinDescription[_step_pin].ulPin, + g_APinDescription[_step_pin].ulPinConfiguration); + const PWMCHANNELMAPPING* mapping = (const PWMCHANNELMAPPING*)driver_data; + PWMC_ConfigureChannel(PWM_INTERFACE, mapping->channel, PWM_CMR_CPRE_MCK_DIV_4, + 0, 0); + // 21 pulses makes for a microsecond pulse. I believe my drivers need 5us. + // I'll set this to 10 just to make sure the drivers receive this. At 20us + // pulse speed, this is still a 1/2 duty cycle PWM. Should be easy for any + // driver to pickup. + + PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F; + PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F; + PWM_INTERFACE->PWM_DIS = PWM_INTERFACE->PWM_DIS & (~mapping->channelMask); + PWMC_SetPeriod(PWM_INTERFACE, mapping->channel, 65535); + PWM_INTERFACE->PWM_DIS = PWM_INTERFACE->PWM_DIS & (~mapping->channelMask); + PWM_INTERFACE->PWM_CH_NUM[mapping->channel].PWM_CDTY = 21 * 10; + PWM_INTERFACE->PWM_DIS = PWM_INTERFACE->PWM_DIS & (~mapping->channelMask); + PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F; + PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F; + + // Rising edge, so we get the interrupt at the beginning of the pulse (gives + // us some extra time) and so we only get 1 int/pulse Change would give 2. + switch (_queue_num) { + case 0: + attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(0), + RISING); + break; + case 1: + attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(1), + RISING); + break; + case 2: + attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(2), + RISING); + break; + case 3: + attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(3), + RISING); + break; + case 4: + attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(4), + RISING); + break; + case 5: + attachInterrupt(digitalPinToInterrupt(_step_pin), ISRQueueName(5), + RISING); + break; + } + _connected = true; + PWM_INTERFACE->PWM_IDR2 = 0x00FFFF0F; + PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F; +} + +void StepperQueue::disconnect() { + const PWMCHANNELMAPPING* mapping = (const PWMCHANNELMAPPING*)driver_data; + PWMC_DisableChannel(PWM_INTERFACE, mapping->channel); + PWM_INTERFACE->PWM_DIS = PWM_INTERFACE->PWM_DIS & (~mapping->channelMask); + + // gin66: Shouldn't there be a detachInterrupt() in order to not have stray + // interrupt ? + // disconnect is a strange term for what we're doing. Instead of + // disconnecting interrupts, we disable the source of the interrupts. If + // something external causes a signal on the pin, yes, we will get an + // interrupt. That should never happen...but there is code in the interrupt + // to detect it because I used that very issue to debug the delay code :) If + // I got a pulse while in delay mode, I knew there was a problem. I had it + // strobe a pin I could see on the logic analyzer. It made it very easy to + // see when "wrong" behavior was happening. + _connected = false; +} + +// gin66: I have reworked all code to only use startQueue(). +// This appears to be less complicated. +// I reworked the rework...sorry, it was easier to start with something that +// worked and then remove unnecessary code as there was clearly unnecessary +// code :) +void StepperQueue::startQueue() { + // This is called only, if isRunning() == false + noInterrupts(); + struct queue_entry* e = &entry[read_idx & QUEUE_LEN_MASK]; + // Somewhere there must be a call to nointerrupts, because addinf this solved + // the no-start issue. + interrupts(); + _hasISRactive = true; + + const PWMCHANNELMAPPING* mapping = (const PWMCHANNELMAPPING*)driver_data; + + // I had this reversed, but the situation where we are starting, but the + // PWM peripheral is running already doesn't come up often. If the + // channel is enabled, we need to use the UPD register to trigger the + // update on the next cycle, otherwise we can directly set PWM_CPRD. + // Setting the UPD register instead of the direct register fortunately wasn't + // harmful, and works as expected. I'd still leave this check in here just + // in case, though I believe everything is controlled well enough to not need + // it. + + if (PWM_INTERFACE->PWM_SR & (1 << mapping->channel)) { + PWM_INTERFACE->PWM_CH_NUM[mapping->channel].PWM_CPRDUPD = e->ticks; + } else { + PWM_INTERFACE->PWM_CH_NUM[mapping->channel].PWM_CPRD = e->ticks; + } + + if (e->steps > 0 || e->hasSteps) { + attachPWMPeripheral(mapping->port, mapping->pin, mapping->channelMask); + return; + } else { + // I could see the confusion...man was I tired of this code when I + // committed :) It was quite dirty. This case is when we DO NOT + // want to fully attach the PWM peripheral, in that we do not want it + // outputting anything. So we set the period, but do not attach it. You + // moved the period to the top, and I changed it to be the period change + // with the check to see if we were already running, which is unlikely + // here, but better to be certain. The rest of this function is about + // making certain the output is disabled, but the PWM interrupt used for + // delays only is enabled and working. + // + // I had found that every single move command always started with a delay. + // So even in startQueue, this is necessary. I had seen that even before + // the change for adding delays for the dir pin! So this block is still + // entirely necessary :) I'll make better comments though in the code + // below... + + // Disconnect the step pin entirely, and force its output to 0! We're + // in a delay, we do not want extra pulses generated! + PIO_SetOutput(mapping->port, g_APinDescription[mapping->pin].ulPin, 0, 0, + 0); + // Disable the PIO interrupt. We probably do not need to do this, and I + // tested my test code without it, but it is still probably the "right + // thing" to do. Don't jump into an interrupt unless we specifically + // say its ok to do so! :) + mapping->port->PIO_IDR = g_APinDescription[mapping->pin].ulPin; + // Here we enable the PWM peripheral, despite having its output + // disconnected. This is how we manage the delays. The pwm peripheral + // happily counts the time until its supposed to generate a pulse. It + // generates the pulse into a high impedance output, because the PIO + // module has it switched off, but it still generates a PWM interrupt + // for us, so we know to advance the queue using the PWM ISR. + PWM_INTERFACE->PWM_ENA |= mapping->channelMask; + _pauseCommanded = true; + // We do not want to set timePWMInterruptEnabled here. It takes a bit to + // explain, but the purpose of the timePWMInterruptEnabled is to check to + // see if we are in the PWM interrupt for the same interrupt as the pulse. + // That seems strange and it is. I could probably have used the PWM + // interrupts only for this purpose, but it was actually nice to seperate + // the delay ISR from the pulse ISR. It made things clean...up until it + // didn't. We would enable the PWM ISR and it would immediatly trigger + // from the PIO ISR. The only reliable way to deal with it was to see if + // we were within a hundred microseconds of the pulse. If we were, we + // skipped that ISR call. Without it, we'd end up skipping the delay. + // Enabling the check here, well, we don't seem to get to the ISR within + // 100 microseconds anyhow, so it...shouldn't....cause any issues if + // present, but why tempt fate/Murphy? We never want to skip a PWM + // interrupt at this point, so lets not give it the opportunity :) + + /*Enable the ISR too so we don't miss it*/ + PWM_INTERFACE->PWM_IER1 = PWM_INTERFACE->PWM_IMR1 | mapping->channelMask; + return; + } +} + +void StepperQueue::forceStop() { + noInterrupts(); + read_idx = next_write_idx; + interrupts(); + const PWMCHANNELMAPPING* mapping = (const PWMCHANNELMAPPING*)driver_data; + PWMC_DisableChannel(PWM_INTERFACE, mapping->channel); + // gin66: I have added this line in the hope to make it work + _hasISRactive = false; +} + +bool StepperQueue::isValidStepPin(uint8_t step_pin) { + if (pinToChannel(step_pin) < 0x08) { + if (!channelsUsed[pinToChannel(step_pin)]) { + return true; + } + } + return false; +} + +bool StepperQueue::isRunning() { return _hasISRactive; } + +int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { return -1; } + +void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { + max_speed_in_ticks = 420; // This equals 50kHz @ 21MHz +} + +void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core) { + fas_engine = engine; +} +#endif diff --git a/src/StepperISR_esp32.cpp b/src/StepperISR_esp32.cpp new file mode 100644 index 0000000..98c5007 --- /dev/null +++ b/src/StepperISR_esp32.cpp @@ -0,0 +1,142 @@ +#include "StepperISR.h" + +#if defined(SUPPORT_ESP32) + +// Here are the global variables to interface with the interrupts +StepperQueue fas_queue[NUM_QUEUES]; + +void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { + uint8_t channel = queue_num; +#ifdef SUPPORT_ESP32_MCPWM_PCNT + if (channel < QUEUES_MCPWM_PCNT) { + use_rmt = false; + init_mcpwm_pcnt(channel, step_pin); + return; + } + channel -= QUEUES_MCPWM_PCNT; +#endif +#ifdef SUPPORT_ESP32_RMT + use_rmt = true; + init_rmt(channel, step_pin); +#endif +} + +void StepperQueue::connect() { +#ifdef SUPPORT_ESP32_RMT + if (use_rmt) { + connect_rmt(); + return; + } +#endif +#ifdef SUPPORT_ESP32_MCPWM_PCNT + connect_mcpwm_pcnt(); +#endif +} + +void StepperQueue::disconnect() { +#ifdef SUPPORT_ESP32_RMT + if (use_rmt) { + disconnect_rmt(); + return; + } +#endif +#ifdef SUPPORT_ESP32_MCPWM_PCNT + disconnect_mcpwm_pcnt(); +#endif +} + +bool StepperQueue::isReadyForCommands() const { +#if defined(SUPPORT_ESP32_RMT) && defined(SUPPORT_ESP32_MCPWM_PCNT) + if (use_rmt) { + return isReadyForCommands_rmt(); + } + return isReadyForCommands_mcpwm_pcnt(); +#elif defined(SUPPORT_ESP32_MCPWM_PCNT) + return isReadyForCommands_mcpwm_pcnt(); +#elif defined(SUPPORT_ESP32_RMT) + return isReadyForCommands_rmt(); +#else +#error "Nothing defined here" +#endif +} + +void StepperQueue::startQueue() { +#ifdef SUPPORT_ESP32_RMT + if (use_rmt) { + startQueue_rmt(); + return; + } +#endif +#ifdef SUPPORT_ESP32_MCPWM_PCNT + startQueue_mcpwm_pcnt(); +#endif +} +void StepperQueue::forceStop() { +#ifdef SUPPORT_ESP32_RMT + if (use_rmt) { + forceStop_rmt(); + return; + } +#endif +#ifdef SUPPORT_ESP32_MCPWM_PCNT + forceStop_mcpwm_pcnt(); +#endif +} +uint16_t StepperQueue::_getPerformedPulses() const { +#ifdef SUPPORT_ESP32_RMT + if (use_rmt) { + return _getPerformedPulses_rmt(); + } +#endif +#ifdef SUPPORT_ESP32_MCPWM_PCNT + return _getPerformedPulses_mcpwm_pcnt(); +#endif + return 0; +} + +//************************************************************************************************* + +bool StepperQueue::isValidStepPin(uint8_t step_pin) { + gpio_drive_cap_t strength; + esp_err_t res = gpio_get_drive_capability((gpio_num_t)step_pin, &strength); + return res == ESP_OK; +} +int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { return -1; } + +//************************************************************************************************* +[[noreturn]] void StepperTask(void *parameter) { + auto *engine = (FastAccelStepperEngine *)parameter; + const TickType_t delay_4ms = + (DELAY_MS_BASE + portTICK_PERIOD_MS - 1) / portTICK_PERIOD_MS; + while (true) { + engine->manageSteppers(); +#if ESP_IDF_VERSION_MAJOR == 4 + // not clear, if the wdt reset is needed. With idf-version 5, the reset + // causes an issue. + esp_task_wdt_reset(); +#endif + vTaskDelay(delay_4ms); + } +} + +void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { + max_speed_in_ticks = 80; // This equals 200kHz @ 16MHz +} + +void fas_init_engine(FastAccelStepperEngine *engine, uint8_t cpu_core) { +#if ESP_IDF_VERSION_MAJOR == 4 +#define STACK_SIZE 2000 +#define PRIORITY configMAX_PRIORITIES +#else +#define STACK_SIZE 3000 +#define PRIORITY (configMAX_PRIORITIES - 1) +#endif + if (cpu_core > 1) { + xTaskCreate(StepperTask, "StepperTask", STACK_SIZE, engine, PRIORITY, nullptr); + } else { + xTaskCreatePinnedToCore(StepperTask, "StepperTask", STACK_SIZE, engine, + PRIORITY, nullptr, cpu_core); + } +} + +#endif diff --git a/src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp b/src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp new file mode 100644 index 0000000..73c2128 --- /dev/null +++ b/src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp @@ -0,0 +1,574 @@ +#include "StepperISR.h" +#if defined(SUPPORT_ESP32_MCPWM_PCNT) && (ESP_IDF_VERSION_MAJOR == 4) + +#define DEFAULT_TIMER_H_L_TRANSITION 160 + +// cannot be updated while timer is running => fix it to 0 +#define TIMER_PRESCALER 0 + +// #define TEST_PROBE 18 + +// Here the associated mapping from queue to mcpwm/pcnt units +// +// As the ISR is accessing this table, the mapping cannot be put into flash, +// even this is actually a constant table +struct mapping_s { + mcpwm_unit_t mcpwm_unit; + uint8_t timer; + mcpwm_io_signals_t pwm_output_pin; + pcnt_unit_t pcnt_unit; + uint8_t input_sig_index; + uint32_t cmpr_tea_int_clr; + uint32_t cmpr_tea_int_ena; + uint32_t cmpr_tea_int_raw; +}; + +static struct mapping_s channel2mapping[NUM_QUEUES] = { + { + mcpwm_unit : MCPWM_UNIT_0, + timer : 0, + pwm_output_pin : MCPWM0A, + pcnt_unit : PCNT_UNIT_0, + input_sig_index : PCNT_SIG_CH0_IN0_IDX, + cmpr_tea_int_clr : MCPWM_OP0_TEA_INT_CLR, + cmpr_tea_int_ena : MCPWM_OP0_TEA_INT_ENA, + cmpr_tea_int_raw : MCPWM_OP0_TEA_INT_RAW + }, + { + mcpwm_unit : MCPWM_UNIT_0, + timer : 1, + pwm_output_pin : MCPWM1A, + pcnt_unit : PCNT_UNIT_1, + input_sig_index : PCNT_SIG_CH0_IN1_IDX, + cmpr_tea_int_clr : MCPWM_OP1_TEA_INT_CLR, + cmpr_tea_int_ena : MCPWM_OP1_TEA_INT_ENA, + cmpr_tea_int_raw : MCPWM_OP1_TEA_INT_RAW + }, + { + mcpwm_unit : MCPWM_UNIT_0, + timer : 2, + pwm_output_pin : MCPWM2A, + pcnt_unit : PCNT_UNIT_2, + input_sig_index : PCNT_SIG_CH0_IN2_IDX, + cmpr_tea_int_clr : MCPWM_OP2_TEA_INT_CLR, + cmpr_tea_int_ena : MCPWM_OP2_TEA_INT_ENA, + cmpr_tea_int_raw : MCPWM_OP2_TEA_INT_RAW + }, + { + mcpwm_unit : MCPWM_UNIT_1, + timer : 0, + pwm_output_pin : MCPWM0A, + pcnt_unit : PCNT_UNIT_3, + input_sig_index : PCNT_SIG_CH0_IN3_IDX, + cmpr_tea_int_clr : MCPWM_OP0_TEA_INT_CLR, + cmpr_tea_int_ena : MCPWM_OP0_TEA_INT_ENA, + cmpr_tea_int_raw : MCPWM_OP0_TEA_INT_RAW + }, +#ifndef HAVE_ESP32S3_PULSE_COUNTER + { + mcpwm_unit : MCPWM_UNIT_1, + timer : 1, + pwm_output_pin : MCPWM1A, + pcnt_unit : PCNT_UNIT_4, + input_sig_index : PCNT_SIG_CH0_IN4_IDX, + cmpr_tea_int_clr : MCPWM_OP1_TEA_INT_CLR, + cmpr_tea_int_ena : MCPWM_OP1_TEA_INT_ENA, + cmpr_tea_int_raw : MCPWM_OP1_TEA_INT_RAW + }, + { + mcpwm_unit : MCPWM_UNIT_1, + timer : 2, + pwm_output_pin : MCPWM2A, + pcnt_unit : PCNT_UNIT_5, + input_sig_index : PCNT_SIG_CH0_IN5_IDX, + cmpr_tea_int_clr : MCPWM_OP2_TEA_INT_CLR, + cmpr_tea_int_ena : MCPWM_OP2_TEA_INT_ENA, + cmpr_tea_int_raw : MCPWM_OP2_TEA_INT_RAW + }, +#endif +}; + +static void IRAM_ATTR prepare_for_next_command( + StepperQueue *queue, const struct queue_entry *e_next) { + uint8_t next_steps = e_next->steps; + if (next_steps > 0) { + const struct mapping_s *mapping = + (const struct mapping_s *)queue->driver_data; + pcnt_unit_t pcnt_unit = mapping->pcnt_unit; + // is updated only on zero +#ifndef HAVE_ESP32S3_PULSE_COUNTER + PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim = next_steps; +#else + PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un = next_steps; +#endif + } +} + +#define isr_pcnt_counter_clear(pcnt_unit) \ + REG_SET_BIT(PCNT_CTRL_REG, (1 << (2 * pcnt_unit))); \ + REG_CLR_BIT(PCNT_CTRL_REG, (1 << (2 * pcnt_unit))) + +static void IRAM_ATTR apply_command(StepperQueue *queue, + const struct queue_entry *e) { + const struct mapping_s *mapping = + (const struct mapping_s *)queue->driver_data; + mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit; + mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1; + pcnt_unit_t pcnt_unit = mapping->pcnt_unit; + uint8_t timer = mapping->timer; + uint8_t steps = e->steps; + if (e->toggle_dir) { + LL_TOGGLE_PIN(queue->dirPin); + } + uint16_t ticks = e->ticks; +#ifndef __ESP32_IDF_V44__ + if (mcpwm->timer[timer].status.value <= 1) { // mcpwm Timer is stopped ? + mcpwm->timer[timer].period.upmethod = 0; // 0 = immediate update, 1 = TEZ +#else /* __ESP32_IDF_V44__ */ + if (mcpwm->timer[timer].timer_status.timer_value <= + 1) { // mcpwm Timer is stopped ? + mcpwm->timer[timer].timer_cfg0.timer_period_upmethod = + 0; // 0 = immediate update, 1 = TEZ +#endif /* __ESP32_IDF_V44__ */ + } else { + // 0 = immediate update, 1 = TEZ +#ifndef __ESP32_IDF_V44__ + mcpwm->timer[timer].period.upmethod = 1; +#else /* __ESP32_IDF_V44__ */ + mcpwm->timer[timer].timer_cfg0.timer_period_upmethod = 1; +#endif /* __ESP32_IDF_V44__ */ + } +#ifndef __ESP32_IDF_V44__ + mcpwm->timer[timer].period.period = ticks; +#else /* __ESP32_IDF_V44__ */ + mcpwm->timer[timer].timer_cfg0.timer_period = ticks; +#endif /* __ESP32_IDF_V44__ */ + if (steps == 0) { + // timer value = 1 - upcounting: output low +#ifndef __ESP32_IDF_V44__ + mcpwm->channel[timer].generator[0].utea = 1; +#else /* __ESP32_IDF_V44__ */ + mcpwm->operators[timer].generator[0].gen_utea = 1; +#endif /* __ESP32_IDF_V44__ */ + mcpwm->int_clr.val = mapping->cmpr_tea_int_clr; + mcpwm->int_ena.val |= mapping->cmpr_tea_int_ena; + } else { + bool disable_mcpwm_interrupt = true; +#ifndef HAVE_ESP32S3_PULSE_COUNTER + if (PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim != steps) { +#else + if (PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un != steps) { +#endif + // For fast pulses, eventually the ISR is late. So take the current pulse + // count into consideration. + // + // Automatic update for next pulse cnt on pulse counter zero does not + // work. For example the sequence: + // 5 pulses + // 1 pause ==> here need to store 3, but not + // available yet 3 pulses + // + // Read counter +#ifndef HAVE_ESP32S3_PULSE_COUNTER + uint16_t val1 = steps - PCNT.cnt_unit[pcnt_unit].cnt_val; +#else + uint16_t val1 = steps - PCNT.cnt_unit[pcnt_unit].pulse_cnt_un; +#endif + // Clear flag for l-->h transition + mcpwm->int_clr.val = mapping->cmpr_tea_int_clr; + // Read counter again +#ifndef HAVE_ESP32S3_PULSE_COUNTER + uint16_t val2 = steps - PCNT.cnt_unit[pcnt_unit].cnt_val; +#else + uint16_t val2 = steps - PCNT.cnt_unit[pcnt_unit].pulse_cnt_un; +#endif + // If no pulse arrives between val1 and val2: + // val2 == val1 + // If pulse arrives between val1 and int_clr: + // mcpwm status is cleared and val2 == val1+1 + // If pulse arrives between int_clr and val2: + // mcpwm status is set and val2 == val1+1 + // => mcwpm status info is not reliable, so clear again + if (val1 != val2) { + // Clear flag again. No pulse can be expected between val2 and here + mcpwm->int_clr.val = mapping->cmpr_tea_int_clr; + } + + // is updated only on zero +#ifndef HAVE_ESP32S3_PULSE_COUNTER + PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim = val2; +#else + PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un = val2; +#endif + // force take over + isr_pcnt_counter_clear(pcnt_unit); + // Check, if pulse has come in + if ((mcpwm->int_raw.val & mapping->cmpr_tea_int_raw) != 0) { + // Pulse has come in + // Check if the pulse has been counted or not +#ifndef HAVE_ESP32S3_PULSE_COUNTER + if (PCNT.cnt_unit[pcnt_unit].cnt_val == 0) { +#else + if (PCNT.cnt_unit[pcnt_unit].pulse_cnt_un == 0) { +#endif + // pulse hasn't been counted, so adjust the limit + // is updated only on zero +#ifndef HAVE_ESP32S3_PULSE_COUNTER + PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim = val2 - 1; +#else + PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un = val2 - 1; +#endif + // force take over + isr_pcnt_counter_clear(pcnt_unit); + + // In case val2 == 1, then there may be a problem. + // The pulse counter may not cause any interrupt with cnt_h_lim == 0 + // and if the mcpwm-interrupt is disabled, then the queue is stalled. + // in this case, we should leave the mcpwm enabled + disable_mcpwm_interrupt = val2 > 0; + } + } + } + // disable mcpwm interrupt + if (disable_mcpwm_interrupt) { + mcpwm->int_ena.val &= ~mapping->cmpr_tea_int_ena; + } + // timer value = 1 - upcounting: output high +#ifndef __ESP32_IDF_V44__ + mcpwm->channel[timer].generator[0].utea = 2; +#else /* __ESP32_IDF_V44__ */ + mcpwm->operators[timer].generator[0].gen_utea = 2; +#endif /* __ESP32_IDF_V44__ */ + } +} + +static void IRAM_ATTR init_stop(StepperQueue *q) { + // init stop is normally called after the first command, + // because the second command is entered too late + // and after the last command aka running out of commands. + const struct mapping_s *mapping = (const struct mapping_s *)q->driver_data; + mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit; + mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1; + uint8_t timer = mapping->timer; +#ifndef __ESP32_IDF_V44__ + mcpwm->timer[timer].mode.start = 0; // 0: stop at TEZ +#else /* __ESP32_IDF_V44__ */ + mcpwm->timer[timer].timer_cfg1.timer_start = 0; // 0: stop at TEZ +#endif /* __ESP32_IDF_V44__ */ + // timer value = 1 - upcounting: output low + mcpwm->int_ena.val &= ~mapping->cmpr_tea_int_ena; + // PCNT.conf_unit[mapping->pcnt_unit].conf2.cnt_h_lim = 1; + q->_isRunning = false; +} + +static void IRAM_ATTR what_is_next(StepperQueue *q) { + // when starting the queue, apply_command for the first entry is called. + // the read pointer stays at this position. This function is reached, + // if the command to which read pointer points to is completed. + bool isPrepared = q->_nextCommandIsPrepared; + q->_nextCommandIsPrepared = false; + uint8_t rp = q->read_idx; + if (rp != q->next_write_idx) { + struct queue_entry *e_completed = &q->entry[rp & QUEUE_LEN_MASK]; + bool repeat_entry = e_completed->repeat_entry != 0; + if (!repeat_entry) { + rp++; + q->read_idx = rp; + } + if (rp != q->next_write_idx) { + struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; + if (!isPrepared) { + prepare_for_next_command(q, e_curr); // a no-op for pause command + const struct mapping_s *mapping = + (const struct mapping_s *)q->driver_data; + isr_pcnt_counter_clear(mapping->pcnt_unit); + } + apply_command(q, e_curr); + if (!repeat_entry) { + rp++; + if (rp != q->next_write_idx) { + struct queue_entry *e_next = &q->entry[rp & QUEUE_LEN_MASK]; + q->_nextCommandIsPrepared = true; + prepare_for_next_command(q, e_next); // a no-op for pause command + } + } else { + q->_nextCommandIsPrepared = false; + } + return; + } + } + // no more commands: stop timer at period end + init_stop(q); +} + +static void IRAM_ATTR pcnt_isr_service(void *arg) { + StepperQueue *q = (StepperQueue *)arg; + what_is_next(q); +} + +// MCPWM_SERVICE is only used in case of pause +#ifndef __ESP32_IDF_V44__ + +#define MCPWM_SERVICE(mcpwm, TIMER, pcnt) \ + if (mcpwm.int_st.cmpr##TIMER##_tea_int_st != 0) { \ + /*managed in apply_command() */ \ + /*mcpwm.int_clr.cmpr##TIMER##_tea_int_clr = 1;*/ \ + StepperQueue *q = &fas_queue[pcnt]; \ + what_is_next(q); \ + } + +#else /* __ESP32_IDF_V44__ */ + +#define MCPWM_SERVICE(mcpwm, TIMER, pcnt) \ + if (mcpwm.int_st.op##TIMER##_tea_int_st != 0) { \ + /*managed in apply_command() */ \ + /*mcpwm.int_clr.cmpr##TIMER##_tea_int_clr = 1;*/ \ + StepperQueue *q = &fas_queue[pcnt]; \ + what_is_next(q); \ + } + +#endif /* __ESP32_IDF_V44__ */ + +static void IRAM_ATTR mcpwm0_isr_service(void *arg) { + // For whatever reason, this interrupt is constantly called even with int_st = + // 0 while the timer is running + MCPWM_SERVICE(MCPWM0, 0, 0); + MCPWM_SERVICE(MCPWM0, 1, 1); + MCPWM_SERVICE(MCPWM0, 2, 2); +} +static void IRAM_ATTR mcpwm1_isr_service(void *arg) { + MCPWM_SERVICE(MCPWM1, 0, 3); +#ifndef HAVE_ESP32S3_PULSE_COUNTER + MCPWM_SERVICE(MCPWM1, 1, 4); + MCPWM_SERVICE(MCPWM1, 2, 5); +#endif +} + +void StepperQueue::init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin) { +#ifdef TEST_PROBE + pinMode(TEST_PROBE, OUTPUT); +#endif + + _initVars(); + _step_pin = step_pin; + + const struct mapping_s *mapping = &channel2mapping[channel_num]; + driver_data = (void *)mapping; + + mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit; + mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1; + pcnt_unit_t pcnt_unit = mapping->pcnt_unit; + uint8_t timer = mapping->timer; + + pcnt_config_t cfg; + // if step_pin is not set here (or 0x30), then it does not work + cfg.pulse_gpio_num = step_pin; // static 0 is 0x30, static 1 is 0x38 + cfg.ctrl_gpio_num = PCNT_PIN_NOT_USED; // static 0 is 0x30, static 1 is 0x38 + cfg.lctrl_mode = PCNT_MODE_KEEP; + cfg.hctrl_mode = PCNT_MODE_KEEP; + cfg.pos_mode = PCNT_COUNT_INC; // increment on rising edge + cfg.neg_mode = PCNT_COUNT_DIS; // ignore falling edge + cfg.counter_h_lim = 1; + cfg.counter_l_lim = 0; + cfg.unit = pcnt_unit; + cfg.channel = PCNT_CHANNEL_0; + pcnt_unit_config(&cfg); + +#ifndef HAVE_ESP32S3_PULSE_COUNTER + PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim = 1; + PCNT.conf_unit[pcnt_unit].conf0.thr_h_lim_en = 1; + PCNT.conf_unit[pcnt_unit].conf0.thr_l_lim_en = 0; +#else + PCNT.conf_unit[pcnt_unit].conf2.cnt_h_lim_un = 1; + PCNT.conf_unit[pcnt_unit].conf0.thr_h_lim_en_un = 1; + PCNT.conf_unit[pcnt_unit].conf0.thr_l_lim_en_un = 0; +#endif + + pcnt_counter_clear(pcnt_unit); + pcnt_counter_resume(pcnt_unit); + pcnt_event_enable(pcnt_unit, PCNT_EVT_H_LIM); + if (channel_num == 0) { + // isr_service_install apparently enables the interrupt + PCNT.int_clr.val = PCNT.int_st.val; + pcnt_isr_service_install(ESP_INTR_FLAG_SHARED | ESP_INTR_FLAG_IRAM); + } + pcnt_isr_handler_add(pcnt_unit, pcnt_isr_service, (void *)this); + + if (timer == 0) { + // Init mcwpm module for use + periph_module_enable(mcpwm_unit == MCPWM_UNIT_0 ? PERIPH_PWM0_MODULE + : PERIPH_PWM1_MODULE); + mcpwm->int_ena.val = 0; // disable all interrupts + mcpwm_isr_register( + mcpwm_unit, + mcpwm_unit == MCPWM_UNIT_0 ? mcpwm0_isr_service : mcpwm1_isr_service, + NULL, ESP_INTR_FLAG_IRAM | ESP_INTR_FLAG_SHARED, NULL); + + // 160 MHz/5 = 32 MHz => 16 MHz in up/down-mode +#ifndef __ESP32_IDF_V44__ + mcpwm->clk_cfg.prescale = 5 - 1; +#else /* __ESP32_IDF_V44__ */ + mcpwm->clk_cfg.clk_prescale = 5 - 1; +#endif /* __ESP32_IDF_V44__ */ + + // timer 0 is input for operator 0 + // timer 1 is input for operator 1 + // timer 2 is input for operator 2 +#ifndef __ESP32_IDF_V44__ + mcpwm->timer_sel.operator0_sel = 0; + mcpwm->timer_sel.operator1_sel = 1; + mcpwm->timer_sel.operator2_sel = 2; +#else /* __ESP32_IDF_V44__ */ + mcpwm->operator_timersel.operator0_timersel = 0; + mcpwm->operator_timersel.operator1_timersel = 1; + mcpwm->operator_timersel.operator2_timersel = 2; +#endif /* __ESP32_IDF_V44__ */ + } +#ifndef __ESP32_IDF_V44__ + mcpwm->timer[timer].period.upmethod = 1; // 0 = immediate update, 1 = TEZ + mcpwm->timer[timer].period.prescale = TIMER_PRESCALER; + mcpwm->timer[timer].period.period = 400; // Random value + mcpwm->timer[timer].mode.mode = 3; // 3=up/down counting + mcpwm->timer[timer].mode.start = 0; // 0: stop at TEZ +#else /* __ESP32_IDF_V44__ */ + // 0 = immediate update, 1 = TEZ + mcpwm->timer[timer].timer_cfg0.timer_period_upmethod = 1; + mcpwm->timer[timer].timer_cfg0.timer_prescale = TIMER_PRESCALER; + mcpwm->timer[timer].timer_cfg0.timer_period = 400; // Random value + mcpwm->timer[timer].timer_cfg1.timer_mod = 3; // 3=up/down counting + mcpwm->timer[timer].timer_cfg1.timer_start = 0; // 0: stop at TEZ +#endif /* __ESP32_IDF_V44__ */ + + // this sequence should reset the timer to 0 +#ifndef __ESP32_IDF_V44__ + mcpwm->timer[timer].sync.timer_phase = 0; // prepare value of 0 + mcpwm->timer[timer].sync.in_en = 1; // enable sync + mcpwm->timer[timer].sync.sync_sw ^= 1; // force a sync + mcpwm->timer[timer].sync.in_en = 0; // disable sync + + mcpwm->channel[timer].cmpr_cfg.a_upmethod = 0; // 0 = immediate update + mcpwm->channel[timer].cmpr_value[0].cmpr_val = 1; // set compare value A + mcpwm->channel[timer].generator[0].val = 0; // clear all trigger actions + mcpwm->channel[timer].generator[1].val = 0; // clear all trigger actions + mcpwm->channel[timer].generator[0].dtep = 1; // low at period + mcpwm->channel[timer].db_cfg.val = 0; // edge delay disabled + mcpwm->channel[timer].carrier_cfg.val = 0; // carrier disabled +#else /* __ESP32_IDF_V44__ */ + mcpwm->timer[timer].timer_sync.timer_phase = 0; // prepare value of 0 + mcpwm->timer[timer].timer_sync.timer_synci_en = 1; // enable sync + mcpwm->timer[timer].timer_sync.timer_sync_sw ^= 1; // force a sync + mcpwm->timer[timer].timer_sync.timer_synci_en = 0; // disable sync + + mcpwm->operators[timer].gen_stmp_cfg.gen_a_upmethod = + 0; // 0 = immediate update + mcpwm->operators[timer].timestamp[0].gen = 1; // set compare value A + mcpwm->operators[timer].generator[0].val = 0; // clear all trigger actions + mcpwm->operators[timer].generator[1].val = 0; // clear all trigger actions + mcpwm->operators[timer].generator[0].gen_dtep = 1; // low at period + mcpwm->operators[timer].dt_cfg.val = 0; // edge delay disabled + mcpwm->operators[timer].carrier_cfg.val = 0; // carrier disabled +#endif /* __ESP32_IDF_V44__ */ + + digitalWrite(step_pin, LOW); + pinMode(step_pin, OUTPUT); + + // at last, link mcpwm to output pin and back into pcnt input + connect(); +} + +void StepperQueue::connect_mcpwm_pcnt() { + const struct mapping_s *mapping = (const struct mapping_s *)driver_data; + mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit; + mcpwm_gpio_init(mcpwm_unit, mapping->pwm_output_pin, _step_pin); + // Doesn't work with gpio_matrix_in + // gpio_matrix_in(step_pin, mapping->input_sig_index, false); + gpio_iomux_in(_step_pin, mapping->input_sig_index); +} + +void StepperQueue::disconnect_mcpwm_pcnt() { + // sig_index = 0x100 => cancel output + gpio_matrix_out(_step_pin, 0x100, false, false); + // untested alternative: + // gpio_reset_pin((gpio_num_t)q->step_pin); +} + +// Mechanism is like this, starting from stopped motor: +// +// * init counter +// * init mcpwm +// * start mcpwm +// * -- pcnt counter counts every L->H-transition at mcpwm.timer = 1 +// * -- if counter reaches planned steps, then counter is reset and +// * interrupt is created +// +// * pcnt interrupt: available time is from mcpwm.timer = 1+x to period +// - read next commmand: store period in counter shadow and +// steps in pcnt +// - without next command: set mcpwm to stop mode on reaching +// period +// + +void StepperQueue::startQueue_mcpwm_pcnt() { +#ifdef TEST_PROBE + // The time used by this command can have an impact + digitalWrite(TEST_PROBE, digitalRead(TEST_PROBE) == HIGH ? LOW : HIGH); +#endif + const struct mapping_s *mapping = (const struct mapping_s *)driver_data; + mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit; + mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1; + uint8_t timer = mapping->timer; + + // apply_command() assumes the pcnt counter to contain executed steps + // and deduct this from the new command. For a starting motor + // need to make sure, that the counter is 0. (issue #33) + pcnt_unit_t pcnt_unit = mapping->pcnt_unit; + pcnt_counter_clear(pcnt_unit); + + _isRunning = true; + _nextCommandIsPrepared = false; + struct queue_entry *e = &entry[read_idx & QUEUE_LEN_MASK]; + apply_command(this, e); + +#ifndef __ESP32_IDF_V44__ + mcpwm->timer[timer].mode.start = 2; // 2=run continuous +#else /* __ESP32_IDF_V44__ */ + mcpwm->timer[timer].timer_cfg1.timer_start = 2; // 2=run continuous +#endif /* __ESP32_IDF_V44__ */ +} +void StepperQueue::forceStop_mcpwm_pcnt() { + init_stop(this); + read_idx = next_write_idx; +} +bool StepperQueue::isReadyForCommands_mcpwm_pcnt() { + if (isRunning()) { + return true; + } + const struct mapping_s *mapping = (const struct mapping_s *)driver_data; + mcpwm_unit_t mcpwm_unit = mapping->mcpwm_unit; + mcpwm_dev_t *mcpwm = mcpwm_unit == MCPWM_UNIT_0 ? &MCPWM0 : &MCPWM1; + uint8_t timer = mapping->timer; +#ifndef __ESP32_IDF_V44__ + if (mcpwm->timer[timer].status.value > 1) { +#else /* __ESP32_IDF_V44__ */ + if (mcpwm->timer[timer].timer_status.timer_value > 1) { +#endif /* __ESP32_IDF_V44__ */ + return false; + } + return true; + // #ifndef __ESP32_IDF_V44__ + // return (mcpwm->timer[timer].mode.start != 2); // 2=run continuous + // #else /* __ESP32_IDF_V44__ */ + // return (mcpwm->timer[timer].timer_cfg1.timer_start != 2); // 2=run + // continuous + // #endif /* __ESP32_IDF_V44__ */ +} +uint16_t StepperQueue::_getPerformedPulses_mcpwm_pcnt() { + const struct mapping_s *mapping = (const struct mapping_s *)driver_data; +#ifndef HAVE_ESP32S3_PULSE_COUNTER + return PCNT.cnt_unit[mapping->pcnt_unit].cnt_val; +#else + return PCNT.cnt_unit[mapping->pcnt_unit].pulse_cnt_un; +#endif +} + +#endif diff --git a/src/StepperISR_idf4_esp32_rmt.cpp b/src/StepperISR_idf4_esp32_rmt.cpp new file mode 100644 index 0000000..9f9e2a6 --- /dev/null +++ b/src/StepperISR_idf4_esp32_rmt.cpp @@ -0,0 +1,511 @@ +#include "StepperISR.h" +#if defined(HAVE_ESP32_RMT) && (ESP_IDF_VERSION_MAJOR == 4) + +// #define TEST_MODE + +#include "fas_arch/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. +#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; +} + +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 + 61) / 62); + } + return; + } + // The ongoing command does not contain steps, so change dir here should be + // ok. But we need the gpio_ll functions, which are always + // inlined...hopefully. + LL_TOGGLE_PIN(q->dirPin); + // 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++ = 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; + } + } + 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 +#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[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; \ + } \ + } +#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); + } + // 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(); + +#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; +#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 +} + +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.conf_ch[channel].conf1.idle_out_en = 0; +} + +void StepperQueue::startQueue_rmt() { +// #define TRACE +#ifdef TRACE + Serial.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); +#endif + rmt_tx_stop(channel); + // rmt_rx_stop(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); + // 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); + } +#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) { + LL_TOGGLE_PIN(dirPin); + 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(' '); + Serial.println(mem[i], HEX); + } + if (!isRunning()) { + Serial.println("STOPPED 1"); + } +#endif + + 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(' '); + 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; + + // This starts the rmt module + RMT.conf_ch[channel].conf1.tx_conti_mode = 1; + + RMT.conf_ch[channel].conf1.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/StepperISR_idf4_esp32c3_rmt.cpp b/src/StepperISR_idf4_esp32c3_rmt.cpp new file mode 100644 index 0000000..4c2e10e --- /dev/null +++ b/src/StepperISR_idf4_esp32c3_rmt.cpp @@ -0,0 +1,652 @@ +#include "StepperISR.h" +#if defined(HAVE_ESP32C3_RMT) && (ESP_IDF_VERSION_MAJOR == 4) + +// #define TEST_MODE +// #define TRACE + +#include "fas_arch/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 HAVE_ESP32C3_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 + LL_TOGGLE_PIN(q->dirPin); + // 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(HAVE_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_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); + 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) { + LL_TOGGLE_PIN(dirPin); + 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/StepperISR_idf4_esp32s3_rmt.cpp b/src/StepperISR_idf4_esp32s3_rmt.cpp new file mode 100644 index 0000000..f37a273 --- /dev/null +++ b/src/StepperISR_idf4_esp32s3_rmt.cpp @@ -0,0 +1,652 @@ +#include "StepperISR.h" +#if defined(HAVE_ESP32S3_RMT) && (ESP_IDF_VERSION_MAJOR == 4) + +// #define TEST_MODE +// #define TRACE + +#include "fas_arch/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 esp32s3 (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 HAVE_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.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); } + +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; +} + +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 + LL_TOGGLE_PIN(q->dirPin); + // 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 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(HAVE_ESP32S3_RMT) +#define RMT_LIMIT tx_lim +#define RMT_FIFO apb_fifo_mask +#else +#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. +// 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.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; \ + } +#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 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_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); + RMT.sys_conf.apb_fifo_mask = 1; // disable fifo mode + } + + _isRunning = false; + _rmtStopped = true; + + connect_rmt(); + +#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; + } + } +#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; +#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_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); +#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; +} + +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.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; + } +#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; + +#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) { + LL_TOGGLE_PIN(dirPin); + 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"); + } +#endif + + 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(' '); + + 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.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; +} +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/StepperISR_idf5_esp32_rmt.cpp b/src/StepperISR_idf5_esp32_rmt.cpp new file mode 100644 index 0000000..e080ad7 --- /dev/null +++ b/src/StepperISR_idf5_esp32_rmt.cpp @@ -0,0 +1,350 @@ +#include "StepperISR.h" +#if defined(HAVE_ESP32_RMT) && (ESP_IDF_VERSION_MAJOR == 5) + +// #define TEST_MODE + +#include "fas_arch/test_probe.h" + +#define PART_SIZE (RMT_SIZE / 2) + +static bool IRAM_ATTR queue_done(rmt_channel_handle_t tx_chan, + const rmt_tx_done_event_data_t *edata, + void *user_ctx) { + auto *q = (StepperQueue *)user_ctx; + q->_isRunning = false; + return false; +} + +#define ENTER_PAUSE(ticks) \ + { \ + uint16_t remaining_ticks = ticks; \ + uint16_t half_ticks_per_symbol = (ticks) / (2 * PART_SIZE); \ + uint32_t main_symbol = 0x00010001 * half_ticks_per_symbol; \ + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { \ + (*symbols++).val = main_symbol; \ + } \ + remaining_ticks -= 2 * (PART_SIZE - 1) * half_ticks_per_symbol; \ + uint16_t first_ticks = remaining_ticks / 2; \ + remaining_ticks -= first_ticks; \ + last_entry = 0x00010000 * first_ticks + remaining_ticks; \ + } + +static size_t IRAM_ATTR encode_commands(const void *data, size_t data_size, + size_t symbols_written, + size_t symbols_free, + rmt_symbol_word_t *symbols, bool *done, + void *arg) { + // this printf causes Guru Meditation + // printf("encode commands\n"); + + auto *q = (StepperQueue *)arg; + + *done = false; + if (symbols_free < PART_SIZE) { + // not sufficient space for the symbols + return 0; + } + + uint8_t rp = q->read_idx; + if (q->_rmtStopped) { + *done = true; + return 0; + } + if ((rp == q->next_write_idx) || q->_rmtStopped) { + // if we return done already here, then single stepping fails + q->_rmtStopped = true; + // Not sure if this pause is really needed + uint16_t last_entry; + ENTER_PAUSE(MIN_CMD_TICKS); + symbols->val = last_entry; + return PART_SIZE; + } + + // 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->lastChunkContainsSteps) { + // So we need a pause. change the finished read entry into a pause + q->lastChunkContainsSteps = false; + uint16_t last_entry; + ENTER_PAUSE(MIN_CMD_TICKS); + symbols->val = last_entry; + return PART_SIZE; + } + // The ongoing command does not contain steps, so change dir here should be + // ok + LL_TOGGLE_PIN(q->dirPin); + // 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->lastChunkContainsSteps = false; + ENTER_PAUSE(ticks); + } else { + q->lastChunkContainsSteps = 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 + (*symbols++).val = 0x40007fff | 0x8000; + (*symbols++).val = 0x20002000; + } + // the last step needs to be stretched to fill PART_SIZE entries + (*symbols++).val = 0x40007fff | 0x8000; + uint16_t delta = PART_SIZE - 2 * steps; + delta <<= 5; + (*symbols++).val = 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++) { + (*symbols++).val = 0x00100010; + } + last_entry = 0x00100010; + steps = 0; + } else { + steps -= PART_SIZE / 2; + for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { + (*symbols++).val = 0x40007fff | 0x8000; + (*symbols++).val = 0x20002000; + } + (*symbols++).val = 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++) { + (*symbols++).val = rmt_entry; + } + // the last step needs to be stretched to fill PART_SIZE entries + uint32_t delta = PART_SIZE - steps_to_do; + delta <<= 18; // shift in upper 16bit and multiply with 4 + (*symbols++).val = rmt_entry - delta; + for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { + (*symbols++).val = 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++) { + (*symbols++).val = 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; + //} + symbols->val = 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; + } + + return PART_SIZE; +} + +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; + pinMode(step_pin, OUTPUT); + digitalWrite(step_pin, LOW); + + rmt_simple_encoder_config_t enc_config = { + .callback = encode_commands, .arg = this, .min_chunk_size = PART_SIZE}; + esp_err_t rc; + rc = rmt_new_simple_encoder(&enc_config, &_tx_encoder); + ESP_ERROR_CHECK_WITHOUT_ABORT(rc); + + connect_rmt(); + _isRunning = false; + _rmtStopped = true; +} + +void StepperQueue::connect_rmt() { + rmt_tx_channel_config_t config; + config.gpio_num = (gpio_num_t)_step_pin; + config.clk_src = RMT_CLK_SRC_DEFAULT; + config.resolution_hz = TICKS_PER_S; + config.mem_block_symbols = 2 * PART_SIZE; + config.trans_queue_depth = 1; + config.intr_priority = 0; + config.flags.invert_out = 0; + config.flags.with_dma = 0; + config.flags.io_loop_back = 0; + config.flags.io_od_mode = 0; + esp_err_t rc = rmt_new_tx_channel(&config, &channel); + ESP_ERROR_CHECK_WITHOUT_ABORT(rc); + + rmt_tx_event_callbacks_t callbacks = {.on_trans_done = queue_done}; + (void)rmt_tx_register_event_callbacks(channel, &callbacks, this); + + _channel_enabled = false; +} + +void StepperQueue::disconnect_rmt() { + if (_channel_enabled || _isRunning || !_rmtStopped) { + return; + } + (void)rmt_del_channel(channel); + channel = nullptr; +} + +void StepperQueue::startQueue_rmt() { +// #define TRACE +#ifdef TRACE + Serial.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); +#endif + +// #define TRACE +#ifdef TRACE + printf("Queue: %d/%d %s\n", read_idx, next_write_idx, + _isRunning ? "Running" : "Stopped"); +#endif + + if (channel == nullptr) { + return; + } + + // 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) { + LL_TOGGLE_PIN(dirPin); + entry[rp & QUEUE_LEN_MASK].toggle_dir = false; + } + +#ifdef TRACE + queue_entry *e = &entry[rp & QUEUE_LEN_MASK]; + printf("first command: ticks=%u steps=%u %s %s\n", e->ticks, e->steps, + e->countUp ? "up" : "down", e->toggle_dir ? "toggle" : ""); +#endif + + if (_channel_enabled) { + // rmt_disable(channel); + // _channel_enabled = false; + } + + lastChunkContainsSteps = false; + _isRunning = true; + _rmtStopped = false; + + // payload and payload bytes must not be 0 + int payload = 0; + rmt_transmit_config_t tx_config; + tx_config.loop_count = 0; + tx_config.flags.eot_level = 0; // output level at end of transmission + tx_config.flags.queue_nonblocking = 1; + _tx_encoder->reset(_tx_encoder); + + if (!_channel_enabled) { + rmt_enable(channel); + _channel_enabled = true; + } + + esp_err_t rc = rmt_transmit(channel, _tx_encoder, &payload, 1, &tx_config); + ESP_ERROR_CHECK_WITHOUT_ABORT(rc); +#ifdef TRACE + printf("Transmission started\n"); +#endif +} +void StepperQueue::forceStop_rmt() { + (void)rmt_disable(channel); + _channel_enabled = false; + _isRunning = false; + _rmtStopped = true; + + // and empty the buffer + read_idx = next_write_idx; +} +bool StepperQueue::isReadyForCommands_rmt() const { + if (_isRunning) { + return !_rmtStopped; + } + return true; +} +uint16_t StepperQueue::_getPerformedPulses_rmt() { return 0; } +#endif diff --git a/src/fas_arch/arduino_avr.h b/src/fas_arch/arduino_avr.h new file mode 100644 index 0000000..9ebd22f --- /dev/null +++ b/src/fas_arch/arduino_avr.h @@ -0,0 +1,83 @@ +#ifndef FAS_ARCH_ARDUINO_AVR_H +#define FAS_ARCH_ARDUINO_AVR_H + +#define SUPPORT_AVR +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1 + +// 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() +#define fasEnableInterrupts() SREG = prevSREG + +// Here are shorthand definitions for number of queues, the queues/channel +// relation and queue length This definitions are derivate specific +#define QUEUE_LEN 16 +#define TICKS_PER_S F_CPU +#define MIN_CMD_TICKS (TICKS_PER_S / 25000) +#define MIN_DIR_DELAY_US 40 +#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000)) +#define DELAY_MS_BASE (65536000 / TICKS_PER_S) + +// debug led timing +#define DEBUG_LED_HALF_PERIOD (TICKS_PER_S / 65536 / 2) + +#define noop_or_wait + +#define SUPPORT_DIR_TOGGLE_PIN_MASK uint8_t + +#define SUPPORT_QUEUE_ENTRY_END_POS_U16 + +//========================================================================== +// +// AVR derivate ATmega 168/328/P +// +//========================================================================== +#if (defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || \ + defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__)) +#define SUPPORT_EXTERNAL_DIRECTION_PIN +#define MAX_STEPPER 2 +#define NUM_QUEUES 2 +#define fas_queue_A fas_queue[0] +#define fas_queue_B fas_queue[1] +enum channels { channelA, channelB }; +//========================================================================== +// +// AVR derivate ATmega 2560 +// +//========================================================================== +#elif defined(__AVR_ATmega2560__) +#define SUPPORT_EXTERNAL_DIRECTION_PIN +#define MAX_STEPPER 3 +#define NUM_QUEUES 3 +#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 }; +//========================================================================== +// +// AVR derivate ATmega 32U4 +// +//========================================================================== +#elif defined(__AVR_ATmega32U4__) +#define MAX_STEPPER 3 +#define NUM_QUEUES 3 +#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 }; +//========================================================================== +// +// For all unsupported AVR derivates +// +//========================================================================== +#else +#error "Unsupported AVR derivate" +#endif + +#endif /* FAS_ARCH_ARDUINO_AVR_H */ diff --git a/src/fas_arch/arduino_esp32.h b/src/fas_arch/arduino_esp32.h new file mode 100644 index 0000000..691b119 --- /dev/null +++ b/src/fas_arch/arduino_esp32.h @@ -0,0 +1,13 @@ +#ifndef FAS_ARCH_ARDUINO_ESP32_H +#define FAS_ARCH_ARDUINO_ESP32_H + +// this is an arduino platform, so include the Arduino.h header file +#include + +// For esp32 using arduino, just use arduino definition +#define fasEnableInterrupts interrupts +#define fasDisableInterrupts noInterrupts + +#include "fas_arch/common_esp32.h" + +#endif /* FAS_ARCH_ARDUINO_ESP32_H */ diff --git a/src/fas_arch/arduino_sam.h b/src/fas_arch/arduino_sam.h new file mode 100644 index 0000000..372f091 --- /dev/null +++ b/src/fas_arch/arduino_sam.h @@ -0,0 +1,34 @@ +#ifndef FAS_ARCH_ARDUINO_SAM_H +#define FAS_ARCH_ARDUINO_SAM_H + +#define SUPPORT_SAM + +// this is an arduino platform, so include the Arduino.h header file +#include +// on SAM just use the arduino macros +#define fasEnableInterrupts interrupts +#define fasDisableInterrupts noInterrupts + +// queue definitions for SAM +#define MAX_STEPPER 6 +#define NUM_QUEUES 6 +#define QUEUE_LEN 32 + +// timing definitions for SAM +#define TICKS_PER_S 21000000L +#define MIN_CMD_TICKS (TICKS_PER_S / 5000) +#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 2 + +// debug led timing +#define DEBUG_LED_HALF_PERIOD 50 + +#define noop_or_wait + +#define SUPPORT_DIR_PIN_MASK uint32_t + +// TO BE CHECKED +#define SUPPORT_QUEUE_ENTRY_END_POS_U16 + +#endif /* FAS_ARCH_ARDUINO_SAM_H */ diff --git a/src/fas_arch/common.h b/src/fas_arch/common.h new file mode 100644 index 0000000..40f17e3 --- /dev/null +++ b/src/fas_arch/common.h @@ -0,0 +1,105 @@ +#ifndef FAS_COMMON_H +#define FAS_COMMON_H + +#define TICKS_FOR_STOPPED_MOTOR 0xffffffff + +#define MOVE_OK 0 +#define MOVE_ERR_NO_DIRECTION_PIN -1 +#define MOVE_ERR_SPEED_IS_UNDEFINED -2 +#define MOVE_ERR_ACCELERATION_IS_UNDEFINED -3 + +// Low level stepper motor command. +// +// You can add these using the addQueueEntry method. +// They will be executed sequentially until the queue is empty. +// +// There are some constraints on the values: +// - `ticks` must be greater or equal to FastAccelStepper::getMaxSpeedInTicks. +// - `ticks*steps` must be greater or equal to MIN_CMD_TICKS +// +// For example: +// A command with ticks=TICKS_PER_S/1000, steps = 3, count_up = true means that: +// 1. The direction pin is set to HIGH. +// 2. One step is generated. +// 3. Exactly 1 ms after the first step, the second step is issued. +// 4. Exactly 1 ms after the second step, the third step is issued. +// 5. The stepper waits for 1 ms. +// 6. The next command is processed. +struct stepper_command_s { + // Number of ticks between each step. + // There are `TICKS_PER_S` ticks per second. This may vary between different + // platforms. + uint16_t ticks; + + // Number of steps to send to the stepper motor during this command. + // If zero, then this command will be treated as a pause, lasting for a number + // of ticks given by `ticks`. + uint8_t steps; + + // True if the direction pin should be high during this command, false if it + // should be low. + bool count_up; +}; + +struct actual_ticks_s { + uint32_t ticks; // ticks == 0 means standstill + bool count_up; +}; + +// I doubt, volatile is needed. +struct queue_end_s { + 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 +#define fas_min(a, b) ((a) > (b) ? (b) : (a)) +#define fas_max(a, b) ((a) > (b) ? (a) : (b)) +#define fas_abs(x) ((x) >= 0 ? (x) : (-x)) + +//============================================================================== +// All architecture specific definitions should be located here +//============================================================================== + +//========================================================================== +#if defined(TEST) +// TEST "architecture" is in use with pc_based testing. +#include "fas_arch/test_pc.h" + +#elif defined(ARDUINO_ARCH_ESP32) +// ESP32 derivates using arduino core +#include "fas_arch/arduino_esp32.h" + +#elif defined(ESP_PLATFORM) +// ESP32 derivates using espidf +#include "fas_arch/espidf_esp32.h" + +#elif defined(ARDUINO_ARCH_SAM) +// SAM-architecture +#include "fas_arch/arduino_sam.h" + +#elif defined(ARDUINO_ARCH_AVR) +// AVR family +#include "fas_arch/arduino_avr.h" + +#else +#error "Unsupported devices" +#endif + +// in order to avoid spikes, first set the value and then make an output +// esp32 idf5 does not like this approach +#ifndef PIN_OUTPUT +#define PIN_OUTPUT(pin, value) \ + { \ + digitalWrite(pin, (value)); \ + pinMode(pin, OUTPUT); \ + } +#endif + +// disable inject_fill_interrupt() for all real devices. Only defined in TEST +#ifndef TEST +#define inject_fill_interrupt(x) +#endif + +#endif /* FAS_COMMON_H */ diff --git a/src/fas_arch/common_esp32.h b/src/fas_arch/common_esp32.h new file mode 100644 index 0000000..5f5c171 --- /dev/null +++ b/src/fas_arch/common_esp32.h @@ -0,0 +1,60 @@ +#ifndef FAS_ARCH_COMMON_ESP32_H +#define FAS_ARCH_COMMON_ESP32_H + +#include + +#define SUPPORT_ESP32 +#define SUPPORT_EXTERNAL_DIRECTION_PIN +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1 + +// Some more esp32 specific includes +#include +#include +#include + +#if ESP_IDF_VERSION_MAJOR == 5 +#if ESP_IDF_VERSION_MINOR < 3 +#error "FastAccelStepper requires esp-idf >= 5.3.0" +#endif +#include "fas_arch/common_esp32_idf5.h" +#elif ESP_IDF_VERSION_MAJOR == 4 +#include "fas_arch/common_esp32_idf4.h" +#elif ESP_IDF_VERSION_MAJOR <= 3 +#pragma "Last supported by FastAccelStepper 0.30.15" +#endif + +// Esp32 queue definitions +#define NUM_QUEUES (QUEUES_MCPWM_PCNT + QUEUES_RMT) +#define MAX_STEPPER (NUM_QUEUES) +#define QUEUE_LEN 32 + +// Esp32 timing definition +#define TICKS_PER_S 16000000L +#define MIN_CMD_TICKS (TICKS_PER_S / 5000) +#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 4 + +#define SUPPORT_QUEUE_ENTRY_START_POS_U16 + +// debug led timing +#define DEBUG_LED_HALF_PERIOD 50 + +#define noop_or_wait vTaskDelay(1) + +// have more than one core +#define SUPPORT_CPU_AFFINITY + +#define LL_TOGGLE_PIN(dirPin) \ + gpio_ll_set_level(&GPIO, (gpio_num_t)dirPin, \ + gpio_ll_get_level(&GPIO, (gpio_num_t)dirPin) ^ 1) + +//========================================================================== +// 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 /* FAS_ARCH_COMMON_ESP32_H */ diff --git a/src/fas_arch/common_esp32_idf4.h b/src/fas_arch/common_esp32_idf4.h new file mode 100644 index 0000000..ea9e8b8 --- /dev/null +++ b/src/fas_arch/common_esp32_idf4.h @@ -0,0 +1,104 @@ +#ifndef FAS_ARCH_COMMON_ESP32_IDF4_H +#define FAS_ARCH_COMMON_ESP32_IDF4_H + +//========================================================================== +// +// ESP32 derivate - the first one +// +//========================================================================== + +#if CONFIG_IDF_TARGET_ESP32 +#define SUPPORT_ESP32_MCPWM_PCNT +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_PULSE_COUNTER 8 +#define HAVE_ESP32_RMT +#define QUEUES_MCPWM_PCNT 6 +#define QUEUES_RMT 8 +#define NEED_RMT_HEADERS +#define NEED_MCPWM_HEADERS +#define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32S2 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32S2 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_PULSE_COUNTER 4 +#define HAVE_ESP32S3_PULSE_COUNTER +#define HAVE_ESP32_RMT +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 4 +#define NEED_RMT_HEADERS +#define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32S3 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32S3 +#define SUPPORT_ESP32_MCPWM_PCNT +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_PULSE_COUNTER 4 +#define HAVE_ESP32S3_PULSE_COUNTER +#define HAVE_ESP32S3_RMT + +#define QUEUES_MCPWM_PCNT 4 +#define QUEUES_RMT 4 +#define NEED_RMT_HEADERS +#define NEED_MCPWM_HEADERS +#define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32C3 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32C3 +#define SUPPORT_ESP32_RMT +#define HAVE_ESP32C3_RMT +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 2 +#define NEED_RMT_HEADERS + +//========================================================================== +// +// For all unsupported ESP32 derivates +// +//========================================================================== +#else +#error "Unsupported derivate" +#endif + +#if ESP_IDF_VERSION_MINOR == 4 +#define __ESP32_IDF_V44__ +#endif + +#include +#include + +#ifdef NEED_MCPWM_HEADERS +#include +#include +#include +#endif + +#ifdef NEED_PCNT_HEADERS +#include +#include +#include +#endif + +#ifdef NEED_RMT_HEADERS +#include +#include +#include +#include + +#define RMT_CHANNEL_T rmt_channel_t +#define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) +#endif + +#endif /* FAS_ARCH_COMMON_ESP32_IDF4_H */ diff --git a/src/fas_arch/common_esp32_idf5.h b/src/fas_arch/common_esp32_idf5.h new file mode 100644 index 0000000..08e2da8 --- /dev/null +++ b/src/fas_arch/common_esp32_idf5.h @@ -0,0 +1,152 @@ +#ifndef FAS_ARCH_COMMON_ESP32_IDF5_H +#define FAS_ARCH_COMMON_ESP32_IDF5_H + +//========================================================================== +// +// ESP32 derivate - the first one +// +//========================================================================== + +#if CONFIG_IDF_TARGET_ESP32 +// #define SUPPORT_ESP32_MCPWM_PCNT +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_PULSE_COUNTER 8 +#define HAVE_ESP32_RMT +#define RMT_SIZE 64 + +// #define QUEUES_MCPWM_PCNT 6 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 8 + +#define NEED_RMT_HEADERS +// #define NEED_MCPWM_HEADERS +#define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32S2 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32S2 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_PULSE_COUNTER 4 +#define HAVE_ESP32S3_PULSE_COUNTER +#define HAVE_ESP32_RMT +#define RMT_SIZE 64 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 4 +#define NEED_RMT_HEADERS +#define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32S3 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32S3 +// #define SUPPORT_ESP32_MCPWM_PCNT +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_PULSE_COUNTER 8 +#define HAVE_ESP32S3_PULSE_COUNTER +#define HAVE_ESP32_RMT +#define RMT_SIZE 48 + +// #define QUEUES_MCPWM_PCNT 4 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 4 +#define NEED_RMT_HEADERS +// #define NEED_MCPWM_HEADERS +#define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32C3 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32C3 +#define SUPPORT_ESP32_RMT +#define HAVE_ESP32_RMT +#define RMT_SIZE 48 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 2 +#define NEED_RMT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32C6 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32C6 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_PULSE_COUNTER 4 +#define HAVE_ESP32_RMT +#define RMT_SIZE 48 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 2 +#define NEED_RMT_HEADERS +#define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32H2 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32H2 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_PULSE_COUNTER 4 +#define HAVE_ESP32_RMT +#define RMT_SIZE 48 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 2 +#define NEED_RMT_HEADERS +#define NEED_PCNT_HEADERS + +//========================================================================== +// +// For all unsupported ESP32 derivates +// +//========================================================================== +#else +#error "Unsupported derivate" +#endif + +// #include +#include +#include + +#ifdef NEED_MCPWM_HEADERS +#include +#include +#include +#endif + +#ifdef NEED_PCNT_HEADERS +#include +// #include +// #include +#include +#include +#include +#include +#include +#endif + +#ifdef NEED_RMT_HEADERS +#include +#include +#include +#include + +#define RMT_CHANNEL_T rmt_channel_handle_t +#define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) +#endif + +// in order to avoid spikes, first set the value and then make an output +// esp32 idf5 does not like this approach => output first, then value +#define PIN_OUTPUT(pin, value) \ + { \ + pinMode(pin, OUTPUT); \ + digitalWrite(pin, (value)); \ + } + +#endif /* FAS_ARCH_COMMON_ESP32_IDF5_H */ diff --git a/src/fas_arch/espidf_esp32.h b/src/fas_arch/espidf_esp32.h new file mode 100644 index 0000000..3458086 --- /dev/null +++ b/src/fas_arch/espidf_esp32.h @@ -0,0 +1,25 @@ +#ifndef FAS_ARCH_ESPIDF_ESP32_H +#define FAS_ARCH_ESPIDF_ESP32_H + +// esp32 specific includes +#include + +// on espidf need to use portDISABLE/ENABLE_INTERRUPTS +#include +#include +#define fasDisableInterrupts portDISABLE_INTERRUPTS +#define fasEnableInterrupts portENABLE_INTERRUPTS + +// The espidf-platform needs a couple of arduino like definitions +#define LOW 0 +#define HIGH 1 +#define OUTPUT GPIO_MODE_OUTPUT +#define pinMode(pin, mode) \ + gpio_set_direction((gpio_num_t)pin, \ + (mode) == OUTPUT ? GPIO_MODE_OUTPUT : GPIO_MODE_INPUT) +#define digitalWrite(pin, level) gpio_set_level((gpio_num_t)pin, level) +#define digitalRead(pin) gpio_get_level((gpio_num_t)pin) + +#include "fas_arch/common_esp32.h" + +#endif /* FAS_ARCH_ESPIDF_ESP32_H */ diff --git a/src/fas_arch/test_pc.h b/src/fas_arch/test_pc.h new file mode 100644 index 0000000..99a25d4 --- /dev/null +++ b/src/fas_arch/test_pc.h @@ -0,0 +1,42 @@ +#ifndef FAS_ARCH_TEST_PC_H +#define FAS_ARCH_TEST_PC_H + +// For pc-based testing like to have assert-macro +#include + +// and some more includes +#include +#include +#include + +#include "../extras/tests/pc_based/stubs.h" + +// For pc-based testing, the macro TEST is defined. The pc-based testing does +// not support the concept of interrupts, so provide an empty definition +#define fasEnableInterrupts() +#define fasDisableInterrupts() + +// The TEST target needs a couple of arduino like definitions +#define LOW 0 +#define HIGH 1 + +// queue definitions for pc based testing +#define MAX_STEPPER 2 +#define NUM_QUEUES 2 +#define fas_queue_A fas_queue[0] +#define fas_queue_B fas_queue[1] +#define QUEUE_LEN 16 + +// timing definitions for pc-based testing +#define TICKS_PER_S 16000000L +#define MIN_CMD_TICKS (TICKS_PER_S / 5000) +#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 + +#define SUPPORT_QUEUE_ENTRY_END_POS_U16 + +#endif /* FAS_ARCH_TEST_PC_H */ diff --git a/src/fas_arch/test_probe.h b/src/fas_arch/test_probe.h new file mode 100644 index 0000000..733b490 --- /dev/null +++ b/src/fas_arch/test_probe.h @@ -0,0 +1,56 @@ +#ifndef TEST_PROBE_H +#define TEST_PROBE_H +// #define TEST_MODE +// test mode in rmt: +// code to investigate rmt module functioning + +// #define 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 + +#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 \ + pinMode(TEST_PROBE_1, OUTPUT); \ + digitalWrite(TEST_PROBE_1, digitalRead(TEST_PROBE_1) == HIGH ? LOW : HIGH) +#else +#define PROBE_1(x) +#define PROBE_1_TOGGLE +#endif +#ifdef TEST_PROBE_2 +#define PROBE_2(x) digitalWrite(TEST_PROBE_2, x) +#define PROBE_2_TOGGLE \ + pinMode(TEST_PROBE_2, OUTPUT); \ + digitalWrite(TEST_PROBE_2, digitalRead(TEST_PROBE_2) == HIGH ? LOW : HIGH) +#else +#define PROBE_2(x) +#define PROBE_2_TOGGLE +#endif +#ifdef TEST_PROBE_3 +#define PROBE_3(x) digitalWrite(TEST_PROBE_3, x) +#define PROBE_3_TOGGLE \ + pinMode(TEST_PROBE_3, OUTPUT); \ + digitalWrite(TEST_PROBE_3, digitalRead(TEST_PROBE_3) == HIGH ? LOW : HIGH) +#else +#define PROBE_3(x) +#define PROBE_3_TOGGLE +#endif + +#endif