From d2ab2f2fb988817b1c97ec9d6904ad2d088aec79 Mon Sep 17 00:00:00 2001 From: Robin Petereit Date: Sun, 23 Feb 2020 01:08:23 +0100 Subject: [PATCH 01/35] Add ESP32 timer and Arduino libraries --- library.json | 6 +- library.properties | 6 +- src/MotorControlBase.h | 13 +++- src/RotateControlBase.h | 3 + src/StepControlBase.h | 2 +- src/Stepper.cpp | 14 +++- src/Stepper.h | 30 ++++++++- src/TeensyStep.h | 16 +++-- src/accelerators/LinRotAccelerator.h | 2 + src/timer/esp32/TimerField.h | 97 ++++++++++++++++++++++++++++ src/timer/generic/TickTimer.cpp | 4 +- src/timer/generic/TickTimer.h | 4 +- src/timer/generic/TimerField.h | 2 + src/timer/teensy3/PIT.cpp | 9 ++- src/timer/teensy3/PIT.h | 2 +- src/version.h | 4 +- 16 files changed, 188 insertions(+), 26 deletions(-) create mode 100644 src/timer/esp32/TimerField.h diff --git a/library.json b/library.json index 415ffea..e708a55 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "TeensyStep", - "keywords": "teensy, stepper, motor, high-speed", - "description": "High speed stepper driver for teensy boards (T3.0 - T3.6)", + "keywords": "teensy, esp32, stepper, motor, high-speed", + "description": "High speed stepper driver for teensy boards (T3.0 - T3.6) and ESP32", "repository": { "type": "git", @@ -15,7 +15,7 @@ "maintainer": true }, "homepage": "https://luni64.github.io/TeensyStep", - "version": "2.0.0", + "version": "2.0.1", "frameworks": "arduino", "platforms": "Teensy" } diff --git a/library.properties b/library.properties index 9037b19..a869d10 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=TeensyStep -version=2.0.0 +version=2.0.1 author=Lutz Niggl maintainer=Lutz Niggl -sentence=High speed stepper driver for PJRC Teensy boards (T3.0 - T3.6) -paragraph= Step rates up to 300000stp/sec. Accelerated and synchronized movement of up to 10 steppers. Due to the low processor load it can easily be used togehter with sensors, displays, serial communication ... +sentence=High speed stepper driver for PJRC Teensy boards (T3.0 - T3.6) and ESP32 +paragraph= Step rates up to 300000stp/sec. Accelerated and synchronized movement of up to 10 steppers. Due to the low processor load it can easily be used together with sensors, displays, serial communication ... category=Device Control url=https://luni64.github.io/TeensyStep/ architectures=* diff --git a/src/MotorControlBase.h b/src/MotorControlBase.h index 4f6fc80..1558bcb 100644 --- a/src/MotorControlBase.h +++ b/src/MotorControlBase.h @@ -20,6 +20,16 @@ class MotorControlBase : TF_Handler virtual ~MotorControlBase(); bool isOk() const { return OK; } + void setup(void (*stepCallback)(void), void (*accCallback)(void), void (*delayCallback)(void)) + { + timerField.setupStepTimer(stepCallback); + timerField.setupAccTimer(accCallback); + timerField.setupDelayTimer(delayCallback); + } + + void stepTimerISR(); + void pulseTimerISR(); + protected: TimerField timerField; MotorControlBase(unsigned pulseWidth, unsigned accUpdatePeriod); @@ -30,9 +40,6 @@ class MotorControlBase : TF_Handler void attachStepper(Stepper &stepper, Steppers &... steppers); void attachStepper(Stepper &stepper); - void stepTimerISR(); - //void accTimerISR() { Serial.println("df"); } - void pulseTimerISR(); Stepper *motorList[MaxMotors + 1]; Stepper *leadMotor; diff --git a/src/RotateControlBase.h b/src/RotateControlBase.h index 2a44f5b..a592d1c 100644 --- a/src/RotateControlBase.h +++ b/src/RotateControlBase.h @@ -2,7 +2,10 @@ #include "MotorControlBase.h" #include + +#ifndef ARDUINO_ARCH_ESP32 #include "core_pins.h" +#endif template class RotateControlBase : public MotorControlBase diff --git a/src/StepControlBase.h b/src/StepControlBase.h index 3bbf928..61b73b6 100644 --- a/src/StepControlBase.h +++ b/src/StepControlBase.h @@ -28,8 +28,8 @@ class StepControlBase : public MotorControlBase // Misc ---------------------------------- void setCallback(void (*_callback)()) { this->callback = _callback; } -protected: void accTimerISR(); +protected: void doMove(int N, bool mode = true); diff --git a/src/Stepper.cpp b/src/Stepper.cpp index 9c0caec..457cf81 100644 --- a/src/Stepper.cpp +++ b/src/Stepper.cpp @@ -1,5 +1,10 @@ #include "Stepper.h" + +#ifdef ARDUINO_ARCH_ESP32 +#include "esp32-hal-gpio.h" +#elif #include "core_pins.h" +#endif Stepper::Stepper(const int _stepPin, const int _dirPin) : current(0), stepPin(_stepPin), dirPin(_dirPin) @@ -15,11 +20,13 @@ Stepper::Stepper(const int _stepPin, const int _dirPin) Stepper &Stepper::setStepPinPolarity(int polarity) { + #ifdef ARDUINO_ARCH_ESP32 + this->polarity = polarity; + #elif // Calculate adresses of bitbanded pin-set and pin-clear registers uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[stepPin].reg; //GPIO_PDOR uint32_t *pinSetReg = (uint32_t *)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 uint32_t *pinClearReg = (uint32_t *)(pinRegAddr + 8 * 32); //GPIO_PCOR = GPIO_PDOR + 8 - // Assign registers according to step option if (polarity == LOW) { @@ -31,12 +38,16 @@ Stepper &Stepper::setStepPinPolarity(int polarity) stepPinActiveReg = pinSetReg; stepPinInactiveReg = pinClearReg; } + #endif clearStepPin(); // set step pin to inactive state return *this; } Stepper &Stepper::setInverseRotation(bool reverse) { + #ifdef ARDUINO_ARCH_ESP32 + this->reverse = reverse; + #elif // Calculate adresses of bitbanded pin-set and pin-clear registers uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[dirPin].reg; //GPIO_PDOR uint32_t *pinSetReg = (uint32_t *)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 @@ -52,6 +63,7 @@ Stepper &Stepper::setInverseRotation(bool reverse) dirPinCwReg = pinSetReg; dirPinCcwReg = pinClearReg; } + #endif return *this; } diff --git a/src/Stepper.h b/src/Stepper.h index 7e9a684..8e030d6 100644 --- a/src/Stepper.h +++ b/src/Stepper.h @@ -1,5 +1,8 @@ #pragma once +#ifdef ARDUINO_ARCH_ESP32 +#include "Arduino.h" +#endif #include #include @@ -48,10 +51,15 @@ class Stepper static bool cmpVmax(const Stepper *a, const Stepper *b) { return std::abs(a->vMax) > std::abs(b->vMax); } // Pin & Dir registers + #ifdef ARDUINO_ARCH_ESP32 + volatile uint8_t polarity; + volatile uint8_t reverse; + #elif volatile uint32_t *stepPinActiveReg; volatile uint32_t *stepPinInactiveReg; volatile uint32_t *dirPinCwReg; volatile uint32_t *dirPinCcwReg; + #endif const int stepPin, dirPin; // Friends @@ -67,11 +75,30 @@ class Stepper // Inline implementation ----------------------------------------- +#ifdef ARDUINO_ARCH_ESP32 +void Stepper::doStep() +{ + digitalWrite(stepPin, polarity); + current += dir; +} + +void Stepper::clearStepPin() const +{ + digitalWrite(stepPin, !polarity); +} + +void Stepper::setDir(int d) +{ + dir = d; + digitalWrite(dirPin, dir == 1 ? reverse : !reverse); +} +#elif void Stepper::doStep() { *stepPinActiveReg = 1; current += dir; } + void Stepper::clearStepPin() const { *stepPinInactiveReg = 1; @@ -82,8 +109,9 @@ void Stepper::setDir(int d) dir = d; dir == 1 ? *dirPinCwReg = 1 : *dirPinCcwReg = 1; } +#endif void Stepper::toggleDir() { setDir(-dir); -} +} \ No newline at end of file diff --git a/src/TeensyStep.h b/src/TeensyStep.h index 4538414..45bec44 100644 --- a/src/TeensyStep.h +++ b/src/TeensyStep.h @@ -9,7 +9,7 @@ #include "accelerators/LinStepAccelerator.h" //#include "accelerators/SinRotAccelerator.h" -#include "timer/generic/TimerField.h" +//#include "timer/generic/TimerField.h" // TEENSY 3.0 - Teensy 3.6 ================================================================================== @@ -25,12 +25,20 @@ //STM32 ==================================================================================================== #elif defined(__STM32_TBD__) -#include "timers/STM32/TimerField.h" +#include "timer/stm32/TimerField.h" + +//ESP32 ====================================================================================== + +#elif defined(ARDUINO_ARCH_ESP32) +#include "Arduino.h" +#include "timer/esp32/TimerField.h" //Some other hardware ====================================================================================== #elif defined(__someHardware_TBD__) #include "timers/someHardware/TimerField2.h" +using StepControlTick = StepControlBase; +using RotateControlTick = RotateControlBase; #endif // Linear acceleration ----------------------------------------------------------------------------------------- @@ -41,9 +49,7 @@ using MotorControl = MotorControlBase; using RotateControl = RotateControlBase; using StepControl = StepControlBase; -using StepControlTick = StepControlBase; -using RotateControlTick = RotateControlBase; - +extern void teensyStepSetup(); // Sine acceleration ------------------------------------------------------------------------------------------- // template diff --git a/src/accelerators/LinRotAccelerator.h b/src/accelerators/LinRotAccelerator.h index ec0c7a6..8391a03 100644 --- a/src/accelerators/LinRotAccelerator.h +++ b/src/accelerators/LinRotAccelerator.h @@ -1,6 +1,8 @@ #pragma once +#ifndef ARDUINO_ARCH_ESP32 #include "wiring.h" +#endif #include #include diff --git a/src/timer/esp32/TimerField.h b/src/timer/esp32/TimerField.h new file mode 100644 index 0000000..81edf26 --- /dev/null +++ b/src/timer/esp32/TimerField.h @@ -0,0 +1,97 @@ +#pragma once + +#include "Arduino.h" +#include +// //#include "imxrt.h" + +#include "../TF_Handler.h" + +// //========================= +// // ESP 32 +// //========================= + +// + +class TimerField +{ +public: + inline TimerField(TF_Handler *_handler); + + inline bool begin(); + inline void end(); + + inline void stepTimerStart(); + inline void stepTimerStop(); + inline bool stepTimerIsRunning() const { return stepTimerRunning; } + inline void setStepFrequency(unsigned f) { timerAlarmWrite(stepTimer, 1. / f, true); } + inline unsigned getStepFrequency() { return 0; } + + inline void accTimerStart() { timerAlarmEnable(accTimer); } + inline void accTimerStop() { timerAlarmDisable(accTimer); } + inline void setAccUpdatePeriod(unsigned period) { timerAlarmWrite(accTimer, period, true); } + + inline void setPulseWidth(unsigned delay) { timerAlarmWrite(stepTimer, delay, false); } + inline void triggerDelay() { timerAlarmEnable(delayTimer); } + + void setupStepTimer(void (*cb)(void)); + void setupAccTimer(void (*cb)(void)); + void setupDelayTimer(void (*cb)(void)); +protected: + TF_Handler *handler; + hw_timer_t *stepTimer; + hw_timer_t *accTimer; + hw_timer_t *delayTimer; + bool stepTimerRunning; +}; + +// IMPLEMENTATION ==================================================================== + +TimerField::TimerField(TF_Handler *_handler) + : handler(_handler), + stepTimer(timerBegin(0, 80, true)), + accTimer(timerBegin(1, 80, true)), + delayTimer(timerBegin(2, 80, true)), + stepTimerRunning(false) +{ +} + +void TimerField::setupStepTimer(void (*cb)(void)) +{ + timerAttachInterrupt(stepTimer, cb, true); + timerAlarmWrite(stepTimer, 1, true); +} + +void TimerField::setupAccTimer(void (*cb)(void)) +{ + timerAttachInterrupt(accTimer, cb, true); + timerAlarmWrite(accTimer, 1, true); +} + +void TimerField::setupDelayTimer(void (*cb)(void)) +{ + timerAttachInterrupt(delayTimer, cb, true); + timerAlarmWrite(delayTimer, 1, false); +} + +void TimerField::stepTimerStart() +{ + timerAlarmEnable(stepTimer); + stepTimerRunning = true; +} + +void TimerField::stepTimerStop() +{ + timerAlarmDisable(stepTimer); + stepTimerRunning = false; +} + +bool TimerField::begin() +{ + Serial.println("begin"); + return true; +} + +void TimerField::end() +{ + //TimerControl::end(); +} \ No newline at end of file diff --git a/src/timer/generic/TickTimer.cpp b/src/timer/generic/TickTimer.cpp index 766266a..5dc10d4 100644 --- a/src/timer/generic/TickTimer.cpp +++ b/src/timer/generic/TickTimer.cpp @@ -1,3 +1,4 @@ +#ifndef ARDUINO_ARCH_ESP32 #include "TickTimer.h" void std::__throw_bad_function_call() @@ -6,4 +7,5 @@ void std::__throw_bad_function_call() } TimerBase* TimerControl::firstTimer = nullptr; -TimerBase* TimerControl::lastTimer = nullptr; \ No newline at end of file +TimerBase* TimerControl::lastTimer = nullptr; +#endif diff --git a/src/timer/generic/TickTimer.h b/src/timer/generic/TickTimer.h index 478ae7d..fda1a4f 100644 --- a/src/timer/generic/TickTimer.h +++ b/src/timer/generic/TickTimer.h @@ -53,7 +53,7 @@ class PeriodicTimer : public TimerBase inline void setPeriod(uint32_t microSeconds) { - deltaCnt = F_CPU / 1'000'000 * microSeconds; + deltaCnt = F_CPU / 1000000 * microSeconds; } static constexpr float minFrequency = (float)F_CPU / std::numeric_limits::max(); @@ -69,7 +69,7 @@ class OneShotTimer : public TimerBase void setDelay(unsigned microSeconds) { - deltaCnt = F_CPU / 1'000'000 * microSeconds; + deltaCnt = F_CPU / 1000000 * microSeconds; } }; diff --git a/src/timer/generic/TimerField.h b/src/timer/generic/TimerField.h index 59be548..5a558e0 100644 --- a/src/timer/generic/TimerField.h +++ b/src/timer/generic/TimerField.h @@ -1,6 +1,8 @@ #pragma once +#ifndef ARDUINO_ARCH_ESP32 #include "wiring.h" +#endif // //#include "imxrt.h" #include "TickTimer.h" diff --git a/src/timer/teensy3/PIT.cpp b/src/timer/teensy3/PIT.cpp index 92cb8fc..3eb8375 100644 --- a/src/timer/teensy3/PIT.cpp +++ b/src/timer/teensy3/PIT.cpp @@ -1,6 +1,8 @@ +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) + #include "PIT.h" - - #include "../TF_Handler.h" + +#include "../TF_Handler.h" namespace { @@ -58,4 +60,5 @@ void PIT::setupChannel() channel = nullptr; break; } -} \ No newline at end of file +} +#endif diff --git a/src/timer/teensy3/PIT.h b/src/timer/teensy3/PIT.h index e438e16..92d315a 100644 --- a/src/timer/teensy3/PIT.h +++ b/src/timer/teensy3/PIT.h @@ -33,4 +33,4 @@ class PIT IntervalTimer timer; void setupChannel(); -}; \ No newline at end of file +}; diff --git a/src/version.h b/src/version.h index ab3ef95..cb250b6 100644 --- a/src/version.h +++ b/src/version.h @@ -1,7 +1,7 @@ namespace TeensyStep { -constexpr const char *version = "V2.0.0"; -constexpr const char *branch = "master"; +constexpr const char *version = "V2.0.1"; +constexpr const char *branch = "esp32"; } // namespace TeensyStep \ No newline at end of file From a9ba4cce8a3bf334b7a4cb92c79038c65af27f6a Mon Sep 17 00:00:00 2001 From: Robin Petereit Date: Sun, 23 Feb 2020 22:44:45 +0100 Subject: [PATCH 02/35] Make handler static for ISR wrapping --- src/MotorControlBase.h | 7 ------- src/TeensyStep.h | 7 +++---- src/timer/esp32/TimerField.cpp | 3 +++ src/timer/esp32/TimerField.h | 30 +++++++----------------------- 4 files changed, 13 insertions(+), 34 deletions(-) create mode 100644 src/timer/esp32/TimerField.cpp diff --git a/src/MotorControlBase.h b/src/MotorControlBase.h index 1558bcb..9f3f710 100644 --- a/src/MotorControlBase.h +++ b/src/MotorControlBase.h @@ -20,13 +20,6 @@ class MotorControlBase : TF_Handler virtual ~MotorControlBase(); bool isOk() const { return OK; } - void setup(void (*stepCallback)(void), void (*accCallback)(void), void (*delayCallback)(void)) - { - timerField.setupStepTimer(stepCallback); - timerField.setupAccTimer(accCallback); - timerField.setupDelayTimer(delayCallback); - } - void stepTimerISR(); void pulseTimerISR(); diff --git a/src/TeensyStep.h b/src/TeensyStep.h index 45bec44..8f6f8c3 100644 --- a/src/TeensyStep.h +++ b/src/TeensyStep.h @@ -22,18 +22,18 @@ #elif defined(__IMXRT1052__) #include "timer/teensy4/TimerField.h" -//STM32 ==================================================================================================== +// STM32 ==================================================================================================== #elif defined(__STM32_TBD__) #include "timer/stm32/TimerField.h" -//ESP32 ====================================================================================== +// ESP32 ====================================================================================== #elif defined(ARDUINO_ARCH_ESP32) #include "Arduino.h" #include "timer/esp32/TimerField.h" -//Some other hardware ====================================================================================== +// Some other hardware ====================================================================================== #elif defined(__someHardware_TBD__) #include "timers/someHardware/TimerField2.h" @@ -49,7 +49,6 @@ using MotorControl = MotorControlBase; using RotateControl = RotateControlBase; using StepControl = StepControlBase; -extern void teensyStepSetup(); // Sine acceleration ------------------------------------------------------------------------------------------- // template diff --git a/src/timer/esp32/TimerField.cpp b/src/timer/esp32/TimerField.cpp new file mode 100644 index 0000000..9574a78 --- /dev/null +++ b/src/timer/esp32/TimerField.cpp @@ -0,0 +1,3 @@ +#include "TimerField.h" + +TF_Handler* TimerField::handler; diff --git a/src/timer/esp32/TimerField.h b/src/timer/esp32/TimerField.h index 81edf26..24ebdd0 100644 --- a/src/timer/esp32/TimerField.h +++ b/src/timer/esp32/TimerField.h @@ -10,8 +10,6 @@ // // ESP 32 // //========================= -// - class TimerField { public: @@ -33,11 +31,9 @@ class TimerField inline void setPulseWidth(unsigned delay) { timerAlarmWrite(stepTimer, delay, false); } inline void triggerDelay() { timerAlarmEnable(delayTimer); } - void setupStepTimer(void (*cb)(void)); - void setupAccTimer(void (*cb)(void)); - void setupDelayTimer(void (*cb)(void)); + protected: - TF_Handler *handler; + static TF_Handler* handler; hw_timer_t *stepTimer; hw_timer_t *accTimer; hw_timer_t *delayTimer; @@ -47,29 +43,17 @@ class TimerField // IMPLEMENTATION ==================================================================== TimerField::TimerField(TF_Handler *_handler) - : handler(_handler), - stepTimer(timerBegin(0, 80, true)), + : stepTimer(timerBegin(0, 80, true)), accTimer(timerBegin(1, 80, true)), delayTimer(timerBegin(2, 80, true)), stepTimerRunning(false) { -} - -void TimerField::setupStepTimer(void (*cb)(void)) -{ - timerAttachInterrupt(stepTimer, cb, true); + handler = _handler; + timerAttachInterrupt(stepTimer,[]{handler->stepTimerISR();}, true); timerAlarmWrite(stepTimer, 1, true); -} - -void TimerField::setupAccTimer(void (*cb)(void)) -{ - timerAttachInterrupt(accTimer, cb, true); + timerAttachInterrupt(accTimer, []{handler->accTimerISR();}, true); timerAlarmWrite(accTimer, 1, true); -} - -void TimerField::setupDelayTimer(void (*cb)(void)) -{ - timerAttachInterrupt(delayTimer, cb, true); + timerAttachInterrupt(delayTimer, []{handler->pulseTimerISR();}, true); timerAlarmWrite(delayTimer, 1, false); } From 490dd85d9bd4721cdfb3a0b2b490b83d886431c4 Mon Sep 17 00:00:00 2001 From: Robin Petereit Date: Mon, 24 Feb 2020 11:18:15 +0100 Subject: [PATCH 03/35] Switch to non FPU square roots (FPU not supported in interrupt on ESP32) --- src/accelerators/LinStepAccelerator.h | 4 ++-- src/accelerators/SinRotAccelerator.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/accelerators/LinStepAccelerator.h b/src/accelerators/LinStepAccelerator.h index 4ca4342..6b6285b 100644 --- a/src/accelerators/LinStepAccelerator.h +++ b/src/accelerators/LinStepAccelerator.h @@ -48,7 +48,7 @@ int32_t LinStepAccelerator::updateSpeed(int32_t curPos) // acceleration phase ------------------------------------- if (stepsDone < accLength) - return sqrtf(two_a * stepsDone + v_min2); + return sqrt(two_a * stepsDone + v_min2); // constant speed phase ------------------------------------ if (stepsDone < decStart) @@ -56,7 +56,7 @@ int32_t LinStepAccelerator::updateSpeed(int32_t curPos) //deceleration phase -------------------------------------- if(stepsDone < delta_tgt) - return sqrtf(two_a * ((stepsDone < delta_tgt - 1) ? delta_tgt - stepsDone - 2 : 0) + v_min2); + return sqrt(two_a * ((stepsDone < delta_tgt - 1) ? delta_tgt - stepsDone - 2 : 0) + v_min2); //we are done, make sure to return 0 to stop the step timer return 0; diff --git a/src/accelerators/SinRotAccelerator.h b/src/accelerators/SinRotAccelerator.h index 0979540..00886d1 100644 --- a/src/accelerators/SinRotAccelerator.h +++ b/src/accelerators/SinRotAccelerator.h @@ -29,7 +29,7 @@ class SinRotAccelerator inline float signed_sqrt(int32_t x) // signed square root { - return x > 0 ? sqrtf(x) : -sqrtf(-x); + return x > 0 ? sqrt(x) : -sqrt(-x); } }; From 045387639647c087e1f9d2130da6308b6e189dbb Mon Sep 17 00:00:00 2001 From: Robin Petereit Date: Mon, 24 Feb 2020 11:19:05 +0100 Subject: [PATCH 04/35] Update pin mode before setting pin --- src/Stepper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Stepper.cpp b/src/Stepper.cpp index 457cf81..bb6a1bf 100644 --- a/src/Stepper.cpp +++ b/src/Stepper.cpp @@ -9,13 +9,13 @@ Stepper::Stepper(const int _stepPin, const int _dirPin) : current(0), stepPin(_stepPin), dirPin(_dirPin) { + pinMode(stepPin, OUTPUT); + pinMode(dirPin, OUTPUT); + setStepPinPolarity(HIGH); setInverseRotation(false); setAcceleration(aDefault); setMaxSpeed(vMaxDefault); - - pinMode(stepPin, OUTPUT); - pinMode(dirPin, OUTPUT); } Stepper &Stepper::setStepPinPolarity(int polarity) From 4482a9bc3fff9160dd50470c10855fd57afbc053 Mon Sep 17 00:00:00 2001 From: Robin Petereit Date: Mon, 24 Feb 2020 11:36:59 +0100 Subject: [PATCH 05/35] Update ESP32 timer with mux --- src/timer/esp32/TimerField.cpp | 1 + src/timer/esp32/TimerField.h | 40 ++++++++++++++++++++++------------ 2 files changed, 27 insertions(+), 14 deletions(-) diff --git a/src/timer/esp32/TimerField.cpp b/src/timer/esp32/TimerField.cpp index 9574a78..6aa8ab2 100644 --- a/src/timer/esp32/TimerField.cpp +++ b/src/timer/esp32/TimerField.cpp @@ -1,3 +1,4 @@ #include "TimerField.h" TF_Handler* TimerField::handler; +portMUX_TYPE TimerField::timerMux = portMUX_INITIALIZER_UNLOCKED; diff --git a/src/timer/esp32/TimerField.h b/src/timer/esp32/TimerField.h index 24ebdd0..6f58cbb 100644 --- a/src/timer/esp32/TimerField.h +++ b/src/timer/esp32/TimerField.h @@ -2,7 +2,6 @@ #include "Arduino.h" #include -// //#include "imxrt.h" #include "../TF_Handler.h" @@ -20,24 +19,26 @@ class TimerField inline void stepTimerStart(); inline void stepTimerStop(); + inline void setStepFrequency(unsigned f); + inline unsigned getStepFrequency() { return timerAlarmRead(stepTimer); } inline bool stepTimerIsRunning() const { return stepTimerRunning; } - inline void setStepFrequency(unsigned f) { timerAlarmWrite(stepTimer, 1. / f, true); } - inline unsigned getStepFrequency() { return 0; } inline void accTimerStart() { timerAlarmEnable(accTimer); } inline void accTimerStop() { timerAlarmDisable(accTimer); } inline void setAccUpdatePeriod(unsigned period) { timerAlarmWrite(accTimer, period, true); } - inline void setPulseWidth(unsigned delay) { timerAlarmWrite(stepTimer, delay, false); } - inline void triggerDelay() { timerAlarmEnable(delayTimer); } + inline void triggerDelay() { timerAlarmEnable(pulseTimer); } + inline void setPulseWidth(unsigned pulseWidth) { timerAlarmWrite(pulseTimer, pulseWidth, false); } + static portMUX_TYPE timerMux; protected: - static TF_Handler* handler; + static TF_Handler *handler; hw_timer_t *stepTimer; hw_timer_t *accTimer; - hw_timer_t *delayTimer; + hw_timer_t *pulseTimer; bool stepTimerRunning; + unsigned lastF; }; // IMPLEMENTATION ==================================================================== @@ -45,16 +46,17 @@ class TimerField TimerField::TimerField(TF_Handler *_handler) : stepTimer(timerBegin(0, 80, true)), accTimer(timerBegin(1, 80, true)), - delayTimer(timerBegin(2, 80, true)), - stepTimerRunning(false) + pulseTimer(timerBegin(2, 80, true)), + stepTimerRunning(false), + lastF(0) { handler = _handler; - timerAttachInterrupt(stepTimer,[]{handler->stepTimerISR();}, true); + timerAttachInterrupt(stepTimer, [] { handler->stepTimerISR(); }, true); timerAlarmWrite(stepTimer, 1, true); - timerAttachInterrupt(accTimer, []{handler->accTimerISR();}, true); + timerAttachInterrupt(accTimer, [] { handler->accTimerISR(); }, true); timerAlarmWrite(accTimer, 1, true); - timerAttachInterrupt(delayTimer, []{handler->pulseTimerISR();}, true); - timerAlarmWrite(delayTimer, 1, false); + timerAttachInterrupt(pulseTimer, [] { handler->pulseTimerISR(); }, true); + timerAlarmWrite(pulseTimer, 1, false); } void TimerField::stepTimerStart() @@ -69,6 +71,14 @@ void TimerField::stepTimerStop() stepTimerRunning = false; } +void TimerField::setStepFrequency(unsigned f) +{ + if (f != 0) + timerAlarmWrite(stepTimer, 1000000 / f, true); // Timer runs at 1 microsecond interval + else + stepTimerStop(); +} + bool TimerField::begin() { Serial.println("begin"); @@ -77,5 +87,7 @@ bool TimerField::begin() void TimerField::end() { - //TimerControl::end(); + timerAlarmDisable(stepTimer); + timerAlarmDisable(accTimer); + timerAlarmDisable(pulseTimer); } \ No newline at end of file From 480c22929bf4c78aa0ccd5143b041672e7c6ca69 Mon Sep 17 00:00:00 2001 From: RCP1 Date: Mon, 24 Feb 2020 12:31:23 +0100 Subject: [PATCH 06/35] Update README for ESP32 --- README.md | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 40e340b..fbfd4da 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,9 @@ -# TeensyStep V2 - Fast Stepper Library for PJRC Teensy boards - -Please note: This is version 2 of the library. This version has a new user interface. The version 1 is still available in the branch [Version-1](https://github.com/luni64/TeensyStep/tree/Version-1). - - -## Detailed Documentation can be found here [https://luni64.github.io/TeensyStep/](https://luni64.github.io/TeensyStep/) +# ESP32Step - Fast Stepper Library for ESP32 boards +Fork of [TeensyStep V2 library](https://luni64.github.io/TeensyStep/) adopted for ESP32 boards. It uses the timers 0, 1, 2 via the esp32-arduino interface. ## Purpose of the Library -**TeensyStep** is an efficient Arduino library compatible with Teensy 3.0, 3.1, 3.2, 3.5 and 3.6. The library is able to handle synchronous and independent movement and continuous rotation of steppers with pulse rates of up to 300'000 steps per second. The following table shows a summary of the **TeensyStep** specification: +**ESP32Step** is an efficient Arduino library compatible with ESP32 and Teensy 3.0, 3.1, 3.2, 3.5 and 3.6. The library is able to handle synchronous and independent movement and continuous rotation of steppers with pulse rates of up to 300'000 steps per second. The following table shows a summary of the **ESP32Step** specifications (taken over from TeensyStep, not yet verified for ESP32): | Description | Specification | Default | |:-------------------------------------------|:-------------------------:|:----------------:| @@ -20,9 +16,5 @@ Please note: This is version 2 of the library. This version has a new user inter | Settable step pulse width | 1-100µs | 5µs | | Settable direction signal polarity | cw / ccw | cw | -Here a quick demonstration video showing two motors running in sync with 160'000 steps/sec - - - +## Architecture +The base idea is to use one reloading interval timer for setting the step pin at a stepper velocity based frequency, one non-reloading (one shot) timer for clearing the step pin and thus defining the pulse width, and one (slower) reloading interval timer for updating the step timer frequency for acceleration / deceleration. \ No newline at end of file From ea108bb95b6f395dc7551356fa6f5e498f673ffc Mon Sep 17 00:00:00 2001 From: RCP1 Date: Wed, 11 Mar 2020 23:07:10 +0100 Subject: [PATCH 07/35] Update README.md --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index fbfd4da..9f37100 100644 --- a/README.md +++ b/README.md @@ -17,4 +17,7 @@ Fork of [TeensyStep V2 library](https://luni64.github.io/TeensyStep/) adopted fo | Settable direction signal polarity | cw / ccw | cw | ## Architecture -The base idea is to use one reloading interval timer for setting the step pin at a stepper velocity based frequency, one non-reloading (one shot) timer for clearing the step pin and thus defining the pulse width, and one (slower) reloading interval timer for updating the step timer frequency for acceleration / deceleration. \ No newline at end of file +The base idea is to use one reloading interval timer for setting the step pin at a stepper velocity based frequency, one non-reloading (one shot) timer for clearing the step pin and thus defining the pulse width, and one (slower) reloading interval timer for updating the step timer frequency for acceleration / deceleration. + +## TODO +Currently, the ESP32 timers 0, 1, 2 are hard-coded. In consequence only one motor controller can run at the same time. I'll wait for an update of TeensyStep for a more general timer interface (e.g. [TeensyTimerTool](https://github.com/luni64/TeensyTimerTool)). From 544afe749791927e713b778fc1c51f7a74711cde Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 11:23:27 +0100 Subject: [PATCH 08/35] Working test code both teensy and stm32, stm32 is however atm much slower at startup and produces at least one extra step --- .gitignore | 6 ++- platformio.ini | 31 +++++++++++ src/RotateControlBase.h | 32 ++++++------ src/Stepper.cpp | 16 +++--- src/Stepper.h | 31 +++++------ src/TeensyStep.h | 2 +- src/main.cpp | 25 +++++++++ src/timer/generic/TickTimer.cpp | 1 + src/timer/generic/TickTimer.h | 34 +++++++----- src/timer/stm32/TimerField.cpp | 3 ++ src/timer/stm32/TimerField.h | 92 +++++++++++++++++++++++++++++++++ 11 files changed, 218 insertions(+), 55 deletions(-) create mode 100644 platformio.ini create mode 100644 src/main.cpp create mode 100644 src/timer/stm32/TimerField.cpp create mode 100644 src/timer/stm32/TimerField.h diff --git a/.gitignore b/.gitignore index c0e9de0..b166132 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ -# Windows image file caches +# Windows image file caches Thumbs.db ehthumbs.db @@ -52,3 +52,7 @@ $RECYCLE.BIN/ Network Trash Folder Temporary Items .apdisk + + +.pio +.vscode \ No newline at end of file diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..4845840 --- /dev/null +++ b/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 + +[env:nucleo_f429zi] +platform = ststm32 +board = nucleo_f429zi +framework = arduino +platform_packages = framework-arduinoststm32@https://github.com/stm32duino/Arduino_Core_STM32 +src_filter = + +<*> + - + - + - + +[env:teensy35] +platform = teensy +board = teensy35 +build_flags = -DTEENSY +framework = arduino +src_filter = + +<*> + - + - + - \ No newline at end of file diff --git a/src/RotateControlBase.h b/src/RotateControlBase.h index a592d1c..7f2f93f 100644 --- a/src/RotateControlBase.h +++ b/src/RotateControlBase.h @@ -3,7 +3,7 @@ #include "MotorControlBase.h" #include -#ifndef ARDUINO_ARCH_ESP32 +#ifdef TEENSY #include "core_pins.h" #endif @@ -11,12 +11,12 @@ template class RotateControlBase : public MotorControlBase { public: - RotateControlBase(unsigned pulseWidth = 5, unsigned accUpdatePeriod = 5000); + RotateControlBase(unsigned pulseWidth = 5, unsigned accUpdatePeriod = 5000); // Non-blocking movements ---------------- template void rotateAsync(Steppers &... steppers); - + template void rotateAsync(Stepper *(&motors)[N]); @@ -62,7 +62,7 @@ void RotateControlBase::doRotate(int N, float speedFactor) if (this->leadMotor->vMax == 0) return; - this->leadMotor->currentSpeed = 0; + this->leadMotor->currentSpeed = 0; this->leadMotor->A = std::abs(this->leadMotor->vMax); for (int i = 1; i < N; i++) @@ -71,24 +71,24 @@ void RotateControlBase::doRotate(int N, float speedFactor) this->motorList[i]->B = 2 * this->motorList[i]->A - this->leadMotor->A; } uint32_t acceleration = (*std::min_element(this->motorList, this->motorList + N, Stepper::cmpAcc))->a; // use the lowest acceleration for the move - - // Start moving--------------------------------------------------------------------------------------- + + // Start moving--------------------------------------------------------------------------------------- accelerator.prepareRotation(this->leadMotor->current, this->leadMotor->vMax, acceleration, this->accUpdatePeriod, speedFactor); - this->timerField.setStepFrequency(0); - this->timerField.accTimerStart(); + this->timerField.setStepFrequency(0); + this->timerField.accTimerStart(); } // ISR ----------------------------------------------------------------------------------------------------------- template void RotateControlBase::accTimerISR() -{ +{ int32_t newSpeed = accelerator.updateSpeed(this->leadMotor->current); // get new speed for the leading motor - + //Serial.printf("rc,curSpeed: %i newspd:%i\n",this->leadMotor->currentSpeed, newSpeed); if (this->leadMotor->currentSpeed == newSpeed) - { + { return; // nothing changed, just keep running } @@ -102,10 +102,10 @@ void RotateControlBase::accTimerISR() } delayMicroseconds(this->pulseWidth); } - - - this->timerField.setStepFrequency(std::abs(newSpeed)); // speed changed, update timer - this->leadMotor->currentSpeed = newSpeed; + + + this->timerField.setStepFrequency(std::abs(newSpeed)); // speed changed, update timer + this->leadMotor->currentSpeed = newSpeed; } // ROTATE Commands ------------------------------------------------------------------------------- @@ -120,7 +120,7 @@ void RotateControlBase::rotateAsync(Steppers &... steppers) template template -void RotateControlBase::rotateAsync(Stepper *(&steppers)[N]) +void RotateControlBase::rotateAsync(Stepper *(&steppers)[N]) { this->attachStepper(steppers); doRotate(N); diff --git a/src/Stepper.cpp b/src/Stepper.cpp index bb6a1bf..666ac11 100644 --- a/src/Stepper.cpp +++ b/src/Stepper.cpp @@ -2,8 +2,8 @@ #ifdef ARDUINO_ARCH_ESP32 #include "esp32-hal-gpio.h" -#elif -#include "core_pins.h" +#else +//#include "core_pins.h" #endif Stepper::Stepper(const int _stepPin, const int _dirPin) @@ -20,9 +20,7 @@ Stepper::Stepper(const int _stepPin, const int _dirPin) Stepper &Stepper::setStepPinPolarity(int polarity) { - #ifdef ARDUINO_ARCH_ESP32 - this->polarity = polarity; - #elif + #ifdef TEENSY // Calculate adresses of bitbanded pin-set and pin-clear registers uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[stepPin].reg; //GPIO_PDOR uint32_t *pinSetReg = (uint32_t *)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 @@ -38,6 +36,8 @@ Stepper &Stepper::setStepPinPolarity(int polarity) stepPinActiveReg = pinSetReg; stepPinInactiveReg = pinClearReg; } + #else + this->polarity = polarity; #endif clearStepPin(); // set step pin to inactive state return *this; @@ -45,9 +45,7 @@ Stepper &Stepper::setStepPinPolarity(int polarity) Stepper &Stepper::setInverseRotation(bool reverse) { - #ifdef ARDUINO_ARCH_ESP32 - this->reverse = reverse; - #elif + #ifdef TEENSY // Calculate adresses of bitbanded pin-set and pin-clear registers uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[dirPin].reg; //GPIO_PDOR uint32_t *pinSetReg = (uint32_t *)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 @@ -63,6 +61,8 @@ Stepper &Stepper::setInverseRotation(bool reverse) dirPinCwReg = pinSetReg; dirPinCcwReg = pinClearReg; } + #else + this->reverse = reverse; #endif return *this; } diff --git a/src/Stepper.h b/src/Stepper.h index 8e030d6..b4519e8 100644 --- a/src/Stepper.h +++ b/src/Stepper.h @@ -1,8 +1,6 @@ #pragma once -#ifdef ARDUINO_ARCH_ESP32 -#include "Arduino.h" -#endif +#include #include #include @@ -37,7 +35,7 @@ class Stepper inline void toggleDir(); volatile int32_t current; - volatile int32_t currentSpeed; + volatile int32_t currentSpeed; volatile int32_t target; int32_t A, B; // Bresenham paramters @@ -51,14 +49,14 @@ class Stepper static bool cmpVmax(const Stepper *a, const Stepper *b) { return std::abs(a->vMax) > std::abs(b->vMax); } // Pin & Dir registers - #ifdef ARDUINO_ARCH_ESP32 - volatile uint8_t polarity; - volatile uint8_t reverse; - #elif + #ifdef TEENSY volatile uint32_t *stepPinActiveReg; volatile uint32_t *stepPinInactiveReg; volatile uint32_t *dirPinCwReg; volatile uint32_t *dirPinCcwReg; + #else + volatile uint8_t polarity; + volatile uint8_t reverse; #endif const int stepPin, dirPin; @@ -74,40 +72,39 @@ class Stepper }; // Inline implementation ----------------------------------------- - -#ifdef ARDUINO_ARCH_ESP32 +#if TEENSY void Stepper::doStep() { - digitalWrite(stepPin, polarity); + *stepPinActiveReg = 1; current += dir; } void Stepper::clearStepPin() const { - digitalWrite(stepPin, !polarity); + *stepPinInactiveReg = 1; } void Stepper::setDir(int d) { dir = d; - digitalWrite(dirPin, dir == 1 ? reverse : !reverse); + dir == 1 ? *dirPinCwReg = 1 : *dirPinCcwReg = 1; } -#elif +#else void Stepper::doStep() { - *stepPinActiveReg = 1; + digitalWrite(stepPin, polarity); current += dir; } void Stepper::clearStepPin() const { - *stepPinInactiveReg = 1; + digitalWrite(stepPin, !polarity); } void Stepper::setDir(int d) { dir = d; - dir == 1 ? *dirPinCwReg = 1 : *dirPinCcwReg = 1; + digitalWrite(dirPin, dir == 1 ? reverse : !reverse); } #endif diff --git a/src/TeensyStep.h b/src/TeensyStep.h index 8f6f8c3..12c5123 100644 --- a/src/TeensyStep.h +++ b/src/TeensyStep.h @@ -24,7 +24,7 @@ // STM32 ==================================================================================================== -#elif defined(__STM32_TBD__) +#elif defined(STM32F4xx) #include "timer/stm32/TimerField.h" // ESP32 ====================================================================================== diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..0e28964 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,25 @@ +#include +#include "TeensyStep.h" + +#define STATUS_PIN PIN_A0 + +Stepper motor(PIN_A1, PIN_A2); // STEP pin: 2, DIR pin: 3 +StepControl controller; // Use default settings + +bool led_status = false; + +void setup(){ + Serial.begin(115200); + pinMode(LED_BUILTIN, OUTPUT); + pinMode(STATUS_PIN, OUTPUT); + digitalWrite(LED_BUILTIN, led_status); + digitalWrite(STATUS_PIN, led_status); +} + +void loop(){ + led_status = !led_status; + digitalWrite(LED_BUILTIN, led_status); + digitalWrite(STATUS_PIN, led_status); + motor.setTargetRel(1); // Set target position to 1000 steps from current position + controller.move(motor); // Do the move +} \ No newline at end of file diff --git a/src/timer/generic/TickTimer.cpp b/src/timer/generic/TickTimer.cpp index 5dc10d4..ee809cc 100644 --- a/src/timer/generic/TickTimer.cpp +++ b/src/timer/generic/TickTimer.cpp @@ -6,6 +6,7 @@ void std::__throw_bad_function_call() while(1); } +float PeriodicTimer::minFrequency = (float)F_CPU / std::numeric_limits::max(); TimerBase* TimerControl::firstTimer = nullptr; TimerBase* TimerControl::lastTimer = nullptr; #endif diff --git a/src/timer/generic/TickTimer.h b/src/timer/generic/TickTimer.h index fda1a4f..80690db 100644 --- a/src/timer/generic/TickTimer.h +++ b/src/timer/generic/TickTimer.h @@ -1,6 +1,6 @@ #pragma once -#include "Arduino.h" +#include #include #include @@ -21,7 +21,15 @@ class TimerBase prev = next = nullptr; }; - inline void start() { startCnt = ARM_DWT_CYCCNT; run = true; } + static inline uint32_t get_cycles(){ + #ifdef TEENSY + return ARM_DWT_CYCCNT; + #else + return dwt_getCycles(); + #endif + } + + inline void start() { startCnt = get_cycles(); run = true; } inline void stop() { run = false; } inline bool isRunning() const {return run;} @@ -39,9 +47,9 @@ class TimerBase class PeriodicTimer : public TimerBase { public: - PeriodicTimer(callback_t cb) : TimerBase(cb, true) + PeriodicTimer(callback_t cb) : TimerBase(cb, true) { - + } inline void setFrequency(float hz) @@ -56,13 +64,13 @@ class PeriodicTimer : public TimerBase deltaCnt = F_CPU / 1000000 * microSeconds; } - static constexpr float minFrequency = (float)F_CPU / std::numeric_limits::max(); + static float minFrequency; }; class OneShotTimer : public TimerBase { public: - OneShotTimer(callback_t cb, unsigned delay = 0) : TimerBase(cb, false) + OneShotTimer(callback_t cb, unsigned delay = 0) : TimerBase(cb, false) { setDelay(delay); } @@ -78,8 +86,10 @@ class TimerControl public: static void begin() { + #ifdef TEENSY ARM_DEMCR |= ARM_DEMCR_TRCENA; ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA; + #endif } static void attachTimer(TimerBase *timer) @@ -129,19 +139,19 @@ class TimerControl } static inline void tick() - { - digitalWriteFast(2,HIGH) ; + { + digitalWrite(2,HIGH); TimerBase *timer = firstTimer; - + while (timer != nullptr) { //Serial.printf("cnt: %d\n", ARM_DWT_CYCCNT ); - if (timer->run && (ARM_DWT_CYCCNT - timer->startCnt >= timer->deltaCnt)) + if (timer->run && (TimerBase::get_cycles() - timer->startCnt >= timer->deltaCnt)) { if (timer->isPeriodic) - timer->startCnt = ARM_DWT_CYCCNT; + timer->startCnt = TimerBase::get_cycles(); else timer->run = false; @@ -150,7 +160,7 @@ class TimerControl timer = timer->next; } - digitalWriteFast(2,LOW) ; + digitalWrite(2,LOW); } protected: diff --git a/src/timer/stm32/TimerField.cpp b/src/timer/stm32/TimerField.cpp new file mode 100644 index 0000000..9574a78 --- /dev/null +++ b/src/timer/stm32/TimerField.cpp @@ -0,0 +1,3 @@ +#include "TimerField.h" + +TF_Handler* TimerField::handler; diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h new file mode 100644 index 0000000..fea7f36 --- /dev/null +++ b/src/timer/stm32/TimerField.h @@ -0,0 +1,92 @@ +#pragma once + +#include +#include + +#include "../TF_Handler.h" + +class TimerField +{ +public: + inline TimerField(TF_Handler *_handler); + + inline bool begin(); + inline void end(); + + inline void stepTimerStart(); + inline void stepTimerStop(); + inline void setStepFrequency(unsigned f); + inline unsigned getStepFrequency() { return stepTimer.getTimerClkFreq(); } // clock of timer or overflow? + inline bool stepTimerIsRunning() const { return stepTimerRunning; } + + inline void accTimerStart() { accTimer.resume(); } + inline void accTimerStop() { accTimer.pause(); } + inline void setAccUpdatePeriod(unsigned period) { accTimer.setOverflow(period, MICROSEC_FORMAT); } //timerAlarmWrite(accTimer, period, true); } + + inline void triggerDelay() { pulseTimer.resume(); } + inline void setPulseWidth(unsigned pulseWidth) { pulseTimer.setOverflow(pulseWidth, MICROSEC_FORMAT); } // pause/stop or configure on the go works? + +protected: + static TF_Handler *handler; + HardwareTimer stepTimer; + HardwareTimer accTimer; + HardwareTimer pulseTimer; + bool stepTimerRunning; + unsigned lastF; +}; + +// IMPLEMENTATION ==================================================================== + +TimerField::TimerField(TF_Handler *_handler) : + stepTimer(TIM7), + accTimer(TIM8), + pulseTimer(TIM9), + stepTimerRunning(false), + lastF(0) +{ + handler = _handler; + stepTimer.attachInterrupt([] { handler->stepTimerISR(); }); + accTimer.attachInterrupt([] { handler->accTimerISR(); }); + pulseTimer.attachInterrupt([] { handler->pulseTimerISR(); }); +} + +void TimerField::stepTimerStart() +{ + stepTimer.resume(); + stepTimerRunning = true; +} + +void TimerField::stepTimerStop() +{ + stepTimer.pause(); + stepTimerRunning = false; +} + +void TimerField::setStepFrequency(unsigned f) +{ + if(f == 0){ + stepTimerStop(); + return; + } + stepTimer.setOverflow(f, HERTZ_FORMAT); + // stepTimer.refresh(); + /* + m_timer.pause(); + m_timer.setOverflow(micros, MICROSEC_FORMAT); + m_timer.refresh(); + m_timer.resume(); + */ +} + +bool TimerField::begin() +{ + Serial.println("begin"); + return true; +} + +void TimerField::end() +{ + stepTimer.pause(); + accTimer.pause(); + pulseTimer.pause(); +} \ No newline at end of file From a1b9059c7f1aac8674373785161297e7bed8342f Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 13:13:50 +0100 Subject: [PATCH 09/35] Fixed static constexpr variables that didn't compile with debugger --- src/Stepper.cpp | 6 ++++++ src/Stepper.h | 10 +++++----- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/Stepper.cpp b/src/Stepper.cpp index 666ac11..38ec702 100644 --- a/src/Stepper.cpp +++ b/src/Stepper.cpp @@ -6,6 +6,12 @@ //#include "core_pins.h" #endif +const int32_t Stepper::vMaxMax = 300000; // largest speed possible (steps/s) +const uint32_t Stepper::aMax = 500000; // speed up to 500kHz within 1 s (steps/s^2) +const uint32_t Stepper::vMaxDefault = 800; // should work with every motor (1 rev/sec in 1/4-step mode) +const uint32_t Stepper::aDefault = 2500; // reasonably low (~0.5s for reaching the default speed) + + Stepper::Stepper(const int _stepPin, const int _dirPin) : current(0), stepPin(_stepPin), dirPin(_dirPin) { diff --git a/src/Stepper.h b/src/Stepper.h index b4519e8..74f4caf 100644 --- a/src/Stepper.h +++ b/src/Stepper.h @@ -6,12 +6,12 @@ class Stepper { - static constexpr int32_t vMaxMax = 300000; // largest speed possible (steps/s) - static constexpr uint32_t aMax = 500000; // speed up to 500kHz within 1 s (steps/s^2) - static constexpr uint32_t vMaxDefault = 800; // should work with every motor (1 rev/sec in 1/4-step mode) - static constexpr uint32_t aDefault = 2500; // reasonably low (~0.5s for reaching the default speed) - public: + static const int32_t vMaxMax; // largest speed possible (steps/s) + static const uint32_t aMax; // speed up to 500kHz within 1 s (steps/s^2) + static const uint32_t vMaxDefault; // should work with every motor (1 rev/sec in 1/4-step mode) + static const uint32_t aDefault; // reasonably low (~0.5s for reaching the default speed) + Stepper(const int StepPin, const int DirPin); Stepper &setMaxSpeed(int32_t speed); // steps/s From 5865d6fbeb79969d3ccc5f72cc95741b119befc9 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 13:14:59 +0100 Subject: [PATCH 10/35] Changed test program to make it more clear to use with logic analyzer --- src/main.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 0e28964..edec318 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,20 +6,29 @@ Stepper motor(PIN_A1, PIN_A2); // STEP pin: 2, DIR pin: 3 StepControl controller; // Use default settings -bool led_status = false; - void setup(){ Serial.begin(115200); pinMode(LED_BUILTIN, OUTPUT); pinMode(STATUS_PIN, OUTPUT); - digitalWrite(LED_BUILTIN, led_status); - digitalWrite(STATUS_PIN, led_status); + digitalWrite(LED_BUILTIN, LOW); + digitalWrite(STATUS_PIN, LOW); + + #ifdef TEENSY + Serial.println("TEENSY"); + #else + Serial.println("STM32F4"); + #endif } void loop(){ - led_status = !led_status; - digitalWrite(LED_BUILTIN, led_status); - digitalWrite(STATUS_PIN, led_status); - motor.setTargetRel(1); // Set target position to 1000 steps from current position + delay(500); + digitalWrite(LED_BUILTIN, HIGH); + digitalWrite(STATUS_PIN, HIGH); + + motor.setTargetRel(10); // Set target position to 1000 steps from current position controller.move(motor); // Do the move + while(controller.isRunning()); + + digitalWrite(LED_BUILTIN, LOW); + digitalWrite(STATUS_PIN, LOW); } \ No newline at end of file From d8bc495d0d90af9cf204a01abbc68d74a1675f12 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 13:15:35 +0100 Subject: [PATCH 11/35] Fixed too aggressive optimization with variables used in interrupts --- src/timer/stm32/TimerField.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index fea7f36..f585ec1 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -31,8 +31,8 @@ class TimerField HardwareTimer stepTimer; HardwareTimer accTimer; HardwareTimer pulseTimer; - bool stepTimerRunning; - unsigned lastF; + volatile bool stepTimerRunning; + volatile unsigned lastF; }; // IMPLEMENTATION ==================================================================== From 57628ad818e7f575425370b1960b904567a9bf4f Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 14:18:43 +0100 Subject: [PATCH 12/35] Removed unused variable lastF --- src/timer/stm32/TimerField.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index f585ec1..492fec0 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -32,7 +32,6 @@ class TimerField HardwareTimer accTimer; HardwareTimer pulseTimer; volatile bool stepTimerRunning; - volatile unsigned lastF; }; // IMPLEMENTATION ==================================================================== @@ -41,8 +40,7 @@ TimerField::TimerField(TF_Handler *_handler) : stepTimer(TIM7), accTimer(TIM8), pulseTimer(TIM9), - stepTimerRunning(false), - lastF(0) + stepTimerRunning(false) { handler = _handler; stepTimer.attachInterrupt([] { handler->stepTimerISR(); }); From 3419f279da95a4c851ed594986e475ecac3ade9c Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 14:19:34 +0100 Subject: [PATCH 13/35] Changed end pulse interrupt to be one-shot as it should be --- src/timer/stm32/TimerField.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index 492fec0..2de2449 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -23,7 +23,6 @@ class TimerField inline void accTimerStop() { accTimer.pause(); } inline void setAccUpdatePeriod(unsigned period) { accTimer.setOverflow(period, MICROSEC_FORMAT); } //timerAlarmWrite(accTimer, period, true); } - inline void triggerDelay() { pulseTimer.resume(); } inline void setPulseWidth(unsigned pulseWidth) { pulseTimer.setOverflow(pulseWidth, MICROSEC_FORMAT); } // pause/stop or configure on the go works? protected: @@ -45,7 +44,7 @@ TimerField::TimerField(TF_Handler *_handler) : handler = _handler; stepTimer.attachInterrupt([] { handler->stepTimerISR(); }); accTimer.attachInterrupt([] { handler->accTimerISR(); }); - pulseTimer.attachInterrupt([] { handler->pulseTimerISR(); }); + pulseTimer.attachInterrupt([this] { handler->pulseTimerISR(); this->pulseTimer.pause(); }); // one-shot mode } void TimerField::stepTimerStart() From 8f6c9f55bde7b06301491bc894d8bea4d149fca1 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 14:20:32 +0100 Subject: [PATCH 14/35] Added force refresh of interrupt registers in stepTimerStart, speeds start up by 1ms --- src/timer/stm32/TimerField.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index 2de2449..e3f7262 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -50,6 +50,9 @@ TimerField::TimerField(TF_Handler *_handler) : void TimerField::stepTimerStart() { stepTimer.resume(); + // force reload of all timer's registers here saves 1.2ms in startup time + // https://github.com/stm32duino/wiki/wiki/HardwareTimer-library + stepTimer.refresh(); stepTimerRunning = true; } From c7c24a11a29bcde2bfa9e0781a61aeee86cb8d33 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 14:21:10 +0100 Subject: [PATCH 15/35] Added force update on accelerator interrupt start, could miss one interrupt otherwise --- src/timer/stm32/TimerField.h | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index e3f7262..e864bbc 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -19,11 +19,12 @@ class TimerField inline unsigned getStepFrequency() { return stepTimer.getTimerClkFreq(); } // clock of timer or overflow? inline bool stepTimerIsRunning() const { return stepTimerRunning; } - inline void accTimerStart() { accTimer.resume(); } + inline void accTimerStart() { accTimer.resume(); accTimer.refresh(); } inline void accTimerStop() { accTimer.pause(); } - inline void setAccUpdatePeriod(unsigned period) { accTimer.setOverflow(period, MICROSEC_FORMAT); } //timerAlarmWrite(accTimer, period, true); } + inline void setAccUpdatePeriod(unsigned period); - inline void setPulseWidth(unsigned pulseWidth) { pulseTimer.setOverflow(pulseWidth, MICROSEC_FORMAT); } // pause/stop or configure on the go works? + inline void triggerDelay(); + inline void setPulseWidth(unsigned pulseWidth); protected: static TF_Handler *handler; @@ -62,6 +63,20 @@ void TimerField::stepTimerStop() stepTimerRunning = false; } +void TimerField::setAccUpdatePeriod(unsigned period) +{ + accTimer.setOverflow(period, MICROSEC_FORMAT); +} + +void TimerField::triggerDelay() { + pulseTimer.resume(); +} + +void TimerField::setPulseWidth(unsigned pulseWidth) +{ + pulseTimer.setOverflow(pulseWidth, MICROSEC_FORMAT); +} + void TimerField::setStepFrequency(unsigned f) { if(f == 0){ @@ -69,13 +84,6 @@ void TimerField::setStepFrequency(unsigned f) return; } stepTimer.setOverflow(f, HERTZ_FORMAT); - // stepTimer.refresh(); - /* - m_timer.pause(); - m_timer.setOverflow(micros, MICROSEC_FORMAT); - m_timer.refresh(); - m_timer.resume(); - */ } bool TimerField::begin() From bbb9da77aeca32b3005c54f55f5b4e3576aeee19 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 14:25:16 +0100 Subject: [PATCH 16/35] Added documentation. One bug left: second interrupt is always triggered after 20ms, no mather what acceleration that is used. --- src/timer/stm32/TimerField.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index e864bbc..b694034 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -34,6 +34,11 @@ class TimerField volatile bool stepTimerRunning; }; +// TODO: +// Known bug: Second interrupt always happens after 20ms, no mather what acceleration is set. This causes +// all other interrupts to be slightly delayed. With default acceleration the second interrupt happens +// after 20ms instead of after 11.6ms. +// The implementation is hardcoded to use TIM7,TIM8 and TIM9, should be configurable. // IMPLEMENTATION ==================================================================== TimerField::TimerField(TF_Handler *_handler) : From 650970539aa83a42f0fb7f64e1be75f0feba6e42 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 16:22:54 +0100 Subject: [PATCH 17/35] Added example of known bug in the second triggered interrupt --- src/timer/stm32/TimerField.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index b694034..f66608f 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -35,10 +35,16 @@ class TimerField }; // TODO: -// Known bug: Second interrupt always happens after 20ms, no mather what acceleration is set. This causes +// * Known bug: Second interrupt always happens after 20ms, no mather what acceleration is set. This causes // all other interrupts to be slightly delayed. With default acceleration the second interrupt happens // after 20ms instead of after 11.6ms. -// The implementation is hardcoded to use TIM7,TIM8 and TIM9, should be configurable. +// All acceleration inerrupts are shifted by one step, +// +// Example with 10 steps(ms, offset to each other): +// Teensy35 has steps: 0, 11.6, 9.0, 7.6, 6.6, 7.6, 9.0, 11.6, 20.0, 20.0 +// stm32f4 has steps : 0, 20.0, 11.6, 9.0, 7.6, 6.6, 7.5 9, 11.6 20.0 + +// * The implementation is hardcoded to use TIM7,TIM8 and TIM9, should be configurable. // IMPLEMENTATION ==================================================================== TimerField::TimerField(TF_Handler *_handler) : From 73e8196a58fc1de2caffa6321d9839da9c558d73 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 17:07:28 +0100 Subject: [PATCH 18/35] Added ifdef guards to make it possible to build library for multiple targets --- platformio.ini | 14 ++------------ src/RotateControlBase.h | 2 +- src/Stepper.cpp | 4 ++-- src/Stepper.h | 4 ++-- src/main.cpp | 4 ++-- src/timer/TimerFieldBase.h | 5 ++++- src/timer/esp32/TimerField.cpp | 3 +++ src/timer/esp32/TimerField.h | 4 +++- src/timer/generic/TickTimer.h | 2 +- src/timer/generic/TimerField.h | 3 +-- src/timer/stm32/TimerField.cpp | 4 +++- src/timer/stm32/TimerField.h | 5 ++++- src/timer/teensy3/PIT.cpp | 3 +-- src/timer/teensy3/PIT.h | 2 ++ src/timer/teensy3/TeensyStepFTM.cpp | 3 +-- src/timer/teensy3/TeensyStepFTM.h | 4 +++- src/timer/teensy3/TimerField2.h | 4 +++- src/timer/teensy3/config.h | 5 ++++- src/timer/teensy4/TimerField.h | 5 ++++- src/timer/teensy4/config.h | 4 +++- src/timer/teensy4/ticktimer.h | 7 +++++-- 21 files changed, 54 insertions(+), 37 deletions(-) diff --git a/platformio.ini b/platformio.ini index 4845840..0bb4a6e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -13,19 +13,9 @@ platform = ststm32 board = nucleo_f429zi framework = arduino platform_packages = framework-arduinoststm32@https://github.com/stm32duino/Arduino_Core_STM32 -src_filter = - +<*> - - - - - - [env:teensy35] platform = teensy board = teensy35 -build_flags = -DTEENSY -framework = arduino -src_filter = - +<*> - - - - - - \ No newline at end of file +build_flags = -DTEENSY3 +framework = arduino \ No newline at end of file diff --git a/src/RotateControlBase.h b/src/RotateControlBase.h index 7f2f93f..68318b2 100644 --- a/src/RotateControlBase.h +++ b/src/RotateControlBase.h @@ -3,7 +3,7 @@ #include "MotorControlBase.h" #include -#ifdef TEENSY +#ifdef TEENSY3 #include "core_pins.h" #endif diff --git a/src/Stepper.cpp b/src/Stepper.cpp index 38ec702..a406f8a 100644 --- a/src/Stepper.cpp +++ b/src/Stepper.cpp @@ -26,7 +26,7 @@ Stepper::Stepper(const int _stepPin, const int _dirPin) Stepper &Stepper::setStepPinPolarity(int polarity) { - #ifdef TEENSY + #ifdef TEENSY3 // Calculate adresses of bitbanded pin-set and pin-clear registers uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[stepPin].reg; //GPIO_PDOR uint32_t *pinSetReg = (uint32_t *)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 @@ -51,7 +51,7 @@ Stepper &Stepper::setStepPinPolarity(int polarity) Stepper &Stepper::setInverseRotation(bool reverse) { - #ifdef TEENSY + #ifdef TEENSY3 // Calculate adresses of bitbanded pin-set and pin-clear registers uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[dirPin].reg; //GPIO_PDOR uint32_t *pinSetReg = (uint32_t *)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 diff --git a/src/Stepper.h b/src/Stepper.h index 74f4caf..749cdf7 100644 --- a/src/Stepper.h +++ b/src/Stepper.h @@ -49,7 +49,7 @@ class Stepper static bool cmpVmax(const Stepper *a, const Stepper *b) { return std::abs(a->vMax) > std::abs(b->vMax); } // Pin & Dir registers - #ifdef TEENSY + #if defined(TEENSY3) volatile uint32_t *stepPinActiveReg; volatile uint32_t *stepPinInactiveReg; volatile uint32_t *dirPinCwReg; @@ -72,7 +72,7 @@ class Stepper }; // Inline implementation ----------------------------------------- -#if TEENSY +#if defined(TEENSY3) void Stepper::doStep() { *stepPinActiveReg = 1; diff --git a/src/main.cpp b/src/main.cpp index edec318..98c31d0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,8 +13,8 @@ void setup(){ digitalWrite(LED_BUILTIN, LOW); digitalWrite(STATUS_PIN, LOW); - #ifdef TEENSY - Serial.println("TEENSY"); + #ifdef TEENSY3 + Serial.println("TEENSY3"); #else Serial.println("STM32F4"); #endif diff --git a/src/timer/TimerFieldBase.h b/src/timer/TimerFieldBase.h index a9ce386..9771a32 100644 --- a/src/timer/TimerFieldBase.h +++ b/src/timer/TimerFieldBase.h @@ -1,3 +1,4 @@ +#if defined(TEENSY4) #include "TF_Handler.h" class ITimerField @@ -19,4 +20,6 @@ class ITimerField virtual void setPulseWidth(unsigned delay)=0; virtual void triggerDelay()=0; -}; \ No newline at end of file +}; + +#endif \ No newline at end of file diff --git a/src/timer/esp32/TimerField.cpp b/src/timer/esp32/TimerField.cpp index 6aa8ab2..bb6f7ca 100644 --- a/src/timer/esp32/TimerField.cpp +++ b/src/timer/esp32/TimerField.cpp @@ -1,4 +1,7 @@ +#if defined(ARDUINO_ARCH_ESP32) #include "TimerField.h" TF_Handler* TimerField::handler; portMUX_TYPE TimerField::timerMux = portMUX_INITIALIZER_UNLOCKED; + +#endif \ No newline at end of file diff --git a/src/timer/esp32/TimerField.h b/src/timer/esp32/TimerField.h index 6f58cbb..3b3f195 100644 --- a/src/timer/esp32/TimerField.h +++ b/src/timer/esp32/TimerField.h @@ -1,3 +1,4 @@ +#if defined(ARDUINO_ARCH_ESP32) #pragma once #include "Arduino.h" @@ -90,4 +91,5 @@ void TimerField::end() timerAlarmDisable(stepTimer); timerAlarmDisable(accTimer); timerAlarmDisable(pulseTimer); -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/src/timer/generic/TickTimer.h b/src/timer/generic/TickTimer.h index 80690db..8ed4605 100644 --- a/src/timer/generic/TickTimer.h +++ b/src/timer/generic/TickTimer.h @@ -22,7 +22,7 @@ class TimerBase }; static inline uint32_t get_cycles(){ - #ifdef TEENSY + #if defined(TEENSY3) return ARM_DWT_CYCCNT; #else return dwt_getCycles(); diff --git a/src/timer/generic/TimerField.h b/src/timer/generic/TimerField.h index 5a558e0..cd9bafd 100644 --- a/src/timer/generic/TimerField.h +++ b/src/timer/generic/TimerField.h @@ -1,6 +1,5 @@ #pragma once -#ifndef ARDUINO_ARCH_ESP32 #include "wiring.h" #endif // //#include "imxrt.h" @@ -65,5 +64,5 @@ bool TickTimerField::begin() void TickTimerField::end() { - //TimerControl::end(); + //TimerControl::end(); } \ No newline at end of file diff --git a/src/timer/stm32/TimerField.cpp b/src/timer/stm32/TimerField.cpp index 9574a78..a2e43f0 100644 --- a/src/timer/stm32/TimerField.cpp +++ b/src/timer/stm32/TimerField.cpp @@ -1,3 +1,5 @@ +#if defined(STM32F4xx) #include "TimerField.h" - TF_Handler* TimerField::handler; + +#endif \ No newline at end of file diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index f66608f..a3dd0b9 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -1,3 +1,4 @@ +#if defined(STM32F4xx) #pragma once #include @@ -108,4 +109,6 @@ void TimerField::end() stepTimer.pause(); accTimer.pause(); pulseTimer.pause(); -} \ No newline at end of file +} + +#endif \ No newline at end of file diff --git a/src/timer/teensy3/PIT.cpp b/src/timer/teensy3/PIT.cpp index 3eb8375..9fe56f1 100644 --- a/src/timer/teensy3/PIT.cpp +++ b/src/timer/teensy3/PIT.cpp @@ -1,5 +1,4 @@ -#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) - +#if defined(TEENSY3) #include "PIT.h" #include "../TF_Handler.h" diff --git a/src/timer/teensy3/PIT.h b/src/timer/teensy3/PIT.h index 92d315a..a40b102 100644 --- a/src/timer/teensy3/PIT.h +++ b/src/timer/teensy3/PIT.h @@ -1,3 +1,4 @@ +#if defined(TEENSY3) #pragma once #include "IntervalTimer.h" @@ -34,3 +35,4 @@ class PIT void setupChannel(); }; +#endif \ No newline at end of file diff --git a/src/timer/teensy3/TeensyStepFTM.cpp b/src/timer/teensy3/TeensyStepFTM.cpp index 2d44381..15f20be 100644 --- a/src/timer/teensy3/TeensyStepFTM.cpp +++ b/src/timer/teensy3/TeensyStepFTM.cpp @@ -1,5 +1,4 @@ -#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) - +#if defined(TEENSY3) #include "TeensyStepFTM.h" #include "config.h" diff --git a/src/timer/teensy3/TeensyStepFTM.h b/src/timer/teensy3/TeensyStepFTM.h index 429fa62..8ca0eea 100644 --- a/src/timer/teensy3/TeensyStepFTM.h +++ b/src/timer/teensy3/TeensyStepFTM.h @@ -1,3 +1,4 @@ +#if defined(TEENSY3) #pragma once #include "config.h" @@ -32,4 +33,5 @@ namespace TeensyStepFTM } // isFTM is a compile time constant -> compiler completely optimizes the not valid case away timer->CH[channel].SC = FTM_CSC_MSA | FTM_CSC_CHIE; } -} \ No newline at end of file +}} +#endif \ No newline at end of file diff --git a/src/timer/teensy3/TimerField2.h b/src/timer/teensy3/TimerField2.h index c1b6714..6f39254 100644 --- a/src/timer/teensy3/TimerField2.h +++ b/src/timer/teensy3/TimerField2.h @@ -1,3 +1,4 @@ +#if defined(TEENSY3) #pragma once #include "wiring.h" @@ -181,4 +182,5 @@ void TimerField::delayISR(unsigned channel) handler->accTimerISR(); } -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/src/timer/teensy3/config.h b/src/timer/teensy3/config.h index 06759f3..fd5a3cb 100644 --- a/src/timer/teensy3/config.h +++ b/src/timer/teensy3/config.h @@ -1,4 +1,5 @@ -#pragma once +#if defined(TEENSY3) +#pragma once #include #include @@ -170,3 +171,5 @@ namespace TeensyStepFTM return mu * 1E-6 * _timer_frequency / (1 << prescale) + 0.5; } } + +#endif \ No newline at end of file diff --git a/src/timer/teensy4/TimerField.h b/src/timer/teensy4/TimerField.h index 15a2199..10b06f7 100644 --- a/src/timer/teensy4/TimerField.h +++ b/src/timer/teensy4/TimerField.h @@ -1,3 +1,4 @@ +#if defined(TEENSY4) #pragma once #include "wiring.h" @@ -155,4 +156,6 @@ void TimerField::delayISR(unsigned channel) // handler->accTimerISR(); // } -} \ No newline at end of file +} + +#endif \ No newline at end of file diff --git a/src/timer/teensy4/config.h b/src/timer/teensy4/config.h index 338f426..2bb8442 100644 --- a/src/timer/teensy4/config.h +++ b/src/timer/teensy4/config.h @@ -1,4 +1,5 @@ -#pragma once +#if defined(TEENSY4) +#pragma once #include #include @@ -170,3 +171,4 @@ namespace TeensyStepFTM return mu * 1E-6 * _timer_frequency / (1 << prescale) + 0.5; } } +#endif \ No newline at end of file diff --git a/src/timer/teensy4/ticktimer.h b/src/timer/teensy4/ticktimer.h index 9dadc58..7877ab1 100644 --- a/src/timer/teensy4/ticktimer.h +++ b/src/timer/teensy4/ticktimer.h @@ -1,5 +1,6 @@ -#include "Arduino.h" +#if defined(TEENSY4) #include +#include "Arduino.h" //#include using callback_t = void (*)(); @@ -95,4 +96,6 @@ void tickTimer::setFrequency(unsigned f) { } -channelInfo *tickTimer::channels = new channelInfo[tickTimer::maxTimers + 1]; \ No newline at end of file +channelInfo *tickTimer::channels = new channelInfo[tickTimer::maxTimers + 1]; + +#endif \ No newline at end of file From 985e63f0c5ad347e226b1df42e11c3518f1ad885 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 27 Mar 2020 17:19:32 +0100 Subject: [PATCH 19/35] Updated README, library.json --- README.md | 4 ++-- library.json | 14 ++++++++++---- library.properties | 2 +- src/version.h | 4 ++-- 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 9f37100..9bc69b8 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # ESP32Step - Fast Stepper Library for ESP32 boards -Fork of [TeensyStep V2 library](https://luni64.github.io/TeensyStep/) adopted for ESP32 boards. It uses the timers 0, 1, 2 via the esp32-arduino interface. +Fork of [TeensyStep V2 library](https://luni64.github.io/TeensyStep/) adopted for STM32F4, ESP32 boards. It uses the timers 0, 1, 2 via the esp32-arduino interface and timer 7,8,9 for STM32F4. ## Purpose of the Library **ESP32Step** is an efficient Arduino library compatible with ESP32 and Teensy 3.0, 3.1, 3.2, 3.5 and 3.6. The library is able to handle synchronous and independent movement and continuous rotation of steppers with pulse rates of up to 300'000 steps per second. The following table shows a summary of the **ESP32Step** specifications (taken over from TeensyStep, not yet verified for ESP32): @@ -17,7 +17,7 @@ Fork of [TeensyStep V2 library](https://luni64.github.io/TeensyStep/) adopted fo | Settable direction signal polarity | cw / ccw | cw | ## Architecture -The base idea is to use one reloading interval timer for setting the step pin at a stepper velocity based frequency, one non-reloading (one shot) timer for clearing the step pin and thus defining the pulse width, and one (slower) reloading interval timer for updating the step timer frequency for acceleration / deceleration. +The base idea is to use one reloading interval timer for setting the step pin at a stepper velocity based frequency, one non-reloading (one shot) timer for clearing the step pin and thus defining the pulse width, and one (slower) reloading interval timer for updating the step timer frequency for acceleration / deceleration. ## TODO Currently, the ESP32 timers 0, 1, 2 are hard-coded. In consequence only one motor controller can run at the same time. I'll wait for an update of TeensyStep for a more general timer interface (e.g. [TeensyTimerTool](https://github.com/luni64/TeensyTimerTool)). diff --git a/library.json b/library.json index e708a55..f7b260a 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "TeensyStep", - "keywords": "teensy, esp32, stepper, motor, high-speed", - "description": "High speed stepper driver for teensy boards (T3.0 - T3.6) and ESP32", + "keywords": "teensy, stm32f4, esp32, stepper, motor, high-speed", + "description": "High speed stepper driver for teensy boards (T3.0 - T3.6), STM32F4 and ESP32", "repository": { "type": "git", @@ -15,7 +15,13 @@ "maintainer": true }, "homepage": "https://luni64.github.io/TeensyStep", - "version": "2.0.1", + "version": "2.1.1", "frameworks": "arduino", - "platforms": "Teensy" + "platforms": ["Teensy", "stm32", "esp32"], + "build": { + "srcFilter": [ + "+<*>", + "-" + ] +} } diff --git a/library.properties b/library.properties index a869d10..97c4f17 100644 --- a/library.properties +++ b/library.properties @@ -2,7 +2,7 @@ name=TeensyStep version=2.0.1 author=Lutz Niggl maintainer=Lutz Niggl -sentence=High speed stepper driver for PJRC Teensy boards (T3.0 - T3.6) and ESP32 +sentence=High speed stepper driver for PJRC Teensy boards (T3.0 - T3.6) STM32F4, and ESP32 paragraph= Step rates up to 300000stp/sec. Accelerated and synchronized movement of up to 10 steppers. Due to the low processor load it can easily be used together with sensors, displays, serial communication ... category=Device Control url=https://luni64.github.io/TeensyStep/ diff --git a/src/version.h b/src/version.h index cb250b6..ee0b4df 100644 --- a/src/version.h +++ b/src/version.h @@ -1,7 +1,7 @@ namespace TeensyStep { -constexpr const char *version = "V2.0.1"; -constexpr const char *branch = "esp32"; +constexpr const char *version = "V2.1.1"; +constexpr const char *branch = "feature-stm32f4"; } // namespace TeensyStep \ No newline at end of file From f904e4fc7aad77a57b7c0a25f1df97ea766aa92d Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Wed, 8 Apr 2020 17:21:26 +0200 Subject: [PATCH 20/35] Added functionality for stm32 to pick timers by itself, warning do NOT use more than 4 instances of StepControl --- platformio.ini | 2 +- src/timer/stm32/TimerField.cpp | 5 +++-- src/timer/stm32/TimerField.h | 17 ++++++++++++----- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/platformio.ini b/platformio.ini index 0bb4a6e..c471e88 100644 --- a/platformio.ini +++ b/platformio.ini @@ -18,4 +18,4 @@ platform_packages = framework-arduinoststm32@https://github.com/stm32duino/Ardui platform = teensy board = teensy35 build_flags = -DTEENSY3 -framework = arduino \ No newline at end of file +framework = arduino diff --git a/src/timer/stm32/TimerField.cpp b/src/timer/stm32/TimerField.cpp index a2e43f0..6feb2c3 100644 --- a/src/timer/stm32/TimerField.cpp +++ b/src/timer/stm32/TimerField.cpp @@ -1,5 +1,6 @@ #if defined(STM32F4xx) #include "TimerField.h" TF_Handler* TimerField::handler; - -#endif \ No newline at end of file +int TimerField::instances = 0; +TIM_TypeDef* TimerField::timer_mapping[MAX_TIMERS] = { TIM1, TIM2, TIM3, TIM4, TIM5, TIM6, TIM7, TIM8, TIM9, TIM10, TIM11, TIM12 }; +#endif diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index a3dd0b9..3041623 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -6,6 +6,8 @@ #include "../TF_Handler.h" + +#define MAX_TIMERS 12 class TimerField { public: @@ -28,13 +30,18 @@ class TimerField inline void setPulseWidth(unsigned pulseWidth); protected: + static int instances; + static TIM_TypeDef* timer_mapping[MAX_TIMERS]; static TF_Handler *handler; HardwareTimer stepTimer; HardwareTimer accTimer; HardwareTimer pulseTimer; volatile bool stepTimerRunning; -}; + TIM_TypeDef* get_timer() { + return instances >= MAX_TIMERS ? TIM1 : timer_mapping[instances++]; + } +}; // TODO: // * Known bug: Second interrupt always happens after 20ms, no mather what acceleration is set. This causes // all other interrupts to be slightly delayed. With default acceleration the second interrupt happens @@ -49,9 +56,9 @@ class TimerField // IMPLEMENTATION ==================================================================== TimerField::TimerField(TF_Handler *_handler) : - stepTimer(TIM7), - accTimer(TIM8), - pulseTimer(TIM9), + stepTimer(get_timer()), + accTimer(get_timer()), + pulseTimer(get_timer()), stepTimerRunning(false) { handler = _handler; @@ -111,4 +118,4 @@ void TimerField::end() pulseTimer.pause(); } -#endif \ No newline at end of file +#endif From c9b9b74765c528eca21e07c060fa848000a9db3d Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Wed, 8 Apr 2020 18:29:47 +0200 Subject: [PATCH 21/35] Adjusted what timers to use --- src/timer/stm32/TimerField.cpp | 4 ++-- src/timer/stm32/TimerField.h | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/timer/stm32/TimerField.cpp b/src/timer/stm32/TimerField.cpp index 6feb2c3..e644b00 100644 --- a/src/timer/stm32/TimerField.cpp +++ b/src/timer/stm32/TimerField.cpp @@ -1,6 +1,6 @@ #if defined(STM32F4xx) #include "TimerField.h" -TF_Handler* TimerField::handler; int TimerField::instances = 0; -TIM_TypeDef* TimerField::timer_mapping[MAX_TIMERS] = { TIM1, TIM2, TIM3, TIM4, TIM5, TIM6, TIM7, TIM8, TIM9, TIM10, TIM11, TIM12 }; +TIM_TypeDef* TimerField::timer_mapping[MAX_TIMERS] = { TIM1, TIM2, TIM4, TIM5, TIM6, TIM7, TIM8, TIM9, TIM10, TIM11, TIM12, TIM14 }; +// TIM3 and TIM13 used by HAL/FreeRTOS? doesn't work well to use. #endif diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index 3041623..04fb8bb 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -32,7 +32,7 @@ class TimerField protected: static int instances; static TIM_TypeDef* timer_mapping[MAX_TIMERS]; - static TF_Handler *handler; + TF_Handler *handler; HardwareTimer stepTimer; HardwareTimer accTimer; HardwareTimer pulseTimer; @@ -52,7 +52,8 @@ class TimerField // Teensy35 has steps: 0, 11.6, 9.0, 7.6, 6.6, 7.6, 9.0, 11.6, 20.0, 20.0 // stm32f4 has steps : 0, 20.0, 11.6, 9.0, 7.6, 6.6, 7.5 9, 11.6 20.0 -// * The implementation is hardcoded to use TIM7,TIM8 and TIM9, should be configurable. +// * Maximum 4 instances can be used, if more than four are initialized the first will break +// as its interrupt is reused. // IMPLEMENTATION ==================================================================== TimerField::TimerField(TF_Handler *_handler) : @@ -62,8 +63,8 @@ TimerField::TimerField(TF_Handler *_handler) : stepTimerRunning(false) { handler = _handler; - stepTimer.attachInterrupt([] { handler->stepTimerISR(); }); - accTimer.attachInterrupt([] { handler->accTimerISR(); }); + stepTimer.attachInterrupt([this] { handler->stepTimerISR(); }); + accTimer.attachInterrupt([this] { handler->accTimerISR(); }); pulseTimer.attachInterrupt([this] { handler->pulseTimerISR(); this->pulseTimer.pause(); }); // one-shot mode } From 23b51cbc3600adf84c15736e244caa19d62c7ce3 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Wed, 8 Apr 2020 18:32:16 +0200 Subject: [PATCH 22/35] reapplied patch to override setTargetAbs/setTargetRel --- src/Stepper.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Stepper.h b/src/Stepper.h index 749cdf7..e96f506 100644 --- a/src/Stepper.h +++ b/src/Stepper.h @@ -20,8 +20,8 @@ class Stepper Stepper &setStepPinPolarity(int p); // HIGH -> positive pulses, LOW -> negative pulses Stepper &setInverseRotation(bool b); // Change polarity of the dir pulse - void setTargetAbs(int32_t pos); // Set target position absolute - void setTargetRel(int32_t delta); // Set target position relative to current position + virtual void setTargetAbs(int32_t pos); // Set target position absolute + virtual void setTargetRel(int32_t delta); // Set target position relative to current position inline int32_t getPosition() const { return current; } inline void setPosition(int32_t pos) { current = pos; } @@ -111,4 +111,4 @@ void Stepper::setDir(int d) void Stepper::toggleDir() { setDir(-dir); -} \ No newline at end of file +} From 5d30fd0d2c3a0950d5e583494c20957ab96c3e55 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Wed, 8 Apr 2020 18:33:07 +0200 Subject: [PATCH 23/35] Added ifdef to prevent main from being built when used as library --- src/main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index 98c31d0..e98da40 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,3 +1,5 @@ + +#ifdef TEST #include #include "TeensyStep.h" @@ -31,4 +33,6 @@ void loop(){ digitalWrite(LED_BUILTIN, LOW); digitalWrite(STATUS_PIN, LOW); -} \ No newline at end of file +} + +#endif From 2f0420f0c5621b8ebef318822f987a2a566c2615 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Fri, 24 Jul 2020 10:20:16 +0200 Subject: [PATCH 24/35] Changed stepper public function to be virtual --- src/Stepper.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/Stepper.h b/src/Stepper.h index e96f506..edef062 100644 --- a/src/Stepper.h +++ b/src/Stepper.h @@ -14,17 +14,17 @@ class Stepper Stepper(const int StepPin, const int DirPin); - Stepper &setMaxSpeed(int32_t speed); // steps/s - Stepper &setAcceleration(uint32_t _a); // steps/s^2 + virtual Stepper &setMaxSpeed(int32_t speed); // steps/s + virtual Stepper &setAcceleration(uint32_t _a); // steps/s^2 - Stepper &setStepPinPolarity(int p); // HIGH -> positive pulses, LOW -> negative pulses - Stepper &setInverseRotation(bool b); // Change polarity of the dir pulse + virtual Stepper &setStepPinPolarity(int p); // HIGH -> positive pulses, LOW -> negative pulses + virtual Stepper &setInverseRotation(bool b); // Change polarity of the dir pulse virtual void setTargetAbs(int32_t pos); // Set target position absolute virtual void setTargetRel(int32_t delta); // Set target position relative to current position - inline int32_t getPosition() const { return current; } - inline void setPosition(int32_t pos) { current = pos; } + virtual int32_t getPosition() const { return current; } + virtual void setPosition(int32_t pos) { current = pos; } int32_t dir; protected: From 3d8f0406a268d77511e3bfc19f3dabba77e49314 Mon Sep 17 00:00:00 2001 From: Erik Tideman Date: Wed, 9 Sep 2020 17:25:23 +0200 Subject: [PATCH 25/35] Added scope to setTargetRel to not mix up with potential overload --- src/Stepper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Stepper.cpp b/src/Stepper.cpp index a406f8a..251a30e 100644 --- a/src/Stepper.cpp +++ b/src/Stepper.cpp @@ -88,7 +88,7 @@ Stepper &Stepper::setMaxSpeed(int32_t speed) void Stepper::setTargetAbs(int32_t target) { - setTargetRel(target - current); + Stepper::setTargetRel(target - current); } void Stepper::setTargetRel(int32_t delta) @@ -96,4 +96,4 @@ void Stepper::setTargetRel(int32_t delta) setDir(delta < 0 ? -1 : 1); target = current + delta; A = std::abs(delta); -} \ No newline at end of file +} From 7469a838a2d5094b3df466c233569687e2e358cb Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Mon, 21 Sep 2020 17:05:37 +0200 Subject: [PATCH 26/35] Changed example code to run 2 steppers and visit points --- src/main.cpp | 48 +++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 39 insertions(+), 9 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index e98da40..0ae05d5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,19 +1,44 @@ #ifdef TEST #include +#include #include "TeensyStep.h" -#define STATUS_PIN PIN_A0 +using std::vector; -Stepper motor(PIN_A1, PIN_A2); // STEP pin: 2, DIR pin: 3 +#define ENABLE_PIN D8 + +Stepper stepper_x(D2, D5); // STEP pin: 2, DIR pin: 3 +Stepper stepper_y(D3, D6); StepControl controller; // Use default settings + +class CPoint{ + public: + CPoint(int x, int y): x(x), y(y) {} + int x = 0; + int y = 0; +}; + +vector points; + void setup(){ Serial.begin(115200); pinMode(LED_BUILTIN, OUTPUT); - pinMode(STATUS_PIN, OUTPUT); + pinMode(ENABLE_PIN, OUTPUT); digitalWrite(LED_BUILTIN, LOW); - digitalWrite(STATUS_PIN, LOW); + digitalWrite(ENABLE_PIN, LOW); + + points.push_back(new CPoint(100, 100)); + points.push_back(new CPoint(200, 200)); + points.push_back(new CPoint(300, 300)); + points.push_back(new CPoint(400, 400)); + points.push_back(new CPoint(500, 500)); + points.push_back(new CPoint(600, 600)); + points.push_back(new CPoint(700, 700)); + points.push_back(new CPoint(800, 800)); + points.push_back(new CPoint(900, 900)); + points.push_back(new CPoint(1000, 1000)); #ifdef TEENSY3 Serial.println("TEENSY3"); @@ -25,14 +50,19 @@ void setup(){ void loop(){ delay(500); digitalWrite(LED_BUILTIN, HIGH); - digitalWrite(STATUS_PIN, HIGH); + Serial.println("Starting new sequence"); - motor.setTargetRel(10); // Set target position to 1000 steps from current position - controller.move(motor); // Do the move - while(controller.isRunning()); + for(auto& point : points){ + Serial.printf("Moving to x: %d, y: %d\r\n", point->x, point->y); + stepper_x.setTargetAbs(point->x); + stepper_y.setTargetAbs(point->y); + controller.moveAsync(stepper_x, stepper_y); + while(controller.isRunning()); + } + //controller.step_calls = controller.pulse_calls = controller.acc_calls = 0; + //Serial.printf("steps: %u, pulses: %u, acc: %u\r\n", controller.step_calls, controller.pulse_calls, controller.acc_calls); digitalWrite(LED_BUILTIN, LOW); - digitalWrite(STATUS_PIN, LOW); } #endif From a3e085161e44189e3c5c7bfedf71c99c9d3069ab Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Mon, 21 Sep 2020 17:07:51 +0200 Subject: [PATCH 27/35] Removed stm32 custom arduino framework as dependencies have been merged in mainline --- platformio.ini | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/platformio.ini b/platformio.ini index c471e88..1900b3c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,10 +12,10 @@ platform = ststm32 board = nucleo_f429zi framework = arduino -platform_packages = framework-arduinoststm32@https://github.com/stm32duino/Arduino_Core_STM32 +build_flags = -DTEST [env:teensy35] platform = teensy board = teensy35 -build_flags = -DTEENSY3 +build_flags = -DTEENSY3 -DTEST framework = arduino From 8aaa5d2f3a3b89ae5df48cbfc28f8610e9b41864 Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Tue, 22 Sep 2020 07:23:35 +0200 Subject: [PATCH 28/35] Minor comment cleanup --- src/timer/stm32/TimerField.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index 523deae..6218775 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -47,11 +47,11 @@ class TimerField // * Known bug: Second interrupt always happens after 20ms, no mather what acceleration is set. This causes // all other interrupts to be slightly delayed. With default acceleration the second interrupt happens // after 20ms instead of after 11.6ms. -// All acceleration inerrupts are shifted by one step, +// All acceleration interrupts are shifted by one step, // // Example with 10 steps(ms, offset to each other): -// Teensy35 has steps: 0, 11.6, 9.0, 7.6, 6.6, 7.6, 9.0, 11.6, 20.0, 20.0 -// stm32f4 has steps : 0, 20.0, 11.6, 9.0, 7.6, 6.6, 7.5 9, 11.6 20.0 +// Teensy35 has steps: 0, 11.6, 9.0, 7.6, 6.6, 7.6, 9.0, 11.6, 20.0, 20.0 +// stm32f4 has steps : 0, 20.0, 11.6, 9.0, 7.6, 6.6, 7.5, 9.0, 11.6, 20.0 // * Maximum 4 instances can be used, if more than four are initialized the first will break // as its interrupt is reused. From e1e105a98396a4fe08fca1473550b956e72d10bd Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Tue, 22 Sep 2020 07:55:44 +0200 Subject: [PATCH 29/35] Added guards for teensy3's PIT.cpp to prevent it from being compiled for stm32 --- platformio.ini | 6 +----- src/timer/teensy3/PIT.cpp | 4 +++- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/platformio.ini b/platformio.ini index 75666f0..1c4796c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,12 +11,8 @@ [env:nucleo_f429zi] platform = ststm32 board = nucleo_f429zi -framework = arduino build_flags = -DTEST -src_filter = - +<*> - - - - +framework = arduino [env:teensy35] platform = teensy diff --git a/src/timer/teensy3/PIT.cpp b/src/timer/teensy3/PIT.cpp index f64db7d..20edc53 100644 --- a/src/timer/teensy3/PIT.cpp +++ b/src/timer/teensy3/PIT.cpp @@ -1,3 +1,4 @@ +#if defined(TEENSY3) #include "PIT.h" #include "../TF_Handler.h" @@ -61,4 +62,5 @@ namespace TeensyStep return -1; } } -} \ No newline at end of file +} +#endif From 719cd6f6e904ea3c83f0d210bc2f05446516e04d Mon Sep 17 00:00:00 2001 From: Erik Tideman Date: Tue, 22 Sep 2020 08:00:22 +0200 Subject: [PATCH 30/35] removed esp32 code with unknown status --- src/timer/esp32/TimerField.cpp | 7 --- src/timer/esp32/TimerField.h | 95 ---------------------------------- 2 files changed, 102 deletions(-) delete mode 100644 src/timer/esp32/TimerField.cpp delete mode 100644 src/timer/esp32/TimerField.h diff --git a/src/timer/esp32/TimerField.cpp b/src/timer/esp32/TimerField.cpp deleted file mode 100644 index bb6f7ca..0000000 --- a/src/timer/esp32/TimerField.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#if defined(ARDUINO_ARCH_ESP32) -#include "TimerField.h" - -TF_Handler* TimerField::handler; -portMUX_TYPE TimerField::timerMux = portMUX_INITIALIZER_UNLOCKED; - -#endif \ No newline at end of file diff --git a/src/timer/esp32/TimerField.h b/src/timer/esp32/TimerField.h deleted file mode 100644 index 3b3f195..0000000 --- a/src/timer/esp32/TimerField.h +++ /dev/null @@ -1,95 +0,0 @@ -#if defined(ARDUINO_ARCH_ESP32) -#pragma once - -#include "Arduino.h" -#include - -#include "../TF_Handler.h" - -// //========================= -// // ESP 32 -// //========================= - -class TimerField -{ -public: - inline TimerField(TF_Handler *_handler); - - inline bool begin(); - inline void end(); - - inline void stepTimerStart(); - inline void stepTimerStop(); - inline void setStepFrequency(unsigned f); - inline unsigned getStepFrequency() { return timerAlarmRead(stepTimer); } - inline bool stepTimerIsRunning() const { return stepTimerRunning; } - - inline void accTimerStart() { timerAlarmEnable(accTimer); } - inline void accTimerStop() { timerAlarmDisable(accTimer); } - inline void setAccUpdatePeriod(unsigned period) { timerAlarmWrite(accTimer, period, true); } - - inline void triggerDelay() { timerAlarmEnable(pulseTimer); } - inline void setPulseWidth(unsigned pulseWidth) { timerAlarmWrite(pulseTimer, pulseWidth, false); } - - static portMUX_TYPE timerMux; - -protected: - static TF_Handler *handler; - hw_timer_t *stepTimer; - hw_timer_t *accTimer; - hw_timer_t *pulseTimer; - bool stepTimerRunning; - unsigned lastF; -}; - -// IMPLEMENTATION ==================================================================== - -TimerField::TimerField(TF_Handler *_handler) - : stepTimer(timerBegin(0, 80, true)), - accTimer(timerBegin(1, 80, true)), - pulseTimer(timerBegin(2, 80, true)), - stepTimerRunning(false), - lastF(0) -{ - handler = _handler; - timerAttachInterrupt(stepTimer, [] { handler->stepTimerISR(); }, true); - timerAlarmWrite(stepTimer, 1, true); - timerAttachInterrupt(accTimer, [] { handler->accTimerISR(); }, true); - timerAlarmWrite(accTimer, 1, true); - timerAttachInterrupt(pulseTimer, [] { handler->pulseTimerISR(); }, true); - timerAlarmWrite(pulseTimer, 1, false); -} - -void TimerField::stepTimerStart() -{ - timerAlarmEnable(stepTimer); - stepTimerRunning = true; -} - -void TimerField::stepTimerStop() -{ - timerAlarmDisable(stepTimer); - stepTimerRunning = false; -} - -void TimerField::setStepFrequency(unsigned f) -{ - if (f != 0) - timerAlarmWrite(stepTimer, 1000000 / f, true); // Timer runs at 1 microsecond interval - else - stepTimerStop(); -} - -bool TimerField::begin() -{ - Serial.println("begin"); - return true; -} - -void TimerField::end() -{ - timerAlarmDisable(stepTimer); - timerAlarmDisable(accTimer); - timerAlarmDisable(pulseTimer); -} -#endif \ No newline at end of file From be1cb806c2fbf8ee1d1fee2494c1ade42f7838ed Mon Sep 17 00:00:00 2001 From: Erik Tideman Date: Tue, 22 Sep 2020 08:35:01 +0200 Subject: [PATCH 31/35] Cleaned up repo --- library.json | 8 +----- platformio.ini | 21 ---------------- src/main.cpp | 68 -------------------------------------------------- 3 files changed, 1 insertion(+), 96 deletions(-) delete mode 100644 platformio.ini delete mode 100644 src/main.cpp diff --git a/library.json b/library.json index 65bdddb..81d9056 100644 --- a/library.json +++ b/library.json @@ -17,11 +17,5 @@ "homepage": "https://luni64.github.io/TeensyStep", "version": "2.1", "frameworks": "arduino", - "platforms": ["Teensy", "stm32"], - "build": { - "srcFilter": [ - "+<*>", - "-" - ] -} + "platforms": ["Teensy", "stm32"] } diff --git a/platformio.ini b/platformio.ini deleted file mode 100644 index 1c4796c..0000000 --- a/platformio.ini +++ /dev/null @@ -1,21 +0,0 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[env:nucleo_f429zi] -platform = ststm32 -board = nucleo_f429zi -build_flags = -DTEST -framework = arduino - -[env:teensy35] -platform = teensy -board = teensy35 -build_flags = -DTEENSY3 -DTEST -framework = arduino diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 59c8a65..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,68 +0,0 @@ - -#ifdef TEST -#include -#include -#include "TeensyStep.h" - -using std::vector; - -#define ENABLE_PIN 8 - -Stepper stepper_x(2, 5); // STEP pin: 2, DIR pin: 3 -Stepper stepper_y(3, 6); -StepControl controller; // Use default settings - - -class CPoint{ - public: - CPoint(int x, int y): x(x), y(y) {} - int x = 0; - int y = 0; -}; - -vector points; - -void setup(){ - Serial.begin(115200); - pinMode(LED_BUILTIN, OUTPUT); - pinMode(ENABLE_PIN, OUTPUT); - digitalWrite(LED_BUILTIN, LOW); - digitalWrite(ENABLE_PIN, LOW); - - points.push_back(new CPoint(100, 100)); - points.push_back(new CPoint(200, 200)); - points.push_back(new CPoint(300, 300)); - points.push_back(new CPoint(400, 400)); - points.push_back(new CPoint(500, 500)); - points.push_back(new CPoint(600, 600)); - points.push_back(new CPoint(700, 700)); - points.push_back(new CPoint(800, 800)); - points.push_back(new CPoint(900, 900)); - points.push_back(new CPoint(1000, 1000)); - - #ifdef TEENSY3 - Serial.println("TEENSY3"); - #else - Serial.println("STM32F4"); - #endif -} - -void loop(){ - delay(500); - digitalWrite(LED_BUILTIN, HIGH); - Serial.println("Starting new sequence"); - - for(auto& point : points){ - Serial.printf("Moving to x: %d, y: %d\r\n", point->x, point->y); - stepper_x.setTargetAbs(point->x); - stepper_y.setTargetAbs(point->y); - controller.moveAsync(stepper_x, stepper_y); - while(controller.isRunning()); - } - - //controller.step_calls = controller.pulse_calls = controller.acc_calls = 0; - //Serial.printf("steps: %u, pulses: %u, acc: %u\r\n", controller.step_calls, controller.pulse_calls, controller.acc_calls); - digitalWrite(LED_BUILTIN, LOW); -} - -#endif From d956a5217908c8a35c8038a3acc4c1feaabdbb6f Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Tue, 22 Sep 2020 08:51:59 +0200 Subject: [PATCH 32/35] Changed to use platform defines that works for all teensy3 versions --- src/Stepper.cpp | 4 ++-- src/Stepper.h | 4 ++-- src/timer/generic/TickTimer.h | 8 ++++---- src/timer/teensy3/PIT.cpp | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/Stepper.cpp b/src/Stepper.cpp index 107ab6b..6898fe8 100644 --- a/src/Stepper.cpp +++ b/src/Stepper.cpp @@ -18,7 +18,7 @@ namespace TeensyStep Stepper& Stepper::setStepPinPolarity(int polarity) { -#ifdef TEENSY3 +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) // Calculate adresses of bitbanded pin-set and pin-clear registers uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[stepPin].reg; //GPIO_PDOR uint32_t* pinSetReg = (uint32_t*)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 @@ -45,7 +45,7 @@ namespace TeensyStep Stepper& Stepper::setInverseRotation(bool reverse) { -#ifdef TEENSY3 +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) // Calculate adresses of bitbanded pin-set and pin-clear registers uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[dirPin].reg; //GPIO_PDOR uint32_t* pinSetReg = (uint32_t*)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 diff --git a/src/Stepper.h b/src/Stepper.h index 7a2d92b..bd200f5 100644 --- a/src/Stepper.h +++ b/src/Stepper.h @@ -55,7 +55,7 @@ namespace TeensyStep static bool cmpVmax(const Stepper* a, const Stepper* b) { return std::abs(a->vMax) > std::abs(b->vMax); } // Pin & Dir registers -#if defined(TEENSY3) +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) volatile uint32_t* stepPinActiveReg; volatile uint32_t* stepPinInactiveReg; volatile uint32_t* dirPinCwReg; @@ -78,7 +78,7 @@ namespace TeensyStep }; // Inline implementation ----------------------------------------- -#if defined(TEENSY3) +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) void Stepper::doStep() { *stepPinActiveReg = 1; diff --git a/src/timer/generic/TickTimer.h b/src/timer/generic/TickTimer.h index c739a23..8c57c5b 100644 --- a/src/timer/generic/TickTimer.h +++ b/src/timer/generic/TickTimer.h @@ -22,11 +22,11 @@ class TimerBase }; static inline uint32_t get_cycles(){ - #if defined(TEENSY3) +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) return ARM_DWT_CYCCNT; - #else +#else return dwt_getCycles(); - #endif +#endif } inline void start() { startCnt = get_cycles(); run = true; } @@ -86,7 +86,7 @@ class TimerControl public: static void begin() { -#ifdef TEENSY3 +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) ARM_DEMCR |= ARM_DEMCR_TRCENA; ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA; #endif diff --git a/src/timer/teensy3/PIT.cpp b/src/timer/teensy3/PIT.cpp index 20edc53..75ef9f0 100644 --- a/src/timer/teensy3/PIT.cpp +++ b/src/timer/teensy3/PIT.cpp @@ -1,4 +1,4 @@ -#if defined(TEENSY3) +#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) #include "PIT.h" #include "../TF_Handler.h" From f06b82e75e238a148fcfe08c0d966640f4d898fa Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Tue, 22 Sep 2020 09:07:40 +0200 Subject: [PATCH 33/35] Fixed defines for platform teensy4 --- platformio.ini | 21 +++++++++++ src/main.cpp | 68 ++++++++++++++++++++++++++++++++++ src/timer/TimerFieldBase.h | 5 +-- src/timer/teensy4/TimerField.h | 4 +- src/timer/teensy4/config.h | 4 +- src/timer/teensy4/ticktimer.h | 4 +- 6 files changed, 96 insertions(+), 10 deletions(-) create mode 100644 platformio.ini create mode 100644 src/main.cpp diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..1c4796c --- /dev/null +++ b/platformio.ini @@ -0,0 +1,21 @@ +; 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 + +[env:nucleo_f429zi] +platform = ststm32 +board = nucleo_f429zi +build_flags = -DTEST +framework = arduino + +[env:teensy35] +platform = teensy +board = teensy35 +build_flags = -DTEENSY3 -DTEST +framework = arduino diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..59c8a65 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,68 @@ + +#ifdef TEST +#include +#include +#include "TeensyStep.h" + +using std::vector; + +#define ENABLE_PIN 8 + +Stepper stepper_x(2, 5); // STEP pin: 2, DIR pin: 3 +Stepper stepper_y(3, 6); +StepControl controller; // Use default settings + + +class CPoint{ + public: + CPoint(int x, int y): x(x), y(y) {} + int x = 0; + int y = 0; +}; + +vector points; + +void setup(){ + Serial.begin(115200); + pinMode(LED_BUILTIN, OUTPUT); + pinMode(ENABLE_PIN, OUTPUT); + digitalWrite(LED_BUILTIN, LOW); + digitalWrite(ENABLE_PIN, LOW); + + points.push_back(new CPoint(100, 100)); + points.push_back(new CPoint(200, 200)); + points.push_back(new CPoint(300, 300)); + points.push_back(new CPoint(400, 400)); + points.push_back(new CPoint(500, 500)); + points.push_back(new CPoint(600, 600)); + points.push_back(new CPoint(700, 700)); + points.push_back(new CPoint(800, 800)); + points.push_back(new CPoint(900, 900)); + points.push_back(new CPoint(1000, 1000)); + + #ifdef TEENSY3 + Serial.println("TEENSY3"); + #else + Serial.println("STM32F4"); + #endif +} + +void loop(){ + delay(500); + digitalWrite(LED_BUILTIN, HIGH); + Serial.println("Starting new sequence"); + + for(auto& point : points){ + Serial.printf("Moving to x: %d, y: %d\r\n", point->x, point->y); + stepper_x.setTargetAbs(point->x); + stepper_y.setTargetAbs(point->y); + controller.moveAsync(stepper_x, stepper_y); + while(controller.isRunning()); + } + + //controller.step_calls = controller.pulse_calls = controller.acc_calls = 0; + //Serial.printf("steps: %u, pulses: %u, acc: %u\r\n", controller.step_calls, controller.pulse_calls, controller.acc_calls); + digitalWrite(LED_BUILTIN, LOW); +} + +#endif diff --git a/src/timer/TimerFieldBase.h b/src/timer/TimerFieldBase.h index 9771a32..9fd824a 100644 --- a/src/timer/TimerFieldBase.h +++ b/src/timer/TimerFieldBase.h @@ -1,10 +1,9 @@ -#if defined(TEENSY4) #include "TF_Handler.h" class ITimerField { public: - ITimerField(TF_Handler *); + ITimerField(TeensyStep::TF_Handler *); virtual bool begin()=0; virtual void end()=0; @@ -21,5 +20,3 @@ class ITimerField virtual void setPulseWidth(unsigned delay)=0; virtual void triggerDelay()=0; }; - -#endif \ No newline at end of file diff --git a/src/timer/teensy4/TimerField.h b/src/timer/teensy4/TimerField.h index 10b06f7..4d79aca 100644 --- a/src/timer/teensy4/TimerField.h +++ b/src/timer/teensy4/TimerField.h @@ -1,4 +1,4 @@ -#if defined(TEENSY4) +#if defined(__IMXRT1052__) #pragma once #include "wiring.h" @@ -158,4 +158,4 @@ void TimerField::delayISR(unsigned channel) // } } -#endif \ No newline at end of file +#endif diff --git a/src/timer/teensy4/config.h b/src/timer/teensy4/config.h index 2bb8442..110e866 100644 --- a/src/timer/teensy4/config.h +++ b/src/timer/teensy4/config.h @@ -1,4 +1,4 @@ -#if defined(TEENSY4) +#if defined(__IMXRT1052__) #pragma once #include @@ -171,4 +171,4 @@ namespace TeensyStepFTM return mu * 1E-6 * _timer_frequency / (1 << prescale) + 0.5; } } -#endif \ No newline at end of file +#endif diff --git a/src/timer/teensy4/ticktimer.h b/src/timer/teensy4/ticktimer.h index 7877ab1..76851c7 100644 --- a/src/timer/teensy4/ticktimer.h +++ b/src/timer/teensy4/ticktimer.h @@ -1,4 +1,4 @@ -#if defined(TEENSY4) +#if defined(__IMXRT1052__) #include #include "Arduino.h" //#include @@ -98,4 +98,4 @@ void tickTimer::setFrequency(unsigned f) channelInfo *tickTimer::channels = new channelInfo[tickTimer::maxTimers + 1]; -#endif \ No newline at end of file +#endif From e5b6d6dc628b7cee912d9c292f627646037ceead Mon Sep 17 00:00:00 2001 From: Erik tideman Date: Tue, 22 Sep 2020 17:21:43 +0200 Subject: [PATCH 34/35] Cleanup of arduino headers inclusion --- src/ErrorHandler.cpp | 1 + src/ErrorHandler.h | 2 +- src/timer/stm32/TimerField.h | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ErrorHandler.cpp b/src/ErrorHandler.cpp index 04eb410..d62655a 100644 --- a/src/ErrorHandler.cpp +++ b/src/ErrorHandler.cpp @@ -1,3 +1,4 @@ +#include #include "ErrorHandler.h" namespace TeensyStep diff --git a/src/ErrorHandler.h b/src/ErrorHandler.h index 018265a..f5e0f29 100644 --- a/src/ErrorHandler.h +++ b/src/ErrorHandler.h @@ -1,5 +1,5 @@ #pragma once -#include +#include "Stream.h" namespace TeensyStep { diff --git a/src/timer/stm32/TimerField.h b/src/timer/stm32/TimerField.h index 6218775..4a2a828 100644 --- a/src/timer/stm32/TimerField.h +++ b/src/timer/stm32/TimerField.h @@ -1,7 +1,7 @@ #if defined(STM32F4xx) #pragma once -#include +#include #include #include "../TF_Handler.h" From 64791f8bd75307de23959bd916782a4c3ce067ad Mon Sep 17 00:00:00 2001 From: Erik Tideman Date: Tue, 22 Sep 2020 17:23:33 +0200 Subject: [PATCH 35/35] Removed test files --- platformio.ini | 21 ---------------- src/main.cpp | 68 -------------------------------------------------- 2 files changed, 89 deletions(-) delete mode 100644 platformio.ini delete mode 100644 src/main.cpp diff --git a/platformio.ini b/platformio.ini deleted file mode 100644 index 1c4796c..0000000 --- a/platformio.ini +++ /dev/null @@ -1,21 +0,0 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[env:nucleo_f429zi] -platform = ststm32 -board = nucleo_f429zi -build_flags = -DTEST -framework = arduino - -[env:teensy35] -platform = teensy -board = teensy35 -build_flags = -DTEENSY3 -DTEST -framework = arduino diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 59c8a65..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,68 +0,0 @@ - -#ifdef TEST -#include -#include -#include "TeensyStep.h" - -using std::vector; - -#define ENABLE_PIN 8 - -Stepper stepper_x(2, 5); // STEP pin: 2, DIR pin: 3 -Stepper stepper_y(3, 6); -StepControl controller; // Use default settings - - -class CPoint{ - public: - CPoint(int x, int y): x(x), y(y) {} - int x = 0; - int y = 0; -}; - -vector points; - -void setup(){ - Serial.begin(115200); - pinMode(LED_BUILTIN, OUTPUT); - pinMode(ENABLE_PIN, OUTPUT); - digitalWrite(LED_BUILTIN, LOW); - digitalWrite(ENABLE_PIN, LOW); - - points.push_back(new CPoint(100, 100)); - points.push_back(new CPoint(200, 200)); - points.push_back(new CPoint(300, 300)); - points.push_back(new CPoint(400, 400)); - points.push_back(new CPoint(500, 500)); - points.push_back(new CPoint(600, 600)); - points.push_back(new CPoint(700, 700)); - points.push_back(new CPoint(800, 800)); - points.push_back(new CPoint(900, 900)); - points.push_back(new CPoint(1000, 1000)); - - #ifdef TEENSY3 - Serial.println("TEENSY3"); - #else - Serial.println("STM32F4"); - #endif -} - -void loop(){ - delay(500); - digitalWrite(LED_BUILTIN, HIGH); - Serial.println("Starting new sequence"); - - for(auto& point : points){ - Serial.printf("Moving to x: %d, y: %d\r\n", point->x, point->y); - stepper_x.setTargetAbs(point->x); - stepper_y.setTargetAbs(point->y); - controller.moveAsync(stepper_x, stepper_y); - while(controller.isRunning()); - } - - //controller.step_calls = controller.pulse_calls = controller.acc_calls = 0; - //Serial.printf("steps: %u, pulses: %u, acc: %u\r\n", controller.step_calls, controller.pulse_calls, controller.acc_calls); - digitalWrite(LED_BUILTIN, LOW); -} - -#endif