├── .gitattributes ├── .gitignore ├── LICENCE ├── README.md ├── examples ├── Applications │ └── Winder │ │ ├── Winder.cpp │ │ ├── Winder.h │ │ ├── Winder.ino │ │ └── readme.md ├── HelloStepper │ └── HelloStepper.ino ├── Interfacing │ └── Interfacing.ino ├── MultipleSteppers │ └── MultipleSteppers.ino ├── Path_Following │ └── RoseFunctionFollower │ │ └── RoseFunctionFollower.ino └── StepperArray │ ├── StepperArray.ino │ └── readme.md ├── keywords.txt ├── library.json ├── library.properties ├── media ├── indMove.png ├── load_calculation.PNG ├── load_calculation.xlsx ├── seqMove.png ├── stepperArray.png └── sycMove.png └── src ├── MotorControlBase.h ├── RotateControlBase.h ├── StepControlBase.h ├── Stepper.cpp ├── Stepper.h ├── TeensyStep.h ├── accelerators ├── LinRotAccelerator.h ├── LinStepAccelerator.h └── SinRotAccelerator.h ├── timer ├── TF_Handler.h ├── TimerFieldBase.h ├── esp32 │ ├── TimerField.cpp │ └── TimerField.h ├── generic │ ├── TickTimer.cpp │ ├── TickTimer.h │ └── TimerField.h ├── teensy3 │ ├── PIT.cpp │ ├── PIT.h │ ├── TeensyStepFTM.cpp │ ├── TeensyStepFTM.h │ ├── TimerField2.h │ └── config.h └── teensy4 │ ├── PIT.cppc │ ├── PIT.hc │ ├── TeensyStepFTM.cppbak │ ├── TeensyStepFTM.hh │ ├── TimerField.h │ ├── config.h │ └── ticktimer.h └── version.h /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Ignore docs files 6 | _gh_pages 7 | _site* 8 | _drafts 9 | .vscode* 10 | .ruby-version 11 | 12 | # Folder config file 13 | Desktop.ini 14 | 15 | # Recycle Bin used on file shares 16 | $RECYCLE.BIN/ 17 | 18 | # Windows Installer files 19 | *.cab 20 | *.msi 21 | *.msm 22 | *.msp 23 | 24 | # Windows shortcuts 25 | *.lnk 26 | 27 | # ========================= 28 | # Operating System Files 29 | # ========================= 30 | 31 | # OSX 32 | # ========================= 33 | 34 | .DS_Store 35 | .AppleDouble 36 | .LSOverride 37 | 38 | # Thumbnails 39 | ._* 40 | 41 | # Files that might appear in the root of a volume 42 | .DocumentRevisions-V100 43 | .fseventsd 44 | .Spotlight-V100 45 | .TemporaryItems 46 | .Trashes 47 | .VolumeIcon.icns 48 | 49 | # Directories potentially created on remote AFP share 50 | .AppleDB 51 | .AppleDesktop 52 | Network Trash Folder 53 | Temporary Items 54 | .apdisk 55 | -------------------------------------------------------------------------------- /LICENCE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Lutz Niggl 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ESP32Step - Fast Stepper Library for ESP32 boards 2 | 3 | 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. 4 | 5 | ## Purpose of the Library 6 | **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): 7 | 8 | | Description | Specification | Default | 9 | |:-------------------------------------------|:-------------------------:|:----------------:| 10 | | Motor speed / pulse rate |1 - 300'000 stp/s | 800 stp/s | 11 | | Acceleration | 0 - 500'000 stp/s^2 | 2500 stp/s^2 | 12 | | Pull in speed | 50-10'000 stp/s | 100 stp/s | 13 | | Synchronous movement of motors | up to 10 | - | 14 | | Independent movement of motors | 4 groups of 1 to 10 motors| - | 15 | | Settable step pulse polarity | Active HIGH or LOW | Active HIGH | 16 | | Settable step pulse width | 1-100µs | 5µs | 17 | | Settable direction signal polarity | cw / ccw | cw | 18 | 19 | ## Architecture 20 | 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. 21 | 22 | ## TODO 23 | 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)). 24 | -------------------------------------------------------------------------------- /examples/Applications/Winder/Winder.cpp: -------------------------------------------------------------------------------- 1 | #include "Winder.h" 2 | 3 | Winder::Winder(Stepper &_spindle, Stepper &_feeder) 4 | : spindle(_spindle), feeder(_feeder), feederCtrl(5, 1000), spindleCtrl(5, 1000) 5 | { 6 | } 7 | 8 | Winder& Winder::setSpindleParams(unsigned stepsPerRev, unsigned acc) 9 | { 10 | spindleStpPerRev = stepsPerRev; 11 | spindleAcc = acc; 12 | 13 | spindle.setMaxSpeed(1); // -> parameter of overrideSpeed equals real speed 14 | spindle.setAcceleration(spindleAcc); 15 | 16 | oldSpindleSpeed = 0.0f; 17 | oldPitch = 1.0f; 18 | 19 | return *this; 20 | } 21 | 22 | Winder& Winder::setFeederParams(unsigned stpPerMM, unsigned acc) 23 | { 24 | feederStpPerMM = stpPerMM; 25 | feederAcc = acc; 26 | 27 | feeder.setMaxSpeed(1); 28 | feeder.setAcceleration(spindleAcc); 29 | 30 | return *this; 31 | } 32 | 33 | void Winder::begin() 34 | { 35 | pitchFactor = (float)feederStpPerMM / spindleStpPerRev; 36 | 37 | // startup controllers 38 | feederCtrl.rotateAsync(feeder); 39 | feederCtrl.overrideSpeed(0.0f); 40 | 41 | spindleCtrl.rotateAsync(spindle); 42 | spindleCtrl.overrideSpeed(0.0f); 43 | } 44 | 45 | void Winder::setSpindleSpeed(float rpm) 46 | { 47 | targetSpindleSpeed = rpm / 60.0f * spindleStpPerRev; 48 | } 49 | 50 | void Winder::setPitch(float pitch) 51 | { 52 | if(pitch > 0) targetPitch = pitch; 53 | } 54 | 55 | void Winder::updateSpeeds() 56 | { 57 | float targetFeederSpeed = targetPitch * pitchFactor * targetSpindleSpeed; 58 | 59 | if(targetSpindleSpeed != oldSpindleSpeed) // if target speed changed -> update all 60 | { 61 | oldSpindleSpeed = targetSpindleSpeed; 62 | 63 | float feederAccFactor = targetPitch * pitchFactor; 64 | 65 | //Serial.println(feederAccFactor); 66 | 67 | feederCtrl.overrideAcceleration(feederAccFactor); 68 | feederCtrl.overrideSpeed(targetFeederSpeed); 69 | spindleCtrl.overrideSpeed(targetSpindleSpeed); 70 | } 71 | else if(targetPitch != oldPitch) // if only target pitch changed, update feeder speed only 72 | { 73 | oldPitch = targetPitch; 74 | feederCtrl.overrideAcceleration(feederAcc / spindleAcc); 75 | feederCtrl.overrideSpeed(targetFeederSpeed); 76 | } 77 | } -------------------------------------------------------------------------------- /examples/Applications/Winder/Winder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "TeensyStep.h" 4 | 5 | class Winder 6 | { 7 | public: 8 | Winder(Stepper &spindle, Stepper &feeder); 9 | 10 | void begin(); 11 | 12 | Winder &setSpindleParams(unsigned stpPerRev, unsigned acceleration); // steps per spindle revolution & acceleration in stp/s^2 13 | Winder &setFeederParams(unsigned stpPerMM, unsigned acceleration); // steps to move the feeder 1mm, acceleration for pitch trimming 14 | 15 | void setSpindleSpeed(float rpm); // changes the spindle speed to the given rpm 16 | void setPitch(float pitch_in_mm); // changes the winder pitch to the given value 17 | void updateSpeeds(); 18 | 19 | inline int getCurSpindleSpeed() { return spindleCtrl.isRunning() ? spindleCtrl.getCurrentSpeed() : 0; } 20 | inline int getCurFeederSpeed() { return feederCtrl.isRunning() ? feederCtrl.getCurrentSpeed() : 0; } 21 | inline float getCurPitch(){return (float)getCurFeederSpeed()/getCurSpindleSpeed()/pitchFactor; } 22 | 23 | protected: 24 | Stepper &spindle; 25 | Stepper &feeder; 26 | 27 | unsigned spindleStpPerRev, spindleAcc, feederStpPerMM, feederAcc; 28 | float pitchFactor; 29 | 30 | float targetSpindleSpeed, targetPitch; 31 | float oldSpindleSpeed, oldPitch; 32 | 33 | RotateControl feederCtrl, spindleCtrl; 34 | }; -------------------------------------------------------------------------------- /examples/Applications/Winder/Winder.ino: -------------------------------------------------------------------------------- 1 | #include "Encoder.h" 2 | #include "TeensyStep.h" 3 | #include "Winder.h" 4 | 5 | constexpr unsigned feederStpPerMM = 200 * 8 * 5.0f * 0.8f; // e.g. fullstep/rev * microstepping * leadscrew pitch * gearRatio 6 | constexpr unsigned spindleStpPerRev = 200 * 16; // e.g. fullstep/rev * microstepping 7 | constexpr unsigned spindleAcceleration = 15000; 8 | 9 | Stepper spindle(0, 1); 10 | Stepper feeder(2, 3); 11 | Winder winder(spindle, feeder); 12 | 13 | float pitch = 0.2; //mm 14 | 15 | Encoder trimmer(5, 6); 16 | int oldTrimmerVal = 0; 17 | 18 | IntervalTimer printTimer; // used to print out current speeds in the background 19 | 20 | void setup() 21 | { 22 | while (!Serial && millis() < 500); 23 | 24 | // setup background printing 25 | printTimer.begin(printCurrent, 25000); 26 | printTimer.priority(255); // lowest priority, we don't want to disturb stepping 27 | 28 | // setup the winder 29 | winder 30 | .setSpindleParams(spindleStpPerRev, spindleAcceleration) 31 | .setFeederParams(feederStpPerMM, 50000) 32 | .begin(); 33 | 34 | // startup the winder 35 | winder.setSpindleSpeed(600); // spindle speed in rpm 36 | winder.setPitch(0.2f); // pitch in mm 37 | winder.updateSpeeds(); // apply new settings 38 | delay(3000); 39 | } 40 | 41 | void loop() 42 | { 43 | // read in the trimmer and adjust the pitch (+/- 0.01 per encoder detent) 44 | int trimmerVal = trimmer.read(); 45 | if (trimmerVal != oldTrimmerVal) 46 | { 47 | oldTrimmerVal = trimmerVal; 48 | pitch = 0.2f + trimmerVal / 400.0f; // e.g. 0.01 per 4 encoder steps 49 | winder.setPitch(pitch); 50 | winder.updateSpeeds(); 51 | } 52 | 53 | if (millis() > 15000) // stop the spindle after 15s 54 | { 55 | winder.setSpindleSpeed(0); 56 | winder.updateSpeeds(); 57 | while (1) 58 | yield(); // stop sketch here 59 | } 60 | 61 | delay(50); // dont overrun the winder by sending new values to often. 62 | } 63 | 64 | // helpers ---------------------------------------------- 65 | 66 | void printCurrent() 67 | { 68 | unsigned t = millis(); 69 | unsigned feederSpeed = winder.getCurFeederSpeed(); 70 | unsigned spindleSpeed = winder.getCurSpindleSpeed(); 71 | float curPitch = winder.getCurPitch(); 72 | 73 | if (spindleSpeed != 0 || feederSpeed != 0) 74 | { 75 | Serial.printf("%d\t%i\t%i\t%.3f\t%.3f\n", t, spindleSpeed, feederSpeed, curPitch, pitch); 76 | } 77 | } -------------------------------------------------------------------------------- /examples/Applications/Winder/readme.md: -------------------------------------------------------------------------------- 1 | To be done -------------------------------------------------------------------------------- /examples/HelloStepper/HelloStepper.ino: -------------------------------------------------------------------------------- 1 | /*========================================================================== 2 | * This is a minimal sketch showing the usage of TeensyStep 3 | * 4 | * STEP Pulses on Pin 2 (can be any pin) 5 | * DIR Signall on Pin 3 (can be any pin) 6 | * 7 | * The target position is set to 1000 steps relative to the 8 | * current position. The move command of the controller 9 | * moves the motor to the target position. 10 | * 11 | * Default parameters are 12 | * Speed: 800 steps/s 13 | * Acceleration: 2500 steps/s^2 14 | * 15 | * (slow, but good to start with since they will work on any normal stepper) 16 | * 17 | ===========================================================================*/ 18 | 19 | #include "TeensyStep.h" 20 | 21 | Stepper motor(2, 3); // STEP pin: 2, DIR pin: 3 22 | StepControl controller; // Use default settings 23 | 24 | void setup() 25 | { 26 | } 27 | 28 | void loop() 29 | { 30 | motor.setTargetRel(1000); // Set target position to 1000 steps from current position 31 | controller.move(motor); // Do the move 32 | delay(500); 33 | } 34 | -------------------------------------------------------------------------------- /examples/Interfacing/Interfacing.ino: -------------------------------------------------------------------------------- 1 | /*---------------------------------------------------------- 2 | This example demonstrates how to use the serial interface and 3 | digital IO while the controller moves a motor in 4 | the background. 5 | 6 | The following serial commands are implemented 7 | m: move motor 8 | s: start stop sequence 9 | e: emergency stop 10 | h: help 11 | 12 | Additionally a pin can be used to stop the motor 13 | 14 | ------------------------------------------------------------*/ 15 | #include "TeensyStep.h" 16 | 17 | // stepper and controller 18 | constexpr int stpPin = 0, dirPin = 1; 19 | Stepper motor(stpPin, dirPin); 20 | StepControl controller; 21 | 22 | // pin to stop the motor, connect a push button to this pin 23 | constexpr int stopPin = 2; 24 | 25 | // stopwatches 26 | elapsedMillis displayStopwatch = 0; // timing the display of the current position 27 | elapsedMillis blinkStopwatch = 0; // timing the heartbeat LED 28 | elapsedMillis debounceTimer = 0; // debouncing input pins 29 | 30 | int lastPos = 0; 31 | 32 | void handlePins(); 33 | void handleCommands(); 34 | 35 | void setup() 36 | { 37 | while (!Serial); 38 | Serial.println("Simple Serial Stepper Example"); 39 | Serial.println("(type h for help)"); 40 | 41 | 42 | motor.setMaxSpeed(5000); 43 | motor.setAcceleration(50000); 44 | 45 | pinMode(LED_BUILTIN, OUTPUT); 46 | pinMode(stopPin, INPUT_PULLUP); // touch the pin with GND to stop the motor 47 | } 48 | 49 | 50 | void loop() 51 | { 52 | // handle incomming commands on the serial interface ------------------ 53 | handleCommands(); 54 | 55 | // handle input from pins --------------------------------------------- 56 | handlePins(); 57 | 58 | // display the current motor position every 20ms ---------------------- 59 | if (displayStopwatch > 20) 60 | { 61 | displayStopwatch = 0; 62 | 63 | int currentPos = motor.getPosition(); 64 | if (currentPos != lastPos) // only display if it changed 65 | { 66 | lastPos = currentPos; 67 | Serial.println(currentPos); 68 | } 69 | } 70 | 71 | // the usual heartbeat ------------------------------------------------ 72 | if (blinkStopwatch > 250) 73 | { 74 | blinkStopwatch = 0; 75 | digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN)); // toggle LED 76 | } 77 | } 78 | 79 | 80 | //------------------------------------------------ 81 | // Very simple command interface on USB-Serial: 82 | // m : starts the motor 83 | // s : starts stop sequence 84 | // e : emergency stop 85 | 86 | void handleCommands() 87 | { 88 | if (Serial.available() > 0) // skip if the serial buffer is empty 89 | { 90 | char cmd = Serial.read(); // get one char from the buffer... 91 | switch (cmd) // ... and analyze it 92 | { 93 | case 'm': // move command 94 | if (!controller.isRunning()) // skip move command if motor is running already 95 | { 96 | motor.setTargetRel(20000); 97 | controller.moveAsync(motor); 98 | Serial.println("Started motor movement"); 99 | } 100 | else 101 | { 102 | Serial.println("Ignored, motor is already running"); 103 | } 104 | break; 105 | 106 | case 's': // stop command 107 | controller.stopAsync(); // initiate stopping procedure 108 | Serial.println("Stopping motor"); 109 | break; 110 | 111 | case 'e': // emergency stop command 112 | controller.emergencyStop(); 113 | Serial.println("Emergency Stop"); 114 | break; 115 | 116 | case 'h': // help / usage command 117 | case 'u': 118 | Serial.println("\nUsage:"); 119 | Serial.println(" m: move motor"); 120 | Serial.println(" s: start stop sequence"); 121 | Serial.println(" e: emergency stop"); 122 | Serial.println(" h: display this help"); 123 | break; 124 | 125 | default: 126 | break; 127 | } 128 | } 129 | } 130 | 131 | 132 | //------------------------------------------------ 133 | // Connect a pushbutton to the stop pin. 134 | // Stop command will be issued when pin is 135 | // pulled to GND. 136 | // Only very simple debouncing implemented. Use "debounce2.h" or 137 | // similar for a real application 138 | 139 | void handlePins() 140 | { 141 | if (controller.isRunning() && !digitalReadFast(stopPin) && debounceTimer > 200) 142 | { 143 | debounceTimer = 0; 144 | controller.stopAsync(); // initiate stopping procedure 145 | Serial.println("Stopping motor"); 146 | } 147 | } 148 | -------------------------------------------------------------------------------- /examples/MultipleSteppers/MultipleSteppers.ino: -------------------------------------------------------------------------------- 1 | /*========================================================================== 2 | * The sketch shows how to move more than one motor. 3 | * 4 | * If more than one motor is moved by one controller all motors will arrive at 5 | * their targets at the same time. E.g., if the motors are part of a 6 | * x/y transport system, the transport move on a straight diagonal line to the 7 | * target coordinates. 8 | * 9 | * The sketch also shows examples how the motor properties are set up 10 | * 11 | * A 1/16 microstep driver is assumed. You probably want to adjust speed, 12 | * acceleration and distances if you are using a driver with another microstep 13 | * resolution. 14 | ===========================================================================*/ 15 | 16 | #include "TeensyStep.h" 17 | 18 | Stepper motor_1(2, 3); //STEP pin = 2, DIR pin = 3 19 | Stepper motor_2(9,10); //STEP pin = 9, DIR pin = 10 20 | Stepper motor_3(14,15); //STEP pin = 14, DIR pin = 15 21 | 22 | StepControl controller; 23 | 24 | void setup() 25 | { 26 | // setup the motors 27 | motor_1 28 | .setMaxSpeed(50000) // steps/s 29 | .setAcceleration(200000); // steps/s^2 30 | 31 | motor_2 32 | .setMaxSpeed(50000) // steps/s 33 | .setAcceleration(200000); // steps/s^2 34 | motor_3 35 | //.setPullInSpeed(300) // steps/s currently deactivated... 36 | .setMaxSpeed(10000) // steps/s 37 | .setAcceleration(50000) // steps/s^2 38 | .setStepPinPolarity(LOW); // driver expects active low pulses 39 | } 40 | 41 | void loop() 42 | { 43 | constexpr int spr = 16*200; // 3200 steps per revolution 44 | 45 | // lets shake 46 | for(int i = 0; i < 5; i++) 47 | { 48 | motor_1.setTargetRel(spr/4); // 1/4 revolution 49 | controller.move(motor_1); 50 | 51 | motor_1.setTargetRel(-spr/4); 52 | controller.move(motor_1); 53 | } 54 | delay(500); 55 | 56 | // move motor_1 to absolute position (10 revolutions from zero) 57 | // move motor_2 half a revolution forward 58 | // both motors will arrive their target positions at the same time 59 | motor_1.setTargetAbs(10*spr); 60 | motor_2.setTargetRel(spr/2); 61 | controller.move(motor_1, motor_2); 62 | 63 | // now move motor_2 together with motor_3 64 | motor_2.setTargetRel(300); 65 | motor_3.setTargetRel(-800); 66 | controller.move(motor_2, motor_3); 67 | 68 | // move all motors back to their start positions 69 | motor_1.setTargetAbs(0); 70 | motor_2.setTargetAbs(0); 71 | motor_3.setTargetAbs(0); 72 | controller.move(motor_1, motor_2, motor_3); 73 | 74 | delay(1000); 75 | } -------------------------------------------------------------------------------- /examples/Path_Following/RoseFunctionFollower/RoseFunctionFollower.ino: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "TeensyStep.h" 3 | 4 | //spindle settings 5 | constexpr unsigned rpm = 15; 6 | constexpr unsigned spindleSPR = 6400; // stp per rev 7 | constexpr unsigned spindleSpeed = rpm * spindleSPR / 60; // stp/sec 8 | constexpr unsigned spindleStepPin = 1; 9 | constexpr unsigned spindleDirPin = 2; 10 | 11 | //slide settings 12 | constexpr unsigned slideAmplitude = 10000; // stp 13 | constexpr unsigned slideSpeed = 35000; //stp/sec 14 | constexpr unsigned slideStepPin = 3; 15 | constexpr unsigned slideDirPin = 4; 16 | 17 | //accuracy 18 | IntervalTimer tickTimer; 19 | constexpr unsigned recalcPeriod = 25'000; //µs period for calculation of new target points. Straight lines between those points. 20 | constexpr float dt = recalcPeriod / 1E6; // seconds 21 | 22 | // rose fuction 23 | constexpr int n = 5; 24 | constexpr int d = 8; 25 | constexpr float k = (float)n / d; 26 | 27 | float slideFunc(float spindleAngle) 28 | { 29 | float phi = fmodf(spindleAngle * k, TWO_PI); 30 | return slideAmplitude * cosf(phi); 31 | } 32 | 33 | // TeensyStep 34 | RotateControl slideController; 35 | RotateControl spindleController; 36 | 37 | Stepper spindle(spindleStepPin, spindleDirPin); 38 | Stepper slide(slideStepPin, slideDirPin); 39 | 40 | 41 | //------------------------------------------------------------------------------------ 42 | // tick() 43 | // 44 | // This function is called periodically with period recalcPeriod. 45 | // It calculates 46 | // 1) a new target value for the slide depending on the spindle angle 47 | // 2) the new speed for the spindle so that it will reach the target until it is called again 48 | 49 | void tick() 50 | { 51 | float spindleAngle = spindle.getPosition() * (TWO_PI / spindleSPR); //convert steps to angle 52 | float slidePosition = slide.getPosition(); 53 | float slideTarget = slideFunc(spindleAngle); 54 | 55 | float newSpeed = (slideTarget - slidePosition) / dt; // speed to reach target in given delta t (neglecting acceleration) 56 | float speedFac = newSpeed / slideSpeed; // transform in relative factor (-1.0 .. 1.0) 57 | 58 | slideController.overrideSpeed(speedFac); // set new speed 59 | } 60 | 61 | void setup() 62 | { 63 | pinMode(LED_BUILTIN, OUTPUT); 64 | 65 | while (!Serial && millis() < 1000); 66 | 67 | spindle 68 | .setMaxSpeed(spindleSpeed) 69 | .setAcceleration(50000); 70 | 71 | slide 72 | .setMaxSpeed(slideSpeed) 73 | .setAcceleration(150000) 74 | .setPosition(slideFunc(0)); // set start position of counter 75 | 76 | spindleController.rotateAsync(spindle); // let the spindle run with constant speed 77 | slideController.rotateAsync(slide); 78 | slideController.overrideSpeed(0); // start with stopped slide 79 | tick(); 80 | 81 | // use a timer to periodically calculate new targets for the slide 82 | tickTimer.priority(255); // lowest priority, potentially long caclulations need to be interruptable by TeensyStep 83 | tickTimer.begin(tick, recalcPeriod); 84 | } 85 | 86 | void loop() 87 | { 88 | digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN)); 89 | 90 | // print current values of spindle angle [rad] and slide position [steps] 91 | float phi = spindle.getPosition() * (TWO_PI / spindleSPR); 92 | int32_t r = slide.getPosition(); 93 | 94 | Serial.printf("%f\t%d\n", phi, r); 95 | 96 | delay(20); 97 | } 98 | -------------------------------------------------------------------------------- /examples/StepperArray/StepperArray.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Stepper M1(0, 1), M2(2, 3), M3(4, 5), M4(6, 7), M5(8, 9), M6(10, 11); // create 6 motors 4 | StepControl controller; 5 | 6 | void setup() 7 | { 8 | pinMode(LED_BUILTIN,OUTPUT); 9 | delay(100); 10 | 11 | Stepper* motorSet_A[] = {&M1, &M2, &M5, &M6 }; // define an array of pointers to motors 1,2,5 and 6 12 | M1.setTargetAbs(200); // set targets for the motors 13 | M2.setTargetAbs(-500); 14 | M5.setTargetAbs(-100); 15 | M6.setTargetAbs(30); 16 | controller.move(motorSet_A); // move all motors in the array to their targets 17 | 18 | delay(500); // just to generate a nice output on the logic analyzer 19 | 20 | Stepper* motorSet_B[] = {&M1, &M3, &M4}; // another set of motors; 21 | M1.setTargetAbs(-110); 22 | M3.setTargetAbs(50); 23 | M4.setTargetAbs(230); 24 | controller.move(motorSet_B); // move set B to target 25 | 26 | delay(500); // just to generate a nice output on the logic analyzer 27 | 28 | for (int i = 0; i < 4; i++) { // loop through all motors in motorSet_A... 29 | motorSet_A[i]->setTargetAbs(0); // ... set targets to 0... 30 | } 31 | controller.move(motorSet_A); // ... and move home 32 | } 33 | 34 | void loop(){ 35 | digitalWriteFast(LED_BUILTIN,!digitalReadFast(LED_BUILTIN)); 36 | delay(100); 37 | } -------------------------------------------------------------------------------- /examples/StepperArray/readme.md: -------------------------------------------------------------------------------- 1 | This example shows the usage of more than 3 motors. Here a screenshot of the step pulses generated by the sketch. 2 | 3 | ![Alt text](/media/stepperArray.png?raw=true "Sketch output") -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ######################################################## 2 | # Syntax Coloring Map for SparkFun LSM6DS3 IMU Library # 3 | ######################################################## 4 | # Class 5 | ################################################################### 6 | 7 | 8 | ################################################################### 9 | # Methods and Functions 10 | ################################################################### 11 | 12 | begin KEYWORD2 13 | Trigger KEYWORD2 14 | AddChannel KEYWORD2 15 | 16 | ################################################################### 17 | # Constants 18 | ################################################################### 19 | 20 | -------------------------------------------------------------------------------- /library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "TeensyStep", 3 | "keywords": "teensy, esp32, stepper, motor, high-speed", 4 | "description": "High speed stepper driver for teensy boards (T3.0 - T3.6) and ESP32", 5 | "repository": 6 | { 7 | "type": "git", 8 | "url": "https://github.com/luni64/TeensyStep" 9 | }, 10 | "authors": 11 | { 12 | "name": "Lutz Niggl", 13 | "email": "lutz.niggl@lunoptics.com", 14 | "url": "https://github.com/luni64", 15 | "maintainer": true 16 | }, 17 | "homepage": "https://luni64.github.io/TeensyStep", 18 | "version": "2.0.1", 19 | "frameworks": "arduino", 20 | "platforms": "Teensy" 21 | } 22 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=TeensyStep 2 | version=2.0.1 3 | author=Lutz Niggl 4 | maintainer=Lutz Niggl 5 | sentence=High speed stepper driver for PJRC Teensy boards (T3.0 - T3.6) and ESP32 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 together with sensors, displays, serial communication ... 7 | category=Device Control 8 | url=https://luni64.github.io/TeensyStep/ 9 | architectures=* 10 | includes=TeensyStep.h 11 | -------------------------------------------------------------------------------- /media/indMove.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rcp1/ESP32Step/ea108bb95b6f395dc7551356fa6f5e498f673ffc/media/indMove.png -------------------------------------------------------------------------------- /media/load_calculation.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rcp1/ESP32Step/ea108bb95b6f395dc7551356fa6f5e498f673ffc/media/load_calculation.PNG -------------------------------------------------------------------------------- /media/load_calculation.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rcp1/ESP32Step/ea108bb95b6f395dc7551356fa6f5e498f673ffc/media/load_calculation.xlsx -------------------------------------------------------------------------------- /media/seqMove.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rcp1/ESP32Step/ea108bb95b6f395dc7551356fa6f5e498f673ffc/media/seqMove.png -------------------------------------------------------------------------------- /media/stepperArray.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rcp1/ESP32Step/ea108bb95b6f395dc7551356fa6f5e498f673ffc/media/stepperArray.png -------------------------------------------------------------------------------- /media/sycMove.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rcp1/ESP32Step/ea108bb95b6f395dc7551356fa6f5e498f673ffc/media/sycMove.png -------------------------------------------------------------------------------- /src/MotorControlBase.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "timer/TF_Handler.h" 4 | #include "Stepper.h" 5 | 6 | constexpr int MaxMotors = 10; 7 | 8 | template 9 | class MotorControlBase : TF_Handler 10 | { 11 | public: 12 | bool isRunning(); 13 | inline int getCurrentSpeed() 14 | { 15 | return timerField.getStepFrequency(); 16 | } 17 | 18 | void emergencyStop() { timerField.stepTimerStop();} 19 | 20 | virtual ~MotorControlBase(); 21 | bool isOk() const { return OK; } 22 | 23 | void stepTimerISR(); 24 | void pulseTimerISR(); 25 | 26 | protected: 27 | TimerField timerField; 28 | MotorControlBase(unsigned pulseWidth, unsigned accUpdatePeriod); 29 | 30 | template 31 | void attachStepper(Stepper *(&motors)[N]); 32 | template 33 | void attachStepper(Stepper &stepper, Steppers &... steppers); 34 | void attachStepper(Stepper &stepper); 35 | 36 | 37 | Stepper *motorList[MaxMotors + 1]; 38 | Stepper *leadMotor; 39 | 40 | void (*callback)() = nullptr; 41 | 42 | bool OK = false; 43 | 44 | unsigned mCnt; 45 | 46 | enum class Mode 47 | { 48 | target, 49 | notarget 50 | } mode = Mode::notarget; 51 | 52 | uint32_t accUpdatePeriod; 53 | uint32_t pulseWidth; 54 | 55 | MotorControlBase(const MotorControlBase &) = delete; 56 | MotorControlBase &operator=(const MotorControlBase &) = delete; 57 | }; 58 | 59 | // Implementation ============================================================================ 60 | 61 | template 62 | bool MotorControlBase::isRunning() 63 | { 64 | return timerField.stepTimerIsRunning(); 65 | } 66 | 67 | template 68 | MotorControlBase::MotorControlBase(unsigned pulseWidth, unsigned accUpdatePeriod) 69 | : timerField(this), mCnt(0) 70 | { 71 | OK = timerField.begin(); 72 | timerField.setPulseWidth(pulseWidth); 73 | timerField.setAccUpdatePeriod(accUpdatePeriod); 74 | this->accUpdatePeriod = accUpdatePeriod; 75 | this->pulseWidth = pulseWidth; 76 | } 77 | 78 | template 79 | MotorControlBase::~MotorControlBase() 80 | { 81 | if(OK) emergencyStop(); 82 | } 83 | 84 | template 85 | void MotorControlBase::stepTimerISR() 86 | { 87 | leadMotor->doStep(); // move master motor 88 | 89 | Stepper **slave = motorList; 90 | while (*(++slave) != nullptr) // move slave motors if required (https://en.wikipedia.org/wiki/Bresenham) 91 | { 92 | if ((*slave)->B >= 0) 93 | { 94 | (*slave)->doStep(); 95 | (*slave)->B -= leadMotor->A; 96 | } 97 | (*slave)->B += (*slave)->A; 98 | } 99 | timerField.triggerDelay(); // start delay line to dactivate all step pins 100 | 101 | if (mode == Mode::target && (leadMotor->current == leadMotor->target)) // stop timer and call callback if we reached target 102 | { 103 | timerField.stepTimerStop(); 104 | if (callback != nullptr) 105 | callback(); 106 | } 107 | } 108 | 109 | template 110 | void MotorControlBase::pulseTimerISR() 111 | { 112 | Stepper **motor = motorList; 113 | while ((*motor) != nullptr) 114 | { 115 | (*motor++)->clearStepPin(); 116 | } 117 | } 118 | 119 | template 120 | void MotorControlBase::attachStepper(Stepper &stepper) 121 | { 122 | motorList[mCnt++] = &stepper; 123 | motorList[mCnt] = nullptr; 124 | mCnt = 0; 125 | } 126 | 127 | template 128 | template 129 | void MotorControlBase::attachStepper(Stepper &stepper, Steppers &... steppers) 130 | { 131 | static_assert(sizeof...(steppers) < MaxMotors, "Too many motors used. Please increase MaxMotors in file MotorControlBase.h"); 132 | 133 | motorList[this->mCnt++] = &stepper; 134 | attachStepper(steppers...); 135 | } 136 | 137 | template 138 | template 139 | void MotorControlBase::attachStepper(Stepper *(&motors)[N]) 140 | { 141 | static_assert(N <= MaxMotors, "Too many motors used. Please increase MaxMotors in file MotorControlBase.h"); 142 | 143 | for (size_t i = 0; i < N; i++) 144 | { 145 | this->motorList[i] = motors[i]; 146 | } 147 | this->motorList[N] = nullptr; 148 | } -------------------------------------------------------------------------------- /src/RotateControlBase.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "MotorControlBase.h" 4 | #include 5 | 6 | #ifndef ARDUINO_ARCH_ESP32 7 | #include "core_pins.h" 8 | #endif 9 | 10 | template 11 | class RotateControlBase : public MotorControlBase 12 | { 13 | public: 14 | RotateControlBase(unsigned pulseWidth = 5, unsigned accUpdatePeriod = 5000); 15 | 16 | // Non-blocking movements ---------------- 17 | template 18 | void rotateAsync(Steppers &... steppers); 19 | 20 | template 21 | void rotateAsync(Stepper *(&motors)[N]); 22 | 23 | void stopAsync(); 24 | 25 | void emergencyStop() { 26 | accelerator.eStop(); 27 | this->timerField.stepTimerStop(); 28 | } 29 | 30 | // Blocking movements -------------------- 31 | void stop(); 32 | 33 | void overrideSpeed(float speedFac); 34 | void overrideAcceleration(float accFac); 35 | 36 | protected: 37 | void doRotate(int N, float speedFactor = 1.0); 38 | void accTimerISR(); 39 | 40 | Accelerator accelerator; 41 | 42 | RotateControlBase(const RotateControlBase &) = delete; 43 | RotateControlBase &operator=(const RotateControlBase &) = delete; 44 | }; 45 | 46 | // Implementation ************************************************************************************************* 47 | 48 | template 49 | RotateControlBase::RotateControlBase(unsigned pulseWidth, unsigned accUpdatePeriod) 50 | : MotorControlBase(pulseWidth, accUpdatePeriod) 51 | { 52 | this->mode = MotorControlBase::Mode::notarget; 53 | } 54 | 55 | template 56 | void RotateControlBase::doRotate(int N, float speedFactor) 57 | { 58 | //Calculate Bresenham parameters ---------------------------------------------------------------- 59 | std::sort(this->motorList, this->motorList + N, Stepper::cmpVmax); 60 | this->leadMotor = this->motorList[0]; 61 | 62 | if (this->leadMotor->vMax == 0) 63 | return; 64 | 65 | this->leadMotor->currentSpeed = 0; 66 | 67 | this->leadMotor->A = std::abs(this->leadMotor->vMax); 68 | for (int i = 1; i < N; i++) 69 | { 70 | this->motorList[i]->A = std::abs(this->motorList[i]->vMax); 71 | this->motorList[i]->B = 2 * this->motorList[i]->A - this->leadMotor->A; 72 | } 73 | uint32_t acceleration = (*std::min_element(this->motorList, this->motorList + N, Stepper::cmpAcc))->a; // use the lowest acceleration for the move 74 | 75 | // Start moving--------------------------------------------------------------------------------------- 76 | accelerator.prepareRotation(this->leadMotor->current, this->leadMotor->vMax, acceleration, this->accUpdatePeriod, speedFactor); 77 | this->timerField.setStepFrequency(0); 78 | this->timerField.accTimerStart(); 79 | } 80 | 81 | // ISR ----------------------------------------------------------------------------------------------------------- 82 | 83 | template 84 | void RotateControlBase::accTimerISR() 85 | { 86 | int32_t newSpeed = accelerator.updateSpeed(this->leadMotor->current); // get new speed for the leading motor 87 | 88 | //Serial.printf("rc,curSpeed: %i newspd:%i\n",this->leadMotor->currentSpeed, newSpeed); 89 | 90 | if (this->leadMotor->currentSpeed == newSpeed) 91 | { 92 | return; // nothing changed, just keep running 93 | } 94 | 95 | int dir = newSpeed >= 0 ? 1 : -1; // direction changed? -> toggle direction of all motors 96 | if (dir != this->leadMotor->dir) 97 | { 98 | Stepper **motor = this->motorList; 99 | while ((*motor) != nullptr) 100 | { 101 | (*motor++)->toggleDir(); 102 | } 103 | delayMicroseconds(this->pulseWidth); 104 | } 105 | 106 | 107 | this->timerField.setStepFrequency(std::abs(newSpeed)); // speed changed, update timer 108 | this->leadMotor->currentSpeed = newSpeed; 109 | } 110 | 111 | // ROTATE Commands ------------------------------------------------------------------------------- 112 | 113 | template 114 | template 115 | void RotateControlBase::rotateAsync(Steppers &... steppers) 116 | { 117 | this->attachStepper(steppers...); 118 | doRotate(sizeof...(steppers)); 119 | } 120 | 121 | template 122 | template 123 | void RotateControlBase::rotateAsync(Stepper *(&steppers)[N]) 124 | { 125 | this->attachStepper(steppers); 126 | doRotate(N); 127 | } 128 | 129 | template 130 | void RotateControlBase::overrideSpeed(float factor) 131 | { 132 | accelerator.overrideSpeed(factor); 133 | } 134 | 135 | template 136 | void RotateControlBase::overrideAcceleration(float factor) 137 | { 138 | accelerator.overrideAcceleration(factor); 139 | } 140 | 141 | template 142 | void RotateControlBase::stopAsync() 143 | { 144 | accelerator.initiateStopping(this->leadMotor->current); 145 | } 146 | 147 | template 148 | void RotateControlBase::stop() 149 | { 150 | stopAsync(); 151 | while (this->isRunning()) 152 | { 153 | delay(1); 154 | } 155 | } -------------------------------------------------------------------------------- /src/StepControlBase.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "MotorControlBase.h" 4 | #include 5 | 6 | template 7 | class StepControlBase : public MotorControlBase 8 | { 9 | public: 10 | StepControlBase(unsigned pulseWidth = 5, unsigned accUpdatePeriod = 5000); 11 | 12 | // Non-blocking movements ---------------- 13 | template 14 | void moveAsync(Steppers &... steppers); 15 | 16 | template 17 | void moveAsync(Stepper *(&motors)[N]); 18 | void stopAsync(); 19 | 20 | // Blocking movements -------------------- 21 | template 22 | void move(Steppers &... steppers); 23 | 24 | template 25 | void move(Stepper *(&motors)[N]); 26 | void stop(); 27 | 28 | // Misc ---------------------------------- 29 | void setCallback(void (*_callback)()) { this->callback = _callback; } 30 | 31 | void accTimerISR(); 32 | protected: 33 | 34 | void doMove(int N, bool mode = true); 35 | 36 | Accelerator accelerator; 37 | 38 | StepControlBase(const StepControlBase &) = delete; 39 | StepControlBase &operator=(const StepControlBase &) = delete; 40 | }; 41 | 42 | // Implementation ************************************************************************************************* 43 | 44 | template 45 | StepControlBase::StepControlBase(unsigned pulseWidth, unsigned accUpdatePeriod) 46 | : MotorControlBase(pulseWidth, accUpdatePeriod) 47 | { 48 | this->mode = MotorControlBase::Mode::target; 49 | } 50 | 51 | template 52 | void StepControlBase::doMove(int N, bool move) 53 | { 54 | //Calculate Bresenham parameters ---------------------------------------------------------------- 55 | std::sort(this->motorList, this->motorList + N, Stepper::cmpDelta); // The motor which does most steps leads the movement, move to top of list 56 | this->leadMotor = this->motorList[0]; 57 | 58 | for (int i = 1; i < N; i++) 59 | { 60 | this->motorList[i]->B = 2 * this->motorList[i]->A - this->leadMotor->A; 61 | } 62 | 63 | // Calculate acceleration parameters -------------------------------------------------------------- 64 | uint32_t targetSpeed = std::abs((*std::min_element(this->motorList, this->motorList + N, Stepper::cmpVmin))->vMax); // use the lowest max frequency for the move, scale by relSpeed 65 | uint32_t acceleration = (*std::min_element(this->motorList, this->motorList + N, Stepper::cmpAcc))->a; // use the lowest acceleration for the move 66 | if (this->leadMotor->A == 0 || targetSpeed == 0) 67 | return; 68 | 69 | // Start move--------------------------------------------------------------------------------------- 70 | this->timerField.setStepFrequency(accelerator.prepareMovement(this->leadMotor->current, this->leadMotor->target, targetSpeed, acceleration)); 71 | this->timerField.stepTimerStart(); 72 | this->timerField.accTimerStart(); 73 | } 74 | 75 | // ISR ----------------------------------------------------------------------------------------------------------- 76 | 77 | template 78 | void StepControlBase::accTimerISR() 79 | { 80 | if (this->isRunning()) 81 | { 82 | this->timerField.setStepFrequency(accelerator.updateSpeed(this->leadMotor->current)); 83 | } 84 | } 85 | 86 | // MOVE ASYNC Commands ------------------------------------------------------------------------------------------------- 87 | 88 | template 89 | template 90 | void StepControlBase::moveAsync(Steppers &... steppers) 91 | { 92 | this->attachStepper(steppers...); 93 | doMove(sizeof...(steppers)); 94 | } 95 | 96 | template 97 | template 98 | void StepControlBase::moveAsync(Stepper *(&motors)[N]) //move up to maxMotors motors synchronously 99 | { 100 | this->attachStepper(motors); 101 | doMove(N); 102 | } 103 | 104 | // MOVE Commands ------------------------------------------------------------------------------------------------- 105 | 106 | template 107 | template 108 | void StepControlBase::move(Steppers &... steppers) 109 | { 110 | moveAsync(steppers...); 111 | while (this->timerField.stepTimerIsRunning()) 112 | { 113 | delay(1); 114 | } 115 | } 116 | 117 | template 118 | template 119 | void StepControlBase::move(Stepper *(&motors)[N]) 120 | { 121 | moveAsync(motors); 122 | while (this->isRunning()) 123 | { 124 | delay(1); 125 | } 126 | } 127 | 128 | template 129 | void StepControlBase::stopAsync() 130 | { 131 | uint32_t newTarget = accelerator.initiateStopping(this->leadMotor->current); 132 | this->leadMotor->target = this->leadMotor->current + this->leadMotor->dir * newTarget; 133 | } 134 | 135 | template 136 | void StepControlBase::stop() 137 | { 138 | stopAsync(); 139 | while (this->isRunning()) 140 | { 141 | delay(1); 142 | } 143 | } 144 | -------------------------------------------------------------------------------- /src/Stepper.cpp: -------------------------------------------------------------------------------- 1 | #include "Stepper.h" 2 | 3 | #ifdef ARDUINO_ARCH_ESP32 4 | #include "esp32-hal-gpio.h" 5 | #elif 6 | #include "core_pins.h" 7 | #endif 8 | 9 | Stepper::Stepper(const int _stepPin, const int _dirPin) 10 | : current(0), stepPin(_stepPin), dirPin(_dirPin) 11 | { 12 | pinMode(stepPin, OUTPUT); 13 | pinMode(dirPin, OUTPUT); 14 | 15 | setStepPinPolarity(HIGH); 16 | setInverseRotation(false); 17 | setAcceleration(aDefault); 18 | setMaxSpeed(vMaxDefault); 19 | } 20 | 21 | Stepper &Stepper::setStepPinPolarity(int polarity) 22 | { 23 | #ifdef ARDUINO_ARCH_ESP32 24 | this->polarity = polarity; 25 | #elif 26 | // Calculate adresses of bitbanded pin-set and pin-clear registers 27 | uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[stepPin].reg; //GPIO_PDOR 28 | uint32_t *pinSetReg = (uint32_t *)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 29 | uint32_t *pinClearReg = (uint32_t *)(pinRegAddr + 8 * 32); //GPIO_PCOR = GPIO_PDOR + 8 30 | // Assign registers according to step option 31 | if (polarity == LOW) 32 | { 33 | stepPinActiveReg = pinClearReg; 34 | stepPinInactiveReg = pinSetReg; 35 | } 36 | else 37 | { 38 | stepPinActiveReg = pinSetReg; 39 | stepPinInactiveReg = pinClearReg; 40 | } 41 | #endif 42 | clearStepPin(); // set step pin to inactive state 43 | return *this; 44 | } 45 | 46 | Stepper &Stepper::setInverseRotation(bool reverse) 47 | { 48 | #ifdef ARDUINO_ARCH_ESP32 49 | this->reverse = reverse; 50 | #elif 51 | // Calculate adresses of bitbanded pin-set and pin-clear registers 52 | uint32_t pinRegAddr = (uint32_t)digital_pin_to_info_PGM[dirPin].reg; //GPIO_PDOR 53 | uint32_t *pinSetReg = (uint32_t *)(pinRegAddr + 4 * 32); //GPIO_PSOR = GPIO_PDOR + 4 54 | uint32_t *pinClearReg = (uint32_t *)(pinRegAddr + 8 * 32); //GPIO_PCOR = GPIO_PDOR + 8 55 | 56 | if (reverse) 57 | { 58 | dirPinCwReg = pinClearReg; 59 | dirPinCcwReg = pinSetReg; 60 | } 61 | else 62 | { 63 | dirPinCwReg = pinSetReg; 64 | dirPinCcwReg = pinClearReg; 65 | } 66 | #endif 67 | return *this; 68 | } 69 | 70 | Stepper &Stepper::setAcceleration(uint32_t a) // steps/s^2 71 | { 72 | this->a = std::min(aMax, a); 73 | return *this; 74 | } 75 | 76 | Stepper &Stepper::setMaxSpeed(int32_t speed) 77 | { 78 | setDir(speed >= 0 ? 1 : -1); 79 | vMax = std::min(vMaxMax, std::max(-vMaxMax, speed)); 80 | return *this; 81 | } 82 | 83 | void Stepper::setTargetAbs(int32_t target) 84 | { 85 | setTargetRel(target - current); 86 | } 87 | 88 | void Stepper::setTargetRel(int32_t delta) 89 | { 90 | setDir(delta < 0 ? -1 : 1); 91 | target = current + delta; 92 | A = std::abs(delta); 93 | } -------------------------------------------------------------------------------- /src/Stepper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef ARDUINO_ARCH_ESP32 4 | #include "Arduino.h" 5 | #endif 6 | #include 7 | #include 8 | 9 | class Stepper 10 | { 11 | static constexpr int32_t vMaxMax = 300000; // largest speed possible (steps/s) 12 | static constexpr uint32_t aMax = 500000; // speed up to 500kHz within 1 s (steps/s^2) 13 | static constexpr uint32_t vMaxDefault = 800; // should work with every motor (1 rev/sec in 1/4-step mode) 14 | static constexpr uint32_t aDefault = 2500; // reasonably low (~0.5s for reaching the default speed) 15 | 16 | public: 17 | Stepper(const int StepPin, const int DirPin); 18 | 19 | Stepper &setMaxSpeed(int32_t speed); // steps/s 20 | Stepper &setAcceleration(uint32_t _a); // steps/s^2 21 | 22 | Stepper &setStepPinPolarity(int p); // HIGH -> positive pulses, LOW -> negative pulses 23 | Stepper &setInverseRotation(bool b); // Change polarity of the dir pulse 24 | 25 | void setTargetAbs(int32_t pos); // Set target position absolute 26 | void setTargetRel(int32_t delta); // Set target position relative to current position 27 | 28 | inline int32_t getPosition() const { return current; } 29 | inline void setPosition(int32_t pos) { current = pos; } 30 | int32_t dir; 31 | 32 | protected: 33 | inline void doStep(); 34 | inline void clearStepPin() const; 35 | 36 | inline void setDir(int d); 37 | inline void toggleDir(); 38 | 39 | volatile int32_t current; 40 | volatile int32_t currentSpeed; 41 | volatile int32_t target; 42 | 43 | int32_t A, B; // Bresenham paramters 44 | int32_t vMax; 45 | uint32_t a; 46 | 47 | // compare functions 48 | static bool cmpDelta(const Stepper *a, const Stepper *b) { return a->A > b->A; } 49 | static bool cmpAcc(const Stepper *a, const Stepper *b) { return a->a < b->a; } 50 | static bool cmpVmin(const Stepper *a, const Stepper *b) { return std::abs(a->vMax) < std::abs(b->vMax); } 51 | static bool cmpVmax(const Stepper *a, const Stepper *b) { return std::abs(a->vMax) > std::abs(b->vMax); } 52 | 53 | // Pin & Dir registers 54 | #ifdef ARDUINO_ARCH_ESP32 55 | volatile uint8_t polarity; 56 | volatile uint8_t reverse; 57 | #elif 58 | volatile uint32_t *stepPinActiveReg; 59 | volatile uint32_t *stepPinInactiveReg; 60 | volatile uint32_t *dirPinCwReg; 61 | volatile uint32_t *dirPinCcwReg; 62 | #endif 63 | const int stepPin, dirPin; 64 | 65 | // Friends 66 | template 67 | friend class StepControlBase; 68 | 69 | template 70 | friend class RotateControlBase; 71 | 72 | template 73 | friend class MotorControlBase; 74 | }; 75 | 76 | // Inline implementation ----------------------------------------- 77 | 78 | #ifdef ARDUINO_ARCH_ESP32 79 | void Stepper::doStep() 80 | { 81 | digitalWrite(stepPin, polarity); 82 | current += dir; 83 | } 84 | 85 | void Stepper::clearStepPin() const 86 | { 87 | digitalWrite(stepPin, !polarity); 88 | } 89 | 90 | void Stepper::setDir(int d) 91 | { 92 | dir = d; 93 | digitalWrite(dirPin, dir == 1 ? reverse : !reverse); 94 | } 95 | #elif 96 | void Stepper::doStep() 97 | { 98 | *stepPinActiveReg = 1; 99 | current += dir; 100 | } 101 | 102 | void Stepper::clearStepPin() const 103 | { 104 | *stepPinInactiveReg = 1; 105 | } 106 | 107 | void Stepper::setDir(int d) 108 | { 109 | dir = d; 110 | dir == 1 ? *dirPinCwReg = 1 : *dirPinCcwReg = 1; 111 | } 112 | #endif 113 | 114 | void Stepper::toggleDir() 115 | { 116 | setDir(-dir); 117 | } -------------------------------------------------------------------------------- /src/TeensyStep.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "version.h" 4 | 5 | #include "RotateControlBase.h" 6 | #include "StepControlBase.h" 7 | 8 | #include "accelerators/LinRotAccelerator.h" 9 | #include "accelerators/LinStepAccelerator.h" 10 | //#include "accelerators/SinRotAccelerator.h" 11 | 12 | //#include "timer/generic/TimerField.h" 13 | 14 | 15 | // TEENSY 3.0 - Teensy 3.6 ================================================================================== 16 | 17 | #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) 18 | #include "timer/teensy3/TimerField2.h" 19 | 20 | // TEENSY 4 ================================================================================================ 21 | 22 | #elif defined(__IMXRT1052__) 23 | #include "timer/teensy4/TimerField.h" 24 | 25 | // STM32 ==================================================================================================== 26 | 27 | #elif defined(__STM32_TBD__) 28 | #include "timer/stm32/TimerField.h" 29 | 30 | // ESP32 ====================================================================================== 31 | 32 | #elif defined(ARDUINO_ARCH_ESP32) 33 | #include "Arduino.h" 34 | #include "timer/esp32/TimerField.h" 35 | 36 | // Some other hardware ====================================================================================== 37 | 38 | #elif defined(__someHardware_TBD__) 39 | #include "timers/someHardware/TimerField2.h" 40 | using StepControlTick = StepControlBase; 41 | using RotateControlTick = RotateControlBase; 42 | #endif 43 | 44 | // Linear acceleration ----------------------------------------------------------------------------------------- 45 | 46 | 47 | using MotorControl = MotorControlBase; 48 | 49 | using RotateControl = RotateControlBase; 50 | using StepControl = StepControlBase; 51 | 52 | // Sine acceleration ------------------------------------------------------------------------------------------- 53 | 54 | // template 55 | // using RotateControlSin = RotateControlBase; 56 | 57 | //template 58 | //using StepControlSin = StepControlBase; 59 | 60 | // Generic ========================================================================================== 61 | 62 | // template 63 | // using RotateControl_tick = RotateControlBase; 64 | 65 | // template 66 | // using StepControl_tick = StepControlBase; 67 | -------------------------------------------------------------------------------- /src/accelerators/LinRotAccelerator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef ARDUINO_ARCH_ESP32 4 | #include "wiring.h" 5 | #endif 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | class LinRotAccelerator 12 | { 13 | public: 14 | inline void prepareRotation(int32_t currentPosition, int32_t targetSpeed, uint32_t acceleration, uint32_t accUpdatePeriod, float speedFactor = 1.0); 15 | inline int32_t updateSpeed(int32_t currentPosition); 16 | inline int32_t initiateStopping(int32_t currentPosition); 17 | inline void eStop(); 18 | inline void overrideSpeed(float factor); 19 | inline void overrideAcceleration(float factor); 20 | 21 | LinRotAccelerator() = default; 22 | 23 | protected: 24 | LinRotAccelerator(const LinRotAccelerator &) = delete; 25 | LinRotAccelerator &operator=(const LinRotAccelerator &) = delete; 26 | 27 | // int32_t v_tgt, v_cur; 28 | // int32_t v_tgt_orig, dv_orig, dv; 29 | 30 | float v_tgt, v_cur; 31 | float v_tgt_orig, dv_orig, dv_cur, dv; 32 | }; 33 | 34 | // Inline Implementation ===================================================================================================== 35 | 36 | void LinRotAccelerator::prepareRotation(int32_t currentPosition, int32_t targetSpeed, uint32_t a, uint32_t accUpdatePeriod, float speedFactor) 37 | { 38 | v_tgt_orig = targetSpeed; 39 | dv_orig = ((float)a * accUpdatePeriod) / 1E6; 40 | v_cur = 0; 41 | 42 | overrideAcceleration(1.0f); 43 | overrideSpeed(speedFactor); 44 | } 45 | 46 | void LinRotAccelerator::overrideSpeed(float factor) 47 | { 48 | //Serial.printf("a:------ %d\n", a); 49 | 50 | noInterrupts(); 51 | v_tgt = v_tgt_orig * factor; 52 | dv = v_tgt > v_cur ? dv_cur : -dv_cur; 53 | interrupts(); 54 | } 55 | 56 | void LinRotAccelerator::overrideAcceleration(float factor) 57 | { 58 | //Serial.printf("a:------ %d\n", a); 59 | if (factor > 0) 60 | { 61 | noInterrupts(); 62 | dv_cur = dv_orig * factor; 63 | dv *= factor; 64 | interrupts(); 65 | } 66 | } 67 | 68 | int32_t LinRotAccelerator::updateSpeed(int32_t curPos) 69 | { 70 | if (v_cur == v_tgt) 71 | return (int32_t)v_tgt; // already at target, keep spinning with target frequency 72 | 73 | v_cur += dv; 74 | v_cur = dv > 0.0f ? std::min(v_tgt, v_cur) : std::max(v_tgt, v_cur); 75 | 76 | return (int32_t)v_cur; 77 | } 78 | 79 | int32_t LinRotAccelerator::initiateStopping(int32_t curPos) 80 | { 81 | overrideSpeed(0); 82 | return 0; 83 | } 84 | 85 | void LinRotAccelerator::eStop() 86 | { 87 | noInterrupts(); 88 | v_cur = 0.0f; 89 | v_tgt = 0.0f; 90 | interrupts(); 91 | } 92 | -------------------------------------------------------------------------------- /src/accelerators/LinStepAccelerator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class LinStepAccelerator 8 | { 9 | public: 10 | inline int32_t prepareMovement(int32_t currentPos, int32_t targetPos, uint32_t targetSpeed, uint32_t a); 11 | inline int32_t updateSpeed(int32_t currentPosition); 12 | inline uint32_t initiateStopping(int32_t currentPosition); 13 | inline void overrideSpeed(float fac, int32_t currentPosition); 14 | 15 | LinStepAccelerator() = default; 16 | 17 | protected: 18 | LinStepAccelerator(const LinStepAccelerator &) = delete; 19 | LinStepAccelerator &operator=(const LinStepAccelerator &) = delete; 20 | 21 | int32_t s_0; 22 | uint32_t delta_tgt; 23 | uint32_t accLength, decStart; 24 | uint32_t two_a; 25 | uint32_t v_tgt, v_min2; 26 | }; 27 | 28 | // Inline Implementation ===================================================================================================== 29 | 30 | int32_t LinStepAccelerator::prepareMovement(int32_t currentPos, int32_t targetPos, uint32_t targetSpeed, uint32_t a) 31 | { 32 | s_0 = currentPos; 33 | delta_tgt = std::abs(targetPos - currentPos); 34 | v_tgt = targetSpeed; 35 | two_a = 2 * a; 36 | v_min2 = a; 37 | 38 | uint32_t ae = (float)v_tgt * v_tgt / two_a - 0.5f; // length of acceleration phase (we use a float here to avoid overflow in v_tgt^2). Use (1) and vmin^2 = 2a 39 | accLength = std::min(ae, delta_tgt / 2); // limit acceleration phase to half of total steps 40 | decStart = delta_tgt - accLength; 41 | 42 | return accLength == 0 ? v_tgt : (int32_t)sqrtf(v_min2); 43 | } 44 | 45 | int32_t LinStepAccelerator::updateSpeed(int32_t curPos) 46 | { 47 | uint32_t stepsDone = std::abs(s_0 - curPos); 48 | 49 | // acceleration phase ------------------------------------- 50 | if (stepsDone < accLength) 51 | return sqrt(two_a * stepsDone + v_min2); 52 | 53 | // constant speed phase ------------------------------------ 54 | if (stepsDone < decStart) 55 | return v_tgt; 56 | 57 | //deceleration phase -------------------------------------- 58 | if(stepsDone < delta_tgt) 59 | return sqrt(two_a * ((stepsDone < delta_tgt - 1) ? delta_tgt - stepsDone - 2 : 0) + v_min2); 60 | 61 | //we are done, make sure to return 0 to stop the step timer 62 | return 0; 63 | } 64 | 65 | uint32_t LinStepAccelerator::initiateStopping(int32_t curPos) 66 | { 67 | uint32_t stepsDone = std::abs(s_0 - curPos); 68 | 69 | if (stepsDone < accLength) // still accelerating 70 | { 71 | accLength = decStart = 0; // start deceleration 72 | delta_tgt = 2 * stepsDone; // we need the same way to decelerate as we traveled so far 73 | return stepsDone; // return steps to go 74 | } 75 | else if (stepsDone < decStart) // constant speed phase 76 | { 77 | decStart = 0; // start deceleration 78 | delta_tgt = stepsDone + accLength; // normal deceleration distance 79 | return accLength; // return steps to go 80 | } 81 | else // already decelerating 82 | { 83 | return delta_tgt - stepsDone; // return steps to go 84 | } 85 | } -------------------------------------------------------------------------------- /src/accelerators/SinRotAccelerator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "wiring.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | class SinRotAccelerator 10 | { 11 | public: 12 | inline int32_t prepareRotation(int32_t currentPosition, int32_t targetSpeed, uint32_t acceleration, float speedFactor = 1.0); 13 | inline int32_t updateSpeed(int32_t currentPosition); 14 | inline int32_t initiateStopping(int32_t currentPosition); 15 | inline void overrideSpeed(float fac, int32_t currentPosition); 16 | 17 | SinRotAccelerator() = default; 18 | 19 | protected: 20 | SinRotAccelerator(const SinRotAccelerator &) = delete; 21 | SinRotAccelerator &operator=(const SinRotAccelerator &) = delete; 22 | 23 | int32_t dir; 24 | int32_t a, two_a; 25 | int32_t vstp; 26 | int32_t v_tgt, v_tgt_orig, vstp_tgt; 27 | int32_t v_min, v_min_sqr; 28 | int32_t s_0; 29 | 30 | inline float signed_sqrt(int32_t x) // signed square root 31 | { 32 | return x > 0 ? sqrt(x) : -sqrt(-x); 33 | } 34 | }; 35 | 36 | // Implementation ===================================================================================================== 37 | 38 | int32_t SinRotAccelerator::prepareRotation(int32_t currentPosition, int32_t targetSpeed, uint32_t acceleration, float speedFactor) 39 | { 40 | v_tgt_orig = targetSpeed; 41 | a = acceleration; 42 | two_a = 2 * a; 43 | v_min_sqr = a; 44 | v_min = sqrtf(v_min_sqr); 45 | vstp = 0; 46 | overrideSpeed(speedFactor, currentPosition); 47 | 48 | //Serial.printf("%vtgt:%i vstp_tgt:%i \n", v_tgt, vstp_tgt); 49 | return v_min; 50 | } 51 | 52 | int32_t SinRotAccelerator::updateSpeed(int32_t curPos) 53 | { 54 | if (vstp == vstp_tgt) // already at target, keep spinning with target frequency 55 | { 56 | return v_tgt; 57 | } 58 | 59 | vstp += std::abs(curPos - s_0) * dir; 60 | vstp = dir == 1 ? std::min(vstp_tgt, vstp) : std::max(vstp_tgt, vstp); // clamp vstp to target 61 | 62 | //Serial.printf("dir: %i, vstp_tgt:%i, vstp:%i, deltaS:%i\n", dir, vstp_tgt, vstp, deltaS); 63 | s_0 = curPos; 64 | return signed_sqrt(two_a * vstp + v_min_sqr); 65 | } 66 | 67 | void SinRotAccelerator::overrideSpeed(float fac, int32_t curPos) 68 | { 69 | noInterrupts(); 70 | s_0 = curPos; 71 | v_tgt = std::round(v_tgt_orig * fac); 72 | vstp_tgt = ((float)v_tgt * v_tgt) / two_a * (v_tgt > 0 ? 1.0f : -1.0f); 73 | dir = vstp_tgt > vstp ? 1 : -1; 74 | interrupts(); 75 | } 76 | 77 | int32_t SinRotAccelerator::initiateStopping(int32_t curPos) 78 | { 79 | overrideSpeed(0, curPos); 80 | return 0; 81 | } -------------------------------------------------------------------------------- /src/timer/TF_Handler.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | struct TF_Handler 4 | { 5 | virtual void stepTimerISR() = 0; 6 | virtual void accTimerISR() = 0; 7 | virtual void pulseTimerISR() = 0; 8 | }; -------------------------------------------------------------------------------- /src/timer/TimerFieldBase.h: -------------------------------------------------------------------------------- 1 | #include "TF_Handler.h" 2 | 3 | class ITimerField 4 | { 5 | public: 6 | ITimerField(TF_Handler *); 7 | 8 | virtual bool begin()=0; 9 | virtual void end()=0; 10 | 11 | virtual void stepTimerStart()=0; 12 | virtual void stepTimerStop()=0; 13 | virtual bool stepTimerIsRunning() const =0; 14 | virtual void setStepFrequency(unsigned f)=0; 15 | 16 | virtual void accTimerStart()=0; 17 | virtual void accTimerStop()=0; 18 | virtual void setAccUpdatePeriod(unsigned period)=0; 19 | 20 | virtual void setPulseWidth(unsigned delay)=0; 21 | virtual void triggerDelay()=0; 22 | }; -------------------------------------------------------------------------------- /src/timer/esp32/TimerField.cpp: -------------------------------------------------------------------------------- 1 | #include "TimerField.h" 2 | 3 | TF_Handler* TimerField::handler; 4 | portMUX_TYPE TimerField::timerMux = portMUX_INITIALIZER_UNLOCKED; 5 | -------------------------------------------------------------------------------- /src/timer/esp32/TimerField.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Arduino.h" 4 | #include 5 | 6 | #include "../TF_Handler.h" 7 | 8 | // //========================= 9 | // // ESP 32 10 | // //========================= 11 | 12 | class TimerField 13 | { 14 | public: 15 | inline TimerField(TF_Handler *_handler); 16 | 17 | inline bool begin(); 18 | inline void end(); 19 | 20 | inline void stepTimerStart(); 21 | inline void stepTimerStop(); 22 | inline void setStepFrequency(unsigned f); 23 | inline unsigned getStepFrequency() { return timerAlarmRead(stepTimer); } 24 | inline bool stepTimerIsRunning() const { return stepTimerRunning; } 25 | 26 | inline void accTimerStart() { timerAlarmEnable(accTimer); } 27 | inline void accTimerStop() { timerAlarmDisable(accTimer); } 28 | inline void setAccUpdatePeriod(unsigned period) { timerAlarmWrite(accTimer, period, true); } 29 | 30 | inline void triggerDelay() { timerAlarmEnable(pulseTimer); } 31 | inline void setPulseWidth(unsigned pulseWidth) { timerAlarmWrite(pulseTimer, pulseWidth, false); } 32 | 33 | static portMUX_TYPE timerMux; 34 | 35 | protected: 36 | static TF_Handler *handler; 37 | hw_timer_t *stepTimer; 38 | hw_timer_t *accTimer; 39 | hw_timer_t *pulseTimer; 40 | bool stepTimerRunning; 41 | unsigned lastF; 42 | }; 43 | 44 | // IMPLEMENTATION ==================================================================== 45 | 46 | TimerField::TimerField(TF_Handler *_handler) 47 | : stepTimer(timerBegin(0, 80, true)), 48 | accTimer(timerBegin(1, 80, true)), 49 | pulseTimer(timerBegin(2, 80, true)), 50 | stepTimerRunning(false), 51 | lastF(0) 52 | { 53 | handler = _handler; 54 | timerAttachInterrupt(stepTimer, [] { handler->stepTimerISR(); }, true); 55 | timerAlarmWrite(stepTimer, 1, true); 56 | timerAttachInterrupt(accTimer, [] { handler->accTimerISR(); }, true); 57 | timerAlarmWrite(accTimer, 1, true); 58 | timerAttachInterrupt(pulseTimer, [] { handler->pulseTimerISR(); }, true); 59 | timerAlarmWrite(pulseTimer, 1, false); 60 | } 61 | 62 | void TimerField::stepTimerStart() 63 | { 64 | timerAlarmEnable(stepTimer); 65 | stepTimerRunning = true; 66 | } 67 | 68 | void TimerField::stepTimerStop() 69 | { 70 | timerAlarmDisable(stepTimer); 71 | stepTimerRunning = false; 72 | } 73 | 74 | void TimerField::setStepFrequency(unsigned f) 75 | { 76 | if (f != 0) 77 | timerAlarmWrite(stepTimer, 1000000 / f, true); // Timer runs at 1 microsecond interval 78 | else 79 | stepTimerStop(); 80 | } 81 | 82 | bool TimerField::begin() 83 | { 84 | Serial.println("begin"); 85 | return true; 86 | } 87 | 88 | void TimerField::end() 89 | { 90 | timerAlarmDisable(stepTimer); 91 | timerAlarmDisable(accTimer); 92 | timerAlarmDisable(pulseTimer); 93 | } -------------------------------------------------------------------------------- /src/timer/generic/TickTimer.cpp: -------------------------------------------------------------------------------- 1 | #ifndef ARDUINO_ARCH_ESP32 2 | #include "TickTimer.h" 3 | 4 | void std::__throw_bad_function_call() 5 | { 6 | while(1); 7 | } 8 | 9 | TimerBase* TimerControl::firstTimer = nullptr; 10 | TimerBase* TimerControl::lastTimer = nullptr; 11 | #endif 12 | -------------------------------------------------------------------------------- /src/timer/generic/TickTimer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Arduino.h" 4 | #include 5 | #include 6 | 7 | class TimerControl; 8 | 9 | //using callback_t = void (*)(); 10 | using callback_t = std::function; 11 | 12 | class TimerBase 13 | { 14 | public: 15 | TimerBase(callback_t cb, bool _isPeriodic) 16 | { 17 | callback = cb; 18 | deltaCnt = 0; 19 | run = false; 20 | isPeriodic = _isPeriodic; 21 | prev = next = nullptr; 22 | }; 23 | 24 | inline void start() { startCnt = ARM_DWT_CYCCNT; run = true; } 25 | inline void stop() { run = false; } 26 | inline bool isRunning() const {return run;} 27 | 28 | protected: 29 | uint32_t deltaCnt, startCnt; 30 | bool isPeriodic; 31 | bool run; 32 | callback_t callback; 33 | 34 | TimerBase *prev, *next; 35 | 36 | friend TimerControl; 37 | }; 38 | 39 | class PeriodicTimer : public TimerBase 40 | { 41 | public: 42 | PeriodicTimer(callback_t cb) : TimerBase(cb, true) 43 | { 44 | 45 | } 46 | 47 | inline void setFrequency(float hz) 48 | { 49 | //Serial.println(hz); 50 | deltaCnt = hz > minFrequency ? F_CPU / hz : std::numeric_limits::max(); 51 | //Serial.println(deltaCnt); 52 | } 53 | 54 | inline void setPeriod(uint32_t microSeconds) 55 | { 56 | deltaCnt = F_CPU / 1000000 * microSeconds; 57 | } 58 | 59 | static constexpr float minFrequency = (float)F_CPU / std::numeric_limits::max(); 60 | }; 61 | 62 | class OneShotTimer : public TimerBase 63 | { 64 | public: 65 | OneShotTimer(callback_t cb, unsigned delay = 0) : TimerBase(cb, false) 66 | { 67 | setDelay(delay); 68 | } 69 | 70 | void setDelay(unsigned microSeconds) 71 | { 72 | deltaCnt = F_CPU / 1000000 * microSeconds; 73 | } 74 | }; 75 | 76 | class TimerControl 77 | { 78 | public: 79 | static void begin() 80 | { 81 | ARM_DEMCR |= ARM_DEMCR_TRCENA; 82 | ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA; 83 | } 84 | 85 | static void attachTimer(TimerBase *timer) 86 | { 87 | if (timer == nullptr) 88 | return; 89 | 90 | if (firstTimer == nullptr) // empty timer list 91 | { 92 | firstTimer = lastTimer = timer; 93 | timer->prev = nullptr; 94 | timer->next = nullptr; 95 | } 96 | else // append timer at end of the list 97 | { 98 | timer->next = nullptr; 99 | lastTimer->next = timer; 100 | lastTimer = timer; 101 | } 102 | } 103 | 104 | static void detachTimer(TimerBase *timer) 105 | { 106 | if (firstTimer == nullptr || timer == nullptr) 107 | return; 108 | 109 | TimerBase *t = firstTimer; 110 | while (t != timer) 111 | { 112 | t = t->next; 113 | if (t == nullptr) 114 | return; 115 | } 116 | 117 | if (t == firstTimer) 118 | { 119 | firstTimer = t->next; 120 | } 121 | else if (t == lastTimer) 122 | { 123 | lastTimer = t->prev; 124 | } 125 | else 126 | { 127 | t->prev->next = t->next; 128 | } 129 | } 130 | 131 | static inline void tick() 132 | { 133 | digitalWriteFast(2,HIGH) ; 134 | TimerBase *timer = firstTimer; 135 | 136 | while (timer != nullptr) 137 | { 138 | //Serial.printf("cnt: %d\n", ARM_DWT_CYCCNT ); 139 | 140 | if (timer->run && (ARM_DWT_CYCCNT - timer->startCnt >= timer->deltaCnt)) 141 | 142 | { 143 | if (timer->isPeriodic) 144 | timer->startCnt = ARM_DWT_CYCCNT; 145 | else 146 | timer->run = false; 147 | 148 | timer->callback(); 149 | } 150 | timer = timer->next; 151 | } 152 | 153 | digitalWriteFast(2,LOW) ; 154 | } 155 | 156 | protected: 157 | static TimerBase *firstTimer; 158 | static TimerBase *lastTimer; 159 | }; 160 | -------------------------------------------------------------------------------- /src/timer/generic/TimerField.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef ARDUINO_ARCH_ESP32 4 | #include "wiring.h" 5 | #endif 6 | // //#include "imxrt.h" 7 | 8 | #include "TickTimer.h" 9 | #include "../TF_Handler.h" 10 | 11 | // //========================= 12 | // // Teensy 4 13 | // //========================= 14 | 15 | class TickTimerField 16 | { 17 | public: 18 | inline TickTimerField(TF_Handler *); 19 | 20 | inline bool begin(); 21 | inline void end(); 22 | 23 | inline void stepTimerStart() { stepTimer.start(); } 24 | inline void stepTimerStop() { stepTimer.stop(); } 25 | inline bool stepTimerIsRunning() const { return stepTimer.isRunning(); } 26 | inline void setStepFrequency(unsigned f) {stepTimer.setFrequency(f); } 27 | inline unsigned getStepFrequency() { return 0; } 28 | 29 | inline void accTimerStart() { accTimer.start(); } 30 | inline void accTimerStop() { accTimer.stop(); } 31 | inline void setAccUpdatePeriod(unsigned period) { accTimer.setPeriod(period); } 32 | 33 | inline void setPulseWidth(unsigned delay) { delayTimer.setDelay(delay); } 34 | inline void triggerDelay() { delayTimer.start(); } 35 | 36 | protected: 37 | TF_Handler *handler; 38 | 39 | PeriodicTimer stepTimer, accTimer; 40 | OneShotTimer delayTimer; 41 | }; 42 | 43 | // IMPLEMENTATION ==================================================================== 44 | 45 | TickTimerField::TickTimerField(TF_Handler *_handler) 46 | : handler(_handler), 47 | stepTimer([this] { handler->stepTimerISR(); }), 48 | //stepTimer(test), 49 | accTimer([_handler] { _handler->accTimerISR(); }), 50 | delayTimer([this] { handler->pulseTimerISR(); }) 51 | { 52 | TimerControl::attachTimer(&stepTimer); 53 | TimerControl::attachTimer(&accTimer); 54 | TimerControl::attachTimer(&delayTimer); 55 | } 56 | 57 | 58 | 59 | bool TickTimerField::begin() 60 | { 61 | Serial.println("begin"); 62 | TimerControl::begin(); 63 | return true; 64 | } 65 | 66 | void TickTimerField::end() 67 | { 68 | //TimerControl::end(); 69 | } -------------------------------------------------------------------------------- /src/timer/teensy3/PIT.cpp: -------------------------------------------------------------------------------- 1 | #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) 2 | 3 | #include "PIT.h" 4 | 5 | #include "../TF_Handler.h" 6 | 7 | namespace 8 | { 9 | TF_Handler* Handler[4]; 10 | 11 | template 12 | void dispatchFunc() 13 | { 14 | Handler[n]->stepTimerISR(); 15 | } 16 | 17 | constexpr void(*dispatcher[])(void) = 18 | { 19 | dispatchFunc<0>, 20 | dispatchFunc<1>, 21 | dispatchFunc<2>, 22 | dispatchFunc<3> 23 | }; 24 | 25 | void dummyISR(void) {} 26 | } 27 | 28 | bool PIT::begin(TF_Handler* handler) 29 | { 30 | if (!timer.begin(dummyISR, 1E6)) return false; // try to reserve a timer 31 | 32 | setupChannel(); // find pit channel of reserved timer 33 | const int channelNr = channel - KINETISK_PIT_CHANNELS; 34 | Handler[channelNr] = handler; // store handler 35 | timer.priority(32); 36 | timer.begin(dispatcher[channelNr], 1E6); // attach an ISR which will call the stored handler 37 | stop(); // stop doesn't clear TEN, we want to keep the IntervalTimer reserved 38 | 39 | return true; 40 | } 41 | 42 | void PIT::setupChannel() 43 | { 44 | IRQ_NUMBER_t number = (IRQ_NUMBER_t)timer; 45 | switch (number) 46 | { 47 | case IRQ_PIT_CH0: 48 | channel = KINETISK_PIT_CHANNELS + 0; 49 | break; 50 | case IRQ_PIT_CH1: 51 | channel = KINETISK_PIT_CHANNELS + 1; 52 | break; 53 | case IRQ_PIT_CH2: 54 | channel = KINETISK_PIT_CHANNELS + 2; 55 | break; 56 | case IRQ_PIT_CH3: 57 | channel = KINETISK_PIT_CHANNELS + 3; 58 | break; 59 | default: 60 | channel = nullptr; 61 | break; 62 | } 63 | } 64 | #endif 65 | -------------------------------------------------------------------------------- /src/timer/teensy3/PIT.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "IntervalTimer.h" 4 | 5 | class IPitHandler 6 | { 7 | public: 8 | virtual void pitISR() = 0; 9 | }; 10 | 11 | class TF_Handler; 12 | 13 | class PIT 14 | { 15 | public: 16 | bool begin(TF_Handler*); 17 | 18 | inline void end() { timer.end(); } 19 | inline void start() const { channel->TFLG = 1; channel->TCTRL = 0; channel->TCTRL = 3; } 20 | inline void stop() const { channel->TCTRL &= ~PIT_TCTRL_TIE; } 21 | inline void enableInterupt() const { channel->TFLG = 1; channel->TCTRL |= PIT_TCTRL_TIE; } 22 | inline void setFrequency(uint32_t val) const { channel->LDVAL = F_BUS/val;} 23 | inline void setThisReload(uint32_t ldval) const { channel->TCTRL = 0, channel->TFLG = 1;channel->LDVAL = ldval, channel->TCTRL = 3;} 24 | inline void setNextReload(uint32_t ldval) const { channel->LDVAL = ldval; } 25 | inline uint32_t getLDVAL() const { return channel->LDVAL; } 26 | inline uint32_t getCVAL() const { return channel->CVAL; } 27 | inline void clearTIF()const { channel->TFLG = 1; } 28 | inline bool isRunning() const { return channel->TCTRL & PIT_TCTRL_TIE; } 29 | 30 | KINETISK_PIT_CHANNEL_t* channel = nullptr; 31 | 32 | protected: 33 | IntervalTimer timer; 34 | void setupChannel(); 35 | 36 | }; 37 | -------------------------------------------------------------------------------- /src/timer/teensy3/TeensyStepFTM.cpp: -------------------------------------------------------------------------------- 1 | #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) 2 | 3 | #include "TeensyStepFTM.h" 4 | #include "config.h" 5 | 6 | namespace TeensyStepFTM 7 | { 8 | 9 | namespace // private 10 | { 11 | bool isConfigured = false; 12 | IDelayHandler *callbacks[maxChannel]; 13 | } 14 | 15 | void begin() 16 | { 17 | if (!isConfigured) 18 | { 19 | if (USE_TIMER == TIMER_TPM1) 20 | { // enable clocks for tpm timers (ftm clocks are enabled by teensyduino) 21 | SIM_SCGC2 |= SIM_SCGC2_TPM1; 22 | SIM_SOPT2 |= SIM_SOPT2_TPMSRC(2); 23 | } 24 | else if (USE_TIMER == TIMER_TPM2) 25 | { 26 | SIM_SCGC2 |= SIM_SCGC2_TPM2; 27 | SIM_SOPT2 |= SIM_SOPT2_TPMSRC(2); 28 | } 29 | // Default Mode for FTM is (nearly) TPM compatibile 30 | timer->SC = FTM_SC_CLKS(0b00); // Disable clock 31 | timer->MOD = 0xFFFF; // Set full counter range 32 | 33 | for (unsigned i = 0; i < maxChannel; i++) 34 | { // turn off all channels which were enabled by teensyduino for PWM generation 35 | if (isFTM) 36 | { // compiletime constant, compiler optimizes conditional and not valid branch completely away 37 | timer->CH[i].SC &= ~FTM_CSC_CHF; // FTM requires to clear flag by setting bit to 0 38 | } 39 | else 40 | { 41 | timer->CH[i].SC |= FTM_CSC_CHF; // TPM requires to clear flag by setting bit to 1 42 | } 43 | timer->CH[i].SC &= ~FTM_CSC_CHIE; // Disable channel interupt 44 | timer->CH[i].SC = FTM_CSC_MSA; 45 | //timer->CH[i].SC = 0; // disable channel 46 | } 47 | timer->SC = FTM_SC_CLKS(0b01) | FTM_SC_PS(prescale); // Start clock 48 | NVIC_ENABLE_IRQ(irq); // Enable interrupt request for selected timer 49 | 50 | isConfigured = true; 51 | } 52 | } 53 | 54 | unsigned addDelayChannel(IDelayHandler *handler) 55 | { 56 | for (unsigned ch = 0; ch < maxChannel; ch++) 57 | { 58 | if (callbacks[ch] == 0) 59 | { 60 | callbacks[ch] = handler; //Just store the callback function, the rest is done in Trigger function 61 | return ch; 62 | } 63 | } 64 | return maxChannel; 65 | } 66 | 67 | void removeDelayChannel(unsigned chNr) 68 | { 69 | if (chNr < maxChannel) 70 | { 71 | noInterrupts(); 72 | timer->CH[chNr].SC &= ~FTM_CSC_CHIE; // disable channel 73 | callbacks[chNr] = nullptr; 74 | interrupts(); 75 | } 76 | } 77 | } // namespace TeensyDelay2 78 | 79 | //------------------------------------------------------------------------------------------- 80 | // Interupt service routine of the timer selected in config.h. 81 | // The code doesn't touch the other FTM/TPM ISRs so they can still be used for other purposes 82 | // 83 | // Unfortunately we can not inline the ISR because inlinig will generate a "weak" function?. 84 | // Since the original ISR (dummy_isr) is also defined weak the linker 85 | // is allowed to choose any of them. In this case it desided to use dummy_isr :-( 86 | // Using a "strong" (not inlined) function overrides the week dummy_isr 87 | //-------------------------------------------------------------------------------------------- 88 | 89 | using namespace TeensyStepFTM; 90 | 91 | #if USE_TIMER == TIMER_FTM0 92 | void ftm0_isr(void) 93 | #elif USE_TIMER == TIMER_FTM1 94 | void ftm1_isr(void) 95 | #elif USE_TIMER == TIMER_FTM2 96 | void ftm2_isr(void) 97 | #elif USE_TIMER == TIMER_FTM3 98 | void ftm3_isr(void) 99 | #elif USE_TIMER == TIMER_TPM1 100 | void tpm1_isr(void) 101 | #elif USE_TIMER == TIMER_TPM2 102 | void tpm2_isr(void) 103 | #endif 104 | { 105 | for (unsigned i = 0; i < maxChannel; i++) 106 | { 107 | if ((timer->CH[i].SC & (FTM_CSC_CHIE | FTM_CSC_CHF)) == (FTM_CSC_CHIE | FTM_CSC_CHF)) // only handle if channel is active (CHIE set) and overflowed (CHF set) 108 | { 109 | timer->CH[i].SC &= ~FTM_CSC_CHIE; // We want one shot only. (Make sure to reset the CHF flag before re-activating interrupt in trigger function) 110 | callbacks[i]->delayISR(i); // invoke callback function for the channel 111 | } 112 | } 113 | } 114 | #endif -------------------------------------------------------------------------------- /src/timer/teensy3/TeensyStepFTM.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "config.h" 3 | 4 | #include "Arduino.h" 5 | 6 | class IDelayHandler 7 | { 8 | public: 9 | virtual void delayISR(unsigned channel) = 0; 10 | }; 11 | 12 | namespace TeensyStepFTM 13 | { 14 | extern void begin(void); 15 | 16 | extern unsigned addDelayChannel(IDelayHandler* h); 17 | extern void removeDelayChannel(unsigned channel); 18 | 19 | inline void trigger(unsigned delay, unsigned channel) 20 | { 21 | //Serial.printf("chn: %d, tdt: %d\n", channel, microsToReload(delay)); 22 | 23 | timer->SC = 0; // Remove clock to immediately write new channel value 24 | timer->CH[channel].CV = timer->CNT + delay; // Update channel value 25 | timer->SC = FTM_SC_CLKS(0b01) | FTM_SC_PS(prescale); // Reattach clock 26 | 27 | if (isFTM) { // important to clear channel flag (in case we had a channel event since last trigger) 28 | timer->CH[channel].SC &= ~FTM_CSC_CHF; // enable channel interrupt. Can not combine both commands since clearing 29 | } 30 | else { // the channel flag requires a read modify write cycle 31 | timer->CH[channel].SC |= FTM_CSC_CHF; // FTM and TPM timers differ in resetting the interrupt flag 32 | } // isFTM is a compile time constant -> compiler completely optimizes the not valid case away 33 | timer->CH[channel].SC = FTM_CSC_MSA | FTM_CSC_CHIE; 34 | } 35 | } -------------------------------------------------------------------------------- /src/timer/teensy3/TimerField2.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "wiring.h" 4 | #include "PIT.h" 5 | #include "TeensyStepFTM.h" 6 | 7 | #include "../TF_Handler.h" 8 | //#include "../TimerFieldBase.h" 9 | 10 | //========================= 11 | // Teensy3 12 | //========================= 13 | 14 | class TimerField : public IDelayHandler 15 | { 16 | public: 17 | inline TimerField(TF_Handler *); 18 | inline ~TimerField(); 19 | 20 | inline bool begin(); 21 | inline void end(); 22 | 23 | inline void stepTimerStart(); 24 | inline void stepTimerStop(); 25 | inline bool stepTimerIsRunning() const; 26 | inline void setStepFrequency(unsigned f); 27 | inline unsigned getStepFrequency(); 28 | 29 | inline void accTimerStart(); 30 | inline void accTimerStop(); 31 | inline void setAccUpdatePeriod(unsigned period); 32 | 33 | inline void setPulseWidth(unsigned delay); 34 | inline void triggerDelay(); 35 | inline void delayISR(unsigned channel); 36 | 37 | //protected: 38 | PIT stepTimer; 39 | TF_Handler *handler; 40 | 41 | unsigned delayWidth; 42 | unsigned accUpdatePeriod; 43 | 44 | const unsigned accLoopDelayChannel; 45 | const unsigned pinResetDelayChannel; 46 | }; 47 | 48 | // IMPLEMENTATION ==================================================================== 49 | 50 | TimerField::TimerField(TF_Handler *_handler) 51 | : handler(_handler), 52 | accLoopDelayChannel(TeensyStepFTM::addDelayChannel(this)), 53 | pinResetDelayChannel(TeensyStepFTM::addDelayChannel(this)) 54 | { 55 | //Serial.println(accLoopDelayChannel); 56 | } 57 | 58 | TimerField::~TimerField() 59 | { 60 | end(); 61 | } 62 | 63 | bool TimerField::begin() 64 | { 65 | TeensyStepFTM::begin(); 66 | return stepTimer.begin(handler); 67 | } 68 | 69 | void TimerField::end() 70 | { 71 | stepTimer.end(); 72 | TeensyStepFTM::removeDelayChannel(accLoopDelayChannel); 73 | TeensyStepFTM::removeDelayChannel(pinResetDelayChannel); 74 | } 75 | 76 | // Step Timer ------------------------------------------------------ 77 | 78 | void TimerField::stepTimerStart() 79 | { 80 | stepTimer.start(); 81 | } 82 | 83 | void TimerField::stepTimerStop() 84 | { 85 | stepTimer.stop(); 86 | } 87 | 88 | unsigned TimerField::getStepFrequency() 89 | { 90 | return F_BUS / stepTimer.getLDVAL(); 91 | } 92 | 93 | void TimerField::setStepFrequency(unsigned f) 94 | { 95 | if (f != 0) 96 | { 97 | if (stepTimer.isRunning()) 98 | { 99 | uint32_t ldval = stepTimer.getLDVAL(); 100 | 101 | if (ldval < F_BUS / 250 ) // normal step freqency (> 250 Hz) -> set new period for following periods 102 | { 103 | stepTimer.setNextReload(F_BUS / f); 104 | return; 105 | } 106 | 107 | uint32_t newReload = F_BUS / f; 108 | uint32_t cyclesSinceLastStep = ldval - stepTimer.channel->CVAL; 109 | if (cyclesSinceLastStep <= newReload) // time between last pulse and now less than required new period -> wait 110 | { 111 | stepTimer.setThisReload(newReload - cyclesSinceLastStep); 112 | stepTimer.setNextReload(newReload); 113 | } 114 | else 115 | { 116 | stepTimer.setThisReload(newReload); 117 | handler->stepTimerISR(); 118 | } 119 | } 120 | else // not running 121 | { 122 | handler->stepTimerISR(); 123 | stepTimer.setThisReload(F_BUS / f); // restarts implicitly 124 | } 125 | } 126 | else //f==0 127 | { 128 | stepTimer.stop(); 129 | } 130 | } 131 | 132 | bool TimerField::stepTimerIsRunning() const 133 | { 134 | return stepTimer.isRunning(); 135 | } 136 | 137 | // Acceleration Timer ------------------------------------------------------ 138 | 139 | void TimerField::accTimerStart() 140 | { 141 | delayISR(accLoopDelayChannel); 142 | } 143 | 144 | void TimerField::setAccUpdatePeriod(unsigned p) 145 | { 146 | accUpdatePeriod = TeensyStepFTM::microsToReload(p); 147 | } 148 | 149 | void TimerField::accTimerStop() 150 | { 151 | accUpdatePeriod = 0; 152 | } 153 | 154 | // Delay Timer ------------------------------------------------------ 155 | 156 | void TimerField::setPulseWidth(unsigned delay) 157 | { 158 | delayWidth = TeensyStepFTM::microsToReload(delay); 159 | } 160 | 161 | void TimerField::triggerDelay() 162 | { 163 | TeensyStepFTM::trigger(delayWidth, pinResetDelayChannel); 164 | } 165 | 166 | void TimerField::delayISR(unsigned channel) 167 | { 168 | if (channel == pinResetDelayChannel) 169 | { 170 | handler->pulseTimerISR(); 171 | } 172 | 173 | else if (channel == accLoopDelayChannel) 174 | { 175 | if (accUpdatePeriod == 0) 176 | return; 177 | 178 | noInterrupts(); 179 | TeensyStepFTM::trigger(accUpdatePeriod, accLoopDelayChannel); 180 | interrupts(); 181 | 182 | handler->accTimerISR(); 183 | } 184 | } -------------------------------------------------------------------------------- /src/timer/teensy3/config.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace TeensyStepFTM 7 | { 8 | 9 | 10 | //========================================================================== 11 | // Available timer modules for the Teensy XX boards. Please note that those 12 | // timers are also used by the core libraries for PWM and AnalogWrite. 13 | // Therefore, choose a timer which isn't attached to the pins you need for 14 | // PWM or AnalogWrite. (TEENSY LC not yet supported) 15 | // 16 | // D: Default, X: available 17 | // 18 | // TIMER | Channels| Timer is available for | 19 | // | | T-LC | T3.0 | T3.1 | T3.2 | T3.5 | T3.6| 20 | //----------------------|---------|------|------|-------|------|------|-----| 21 | #define TIMER_FTM0 1 //| 8 | | D | D | D | D | D | 22 | #define TIMER_FTM1 2 //| 2 | | X | X | X | X | X | 23 | #define TIMER_FTM2 3 //| 2 | | | X | X | X | X | 24 | #define TIMER_FTM3 4 //| 8 | | | | | X | X | 25 | #define TIMER_TPM0 5 //| 6 | | | | | | | 26 | #define TIMER_TPM1 6 //| 2 | | | | | | X | 27 | #define TIMER_TPM2 7 //| 2 | | | | | | X | 28 | #define TIMER_DEFAULT -1 29 | 30 | // If you need a special timer, please replace "TIMER_DEFAULT" by a timer from the list above 31 | #define USE_TIMER TIMER_DEFAULT 32 | 33 | 34 | //========================================================================== 35 | // Nothing to be changed below here 36 | //========================================================================== 37 | 38 | 39 | 40 | #if USE_TIMER == TIMER_DEFAULT 41 | #undef USE_TIMER 42 | #if defined __MKL26Z64__ 43 | #define USE_TIMER TIMER_TPM0 44 | #else 45 | #define USE_TIMER TIMER_FTM0 46 | #endif 47 | #endif 48 | 49 | constexpr unsigned selTimer = USE_TIMER - 1; 50 | constexpr bool isFTM = selTimer <= 3; 51 | 52 | //================================================ 53 | // Which board do we use 54 | 55 | enum class _boards 56 | { 57 | T_LC, T_30, T31_2, T_35, T_36 58 | }; 59 | 60 | #if defined __MKL26Z64__ 61 | constexpr _boards board = _boards::T_LC; 62 | #elif defined __MK20DX128__ 63 | constexpr _boards board = _boards::T_30; 64 | #elif defined __MK20DX256__ 65 | constexpr _boards board = _boards::T31_2; 66 | #elif defined __MK64FX512__ 67 | constexpr _boards board = _boards::T_35; 68 | #elif defined __MK66FX1M0__ 69 | constexpr _boards board = _boards::T_36; 70 | #endif 71 | 72 | 73 | //==================================================================== 74 | // Memory layout of register banks for FTM and TPM timers 75 | 76 | typedef struct // FTM & TPM Channels 77 | { 78 | uint32_t SC; 79 | uint32_t CV; 80 | } FTM_CH_t; 81 | 82 | typedef struct // FTM register block (this layout is compatible to a TPM register block) 83 | { 84 | uint32_t SC; 85 | uint32_t CNT; 86 | uint32_t MOD; 87 | FTM_CH_t CH[8]; 88 | uint32_t CNTIN; 89 | uint32_t STATUS; 90 | uint32_t MODE; 91 | uint32_t SYNC; 92 | uint32_t OUTINIT; 93 | uint32_t OUTMASK; 94 | uint32_t COMBINE; 95 | uint32_t DEADTIME; 96 | uint32_t EXTTRIG; 97 | uint32_t POL; 98 | uint32_t FMS; 99 | uint32_t FILTER; 100 | uint32_t FLTCTRL; 101 | uint32_t QDCTRL; 102 | uint32_t CONF; 103 | uint32_t FLTPOL; 104 | uint32_t SYNCONF; 105 | uint32_t INVCTRL; 106 | uint32_t SWOCTRL; 107 | uint32_t PWMLOAD; 108 | }FTM_t; 109 | 110 | //----------------------------------------------------------------------------------- 111 | // Definition of base address for register block, number of channels and IRQ number 112 | // 113 | 114 | constexpr uintptr_t TimerBaseAddr[][7] = 115 | { // FTM0 FTM1 FTM2 FTM3 TPM0 TMP1 TMP2 116 | { 0, 0, 0, 0, 0, 0, 0 }, // Teensy LC not yet supported 117 | { 0x40038000, 0x40039000, 0, 0, 0, 0, 0, }, // Teensy 3.0 118 | { 0x40038000, 0x40039000, 0x400B8000, 0, 0, 0, 0, }, // Teensy 3.1/3.2 119 | { 0x40038000, 0x40039000, 0x400B8000, 0x400B9000, 0, 0, 0, }, // Teensy 3.5 120 | { 0x40038000, 0x40039000, 0x400B8000, 0x400B9000, 0, 0x400C9000, 0x400CA000 }, // Teensy 3.6 121 | }; 122 | 123 | constexpr int IRQ_Number[][7] 124 | { 125 | // FTM0 FTM1 FTM2 FTM3 TPM0 TPM1 TPM2 126 | { 0, 0, 0, 0, 0, 0, 0 }, // Teensy LC 127 | { 25, 26, 0, 0, 0, 0, 0 }, // Teensy 3.0 128 | { 62, 63, 64, 0, 0, 0, 0 }, // Teensy 3.1/3.2 129 | { 42, 43, 44, 71, 0, 0, 0 }, // Teensy 3.5 130 | { 42, 43, 44, 71, 0, 88, 89 }, // Teensy 3.6 131 | }; 132 | 133 | constexpr int _nrOfChannels[] 134 | { 135 | 8, // FTM0 136 | 2, // FTM1 137 | 2, // FTM2 138 | 8, // FTM3 139 | 6, // TPM0 140 | 2, // TPM1 141 | 2, // TPM2 142 | }; 143 | 144 | constexpr uintptr_t timerAddr = TimerBaseAddr[(int)board][selTimer]; 145 | constexpr volatile FTM_t* timer = __builtin_constant_p((FTM_t*)timerAddr) ? (FTM_t*)timerAddr : (FTM_t*)timerAddr; // base address for register block of selected timer 146 | constexpr unsigned irq = IRQ_Number[(int)board][selTimer]; // IRQ number of selected timer 147 | constexpr unsigned maxChannel = _nrOfChannels[selTimer]; // Number of channels for selected timer 148 | 149 | static_assert(timer != nullptr && irq != 0, "Board does not support choosen timer"); //Generate compiler error in case the board does not support the selected timer 150 | 151 | //----------------------------------------------------------------------------------- 152 | //Frequency dependent settings 153 | 154 | constexpr unsigned _timer_frequency = isFTM ? F_BUS : 16000000; // FTM timers are clocked with F_BUS, the TPM timers are clocked with OSCERCLK (16MHz for all teensies) 155 | 156 | // Choose prescaler such that one timer cycle corresponds to about 1µs 157 | constexpr unsigned prescale = 158 | _timer_frequency > 120000000 ? 0b111 : 159 | _timer_frequency > 60000000 ? 0b110 : 160 | _timer_frequency > 30000000 ? 0b101 : 161 | _timer_frequency > 15000000 ? 0b100 : 162 | _timer_frequency > 8000000 ? 0b011 : 163 | _timer_frequency > 4000000 ? 0b010 : 164 | _timer_frequency > 2000000 ? 0b001 : 0b000; 165 | 166 | // Calculates required reload value to get a delay of mu microseconds. 167 | // this will be completely evaluated by the compiler as long as mu is known at compile time 168 | constexpr int microsToReload(const float mu) 169 | { 170 | return mu * 1E-6 * _timer_frequency / (1 << prescale) + 0.5; 171 | } 172 | } 173 | -------------------------------------------------------------------------------- /src/timer/teensy4/PIT.cppc: -------------------------------------------------------------------------------- 1 | 2 | #include "PIT.h" 3 | 4 | #include "../TF_Handler.h" 5 | 6 | namespace 7 | { 8 | TF_Handler* Handler[4]; 9 | 10 | template 11 | void dispatchFunc() 12 | { 13 | Handler[n]->stepTimerISR(); 14 | } 15 | 16 | constexpr void(*dispatcher[])(void) = 17 | { 18 | dispatchFunc<0>, 19 | dispatchFunc<1>, 20 | dispatchFunc<2>, 21 | dispatchFunc<3> 22 | }; 23 | 24 | void dummyISR(void) {} 25 | } 26 | 27 | bool PIT::begin(TF_Handler* handler) 28 | { 29 | if (!timer.begin(dummyISR, 1E6)) return false; // try to reserve a timer 30 | 31 | setupChannel(); // find pit channel of reserved timer 32 | const int channelNr = channel - KINETISK_PIT_CHANNELS; 33 | Handler[channelNr] = handler; // store handler 34 | timer.priority(32); 35 | timer.begin(dispatcher[channelNr], 1E6); // attach an ISR which will call the stored handler 36 | stop(); // stop doesn't clear TEN, we want to keep the IntervalTimer reserved 37 | 38 | return true; 39 | } 40 | 41 | void PIT::setupChannel() 42 | { 43 | IRQ_NUMBER_t number = (IRQ_NUMBER_t)timer; 44 | switch (number) 45 | { 46 | case IRQ_PIT_CH0: 47 | channel = IMXRT_PIT_CHANNELS + 0; 48 | break; 49 | case IRQ_PIT_CH1: 50 | channel = IMXRT_PIT_CHANNELS + 1; 51 | break; 52 | case IRQ_PIT_CH2: 53 | channel = IMXRT_PIT_CHANNELS + 2; 54 | break; 55 | case IRQ_PIT_CH3: 56 | channel = IMXRT_PIT_CHANNELS + 3; 57 | break; 58 | default: 59 | channel = nullptr; 60 | break; 61 | } 62 | } -------------------------------------------------------------------------------- /src/timer/teensy4/PIT.hc: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "IntervalTimer.h" 4 | 5 | class IPitHandler 6 | { 7 | public: 8 | virtual void pitISR() = 0; 9 | }; 10 | 11 | class TF_Handler; 12 | 13 | class PIT 14 | { 15 | public: 16 | bool begin(TF_Handler*); 17 | 18 | inline void end() const { channel->TCTRL = 0; } 19 | inline void start() const { channel->TFLG = 1; channel->TCTRL = 0; channel->TCTRL = 3; } 20 | inline void stop() const { channel->TCTRL &= ~PIT_TCTRL_TIE; } 21 | inline void enableInterupt() const { channel->TFLG = 1; channel->TCTRL |= PIT_TCTRL_TIE; } 22 | inline void setFrequency(uint32_t val) const { channel->LDVAL = 24000000/val;} // also starts timer! 23 | inline void clearTIF()const { channel->TFLG = 1; } 24 | inline bool isRunning() const { return channel->TCTRL & PIT_TCTRL_TIE; } 25 | 26 | protected: 27 | public: 28 | IntervalTimer timer; 29 | void setupChannel(); 30 | 31 | IMXRT_PIT_CHANNEL_t* channel = nullptr; 32 | }; -------------------------------------------------------------------------------- /src/timer/teensy4/TeensyStepFTM.cppbak: -------------------------------------------------------------------------------- 1 | #if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) 2 | 3 | #include "TeensyStepFTM.h" 4 | #include "config.h" 5 | 6 | namespace TeensyStepFTM 7 | { 8 | 9 | namespace // private 10 | { 11 | bool isConfigured = false; 12 | IDelayHandler *callbacks[maxChannel]; 13 | } 14 | 15 | void begin() 16 | { 17 | if (!isConfigured) 18 | { 19 | if (USE_TIMER == TIMER_TPM1) 20 | { // enable clocks for tpm timers (ftm clocks are enabled by teensyduino) 21 | SIM_SCGC2 |= SIM_SCGC2_TPM1; 22 | SIM_SOPT2 |= SIM_SOPT2_TPMSRC(2); 23 | } 24 | else if (USE_TIMER == TIMER_TPM2) 25 | { 26 | SIM_SCGC2 |= SIM_SCGC2_TPM2; 27 | SIM_SOPT2 |= SIM_SOPT2_TPMSRC(2); 28 | } 29 | // Default Mode for FTM is (nearly) TPM compatibile 30 | timer->SC = FTM_SC_CLKS(0b00); // Disable clock 31 | timer->MOD = 0xFFFF; // Set full counter range 32 | 33 | for (unsigned i = 0; i < maxChannel; i++) 34 | { // turn off all channels which were enabled by teensyduino for PWM generation 35 | if (isFTM) 36 | { // compiletime constant, compiler optimizes conditional and not valid branch completely away 37 | timer->CH[i].SC &= ~FTM_CSC_CHF; // FTM requires to clear flag by setting bit to 0 38 | } 39 | else 40 | { 41 | timer->CH[i].SC |= FTM_CSC_CHF; // TPM requires to clear flag by setting bit to 1 42 | } 43 | timer->CH[i].SC &= ~FTM_CSC_CHIE; // Disable channel interupt 44 | timer->CH[i].SC = FTM_CSC_MSA; 45 | //timer->CH[i].SC = 0; // disable channel 46 | } 47 | timer->SC = FTM_SC_CLKS(0b01) | FTM_SC_PS(prescale); // Start clock 48 | NVIC_ENABLE_IRQ(irq); // Enable interrupt request for selected timer 49 | 50 | isConfigured = true; 51 | } 52 | } 53 | 54 | unsigned addDelayChannel(IDelayHandler *handler) 55 | { 56 | for (unsigned ch = 0; ch < maxChannel; ch++) 57 | { 58 | if (callbacks[ch] == 0) 59 | { 60 | callbacks[ch] = handler; //Just store the callback function, the rest is done in Trigger function 61 | return ch; 62 | } 63 | } 64 | return maxChannel; 65 | } 66 | 67 | void removeDelayChannel(unsigned chNr) 68 | { 69 | if (chNr < maxChannel) 70 | callbacks[chNr] = nullptr; 71 | } 72 | } // namespace TeensyDelay2 73 | 74 | //------------------------------------------------------------------------------------------- 75 | // Interupt service routine of the timer selected in config.h. 76 | // The code doesn't touch the other FTM/TPM ISRs so they can still be used for other purposes 77 | // 78 | // Unfortunately we can not inline the ISR because inlinig will generate a "weak" function?. 79 | // Since the original ISR (dummy_isr) is also defined weak the linker 80 | // is allowed to choose any of them. In this case it desided to use dummy_isr :-( 81 | // Using a "strong" (not inlined) function overrides the week dummy_isr 82 | //-------------------------------------------------------------------------------------------- 83 | 84 | using namespace TeensyStepFTM; 85 | 86 | #if USE_TIMER == TIMER_FTM0 87 | void ftm0_isr(void) 88 | #elif USE_TIMER == TIMER_FTM1 89 | void ftm1_isr(void) 90 | #elif USE_TIMER == TIMER_FTM2 91 | void ftm2_isr(void) 92 | #elif USE_TIMER == TIMER_FTM3 93 | void ftm3_isr(void) 94 | #elif USE_TIMER == TIMER_TPM1 95 | void tpm1_isr(void) 96 | #elif USE_TIMER == TIMER_TPM2 97 | void tpm2_isr(void) 98 | #endif 99 | { 100 | for (unsigned i = 0; i < maxChannel; i++) 101 | { 102 | if ((timer->CH[i].SC & (FTM_CSC_CHIE | FTM_CSC_CHF)) == (FTM_CSC_CHIE | FTM_CSC_CHF)) // only handle if channel is active (CHIE set) and overflowed (CHF set) 103 | { 104 | timer->CH[i].SC &= ~FTM_CSC_CHIE; // We want one shot only. (Make sure to reset the CHF flag before re-activating interrupt in trigger function) 105 | callbacks[i]->delayISR(i); // invoke callback function for the channel 106 | } 107 | } 108 | } 109 | #endif -------------------------------------------------------------------------------- /src/timer/teensy4/TeensyStepFTM.hh: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "config.h" 3 | 4 | #include "Arduino.h" 5 | 6 | class IDelayHandler 7 | { 8 | public: 9 | virtual void delayISR(unsigned channel) = 0; 10 | }; 11 | 12 | namespace TeensyStepFTM 13 | { 14 | extern void begin(void); 15 | 16 | extern unsigned addDelayChannel(IDelayHandler* h); 17 | extern void removeDelayChannel(unsigned channel); 18 | 19 | inline void trigger(unsigned delay, unsigned channel) 20 | { 21 | //Serial.printf("chn: %d, tdt: %d\n", channel, microsToReload(delay)); 22 | 23 | timer->SC = 0; // Remove clock to immediately write new channel value 24 | timer->CH[channel].CV = timer->CNT + delay; // Update channel value 25 | timer->SC = FTM_SC_CLKS(0b01) | FTM_SC_PS(prescale); // Reattach clock 26 | 27 | if (isFTM) { // important to clear channel flag (in case we had a channel event since last trigger) 28 | timer->CH[channel].SC &= ~FTM_CSC_CHF; // enable channel interrupt. Can not combine both commands since clearing 29 | } 30 | else { // the channel flag requires a read modify write cycle 31 | timer->CH[channel].SC |= FTM_CSC_CHF; // FTM and TPM timers differ in resetting the interrupt flag 32 | } // isFTM is a compile time constant -> compiler completely optimizes the not valid case away 33 | timer->CH[channel].SC = FTM_CSC_MSA | FTM_CSC_CHIE; 34 | } 35 | } -------------------------------------------------------------------------------- /src/timer/teensy4/TimerField.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "wiring.h" 4 | #include "imxrt.h" 5 | 6 | #include "ticktimer.h" 7 | // #include "TeensyStepFTM.h" 8 | 9 | #include "../TF_Handler.h" 10 | //#include "../TimerFieldBase.h" 11 | 12 | #include "IntervalTimer.h" 13 | 14 | //========================= 15 | // Teensy 4 16 | //========================= 17 | 18 | class TimerField 19 | { 20 | public: 21 | inline TimerField(TF_Handler *); 22 | 23 | inline bool begin(); 24 | inline void end(); 25 | 26 | inline void stepTimerStart(); 27 | inline void stepTimerStop(); 28 | inline bool stepTimerIsRunning() const; 29 | inline void setStepFrequency(unsigned f); 30 | inline unsigned getStepFrequency() { 31 | return 0; 32 | 33 | } 34 | 35 | 36 | inline void accTimerStart(); 37 | inline void accTimerStop(); 38 | inline void setAccUpdatePeriod(unsigned period); 39 | 40 | inline void setPulseWidth(unsigned delay); 41 | inline void triggerDelay(); 42 | inline void delayISR(unsigned channel); 43 | 44 | static inline void tick() { tickTimer::tick(); } 45 | 46 | protected: 47 | //PIT stepTimer; 48 | 49 | tickTimer timer; 50 | 51 | TF_Handler *handler; 52 | 53 | unsigned delayWidth; 54 | unsigned accUpdatePeriod; 55 | 56 | unsigned accLoopDelayChannel; 57 | unsigned pinResetDelayChannel; 58 | 59 | unsigned stepFrequency; 60 | }; 61 | 62 | // IMPLEMENTATION ==================================================================== 63 | 64 | TimerField::TimerField(TF_Handler *_handler) 65 | : handler(_handler) 66 | // accLoopDelayChannel(TeensyStepFTM::addDelayChannel(this)), 67 | // pinResetDelayChannel(TeensyStepFTM::addDelayChannel(this)) 68 | { 69 | // Serial.println("tf"); 70 | accLoopDelayChannel = timer.attachPeriodicTimer(handler->accTimerISR); 71 | } 72 | 73 | bool TimerField::begin() 74 | { 75 | // TeensyStepFTM::begin(); 76 | // return stepTimer.begin(handler); 77 | return false; 78 | } 79 | 80 | void TimerField::end() 81 | { 82 | // stepTimer.end(); 83 | // TeensyStepFTM::removeDelayChannel(accLoopDelayChannel); 84 | // TeensyStepFTM::removeDelayChannel(pinResetDelayChannel); 85 | } 86 | 87 | // Step Timer ------------------------------------------------------ 88 | 89 | void TimerField::stepTimerStart() 90 | { 91 | // stepTimer.start(); 92 | } 93 | 94 | void TimerField::stepTimerStop() 95 | { 96 | //stepTimer.stop(); 97 | } 98 | 99 | void TimerField::setStepFrequency(unsigned f) 100 | { 101 | // stepFrequency = f; 102 | // stepTimer.setFrequency(stepFrequency); 103 | } 104 | 105 | bool TimerField::stepTimerIsRunning() const 106 | { 107 | //return stepTimer.isRunning(); 108 | return false; 109 | } 110 | 111 | 112 | // Acceleration Timer ------------------------------------------------------ 113 | 114 | void TimerField::accTimerStart() 115 | { 116 | //delayISR(accLoopDelayChannel); 117 | } 118 | 119 | void TimerField::setAccUpdatePeriod(unsigned p) 120 | { 121 | accUpdatePeriod = p; 122 | } 123 | 124 | void TimerField::accTimerStop() 125 | { 126 | accUpdatePeriod = 0; 127 | } 128 | 129 | // Delay Timer ------------------------------------------------------ 130 | 131 | void TimerField::setPulseWidth(unsigned delay) 132 | { 133 | // delayWidth = TeensyStepFTM::microsToReload(delay); 134 | } 135 | 136 | void TimerField::triggerDelay() 137 | { 138 | // TeensyStepFTM::trigger(delayWidth, pinResetDelayChannel); 139 | } 140 | 141 | void TimerField::delayISR(unsigned channel) 142 | { 143 | // if (channel == pinResetDelayChannel) 144 | // { 145 | // handler->pulseTimerISR(); 146 | // } 147 | 148 | // else if (channel == accLoopDelayChannel) 149 | // { 150 | // if (accUpdatePeriod == 0) return; 151 | 152 | // noInterrupts(); 153 | // // TeensyStepFTM::trigger(accUpdatePeriod, accLoopDelayChannel); 154 | // interrupts(); 155 | 156 | // handler->accTimerISR(); 157 | // } 158 | } -------------------------------------------------------------------------------- /src/timer/teensy4/config.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace TeensyStepFTM 7 | { 8 | 9 | 10 | //========================================================================== 11 | // Available timer modules for the Teensy XX boards. Please note that those 12 | // timers are also used by the core libraries for PWM and AnalogWrite. 13 | // Therefore, choose a timer which isn't attached to the pins you need for 14 | // PWM or AnalogWrite. (TEENSY LC not yet supported) 15 | // 16 | // D: Default, X: available 17 | // 18 | // TIMER | Channels| Timer is available for | 19 | // | | T-LC | T3.0 | T3.1 | T3.2 | T3.5 | T3.6| 20 | //----------------------|---------|------|------|-------|------|------|-----| 21 | #define TIMER_FTM0 1 //| 8 | | D | D | D | D | D | 22 | #define TIMER_FTM1 2 //| 2 | | X | X | X | X | X | 23 | #define TIMER_FTM2 3 //| 2 | | | X | X | X | X | 24 | #define TIMER_FTM3 4 //| 8 | | | | | X | X | 25 | #define TIMER_TPM0 5 //| 6 | | | | | | | 26 | #define TIMER_TPM1 6 //| 2 | | | | | | X | 27 | #define TIMER_TPM2 7 //| 2 | | | | | | X | 28 | #define TIMER_DEFAULT -1 29 | 30 | // If you need a special timer, please replace "TIMER_DEFAULT" by a timer from the list above 31 | #define USE_TIMER TIMER_DEFAULT 32 | 33 | 34 | //========================================================================== 35 | // Nothing to be changed below here 36 | //========================================================================== 37 | 38 | 39 | 40 | #if USE_TIMER == TIMER_DEFAULT 41 | #undef USE_TIMER 42 | #if defined __MKL26Z64__ 43 | #define USE_TIMER TIMER_TPM0 44 | #else 45 | #define USE_TIMER TIMER_FTM0 46 | #endif 47 | #endif 48 | 49 | constexpr unsigned selTimer = USE_TIMER - 1; 50 | constexpr bool isFTM = selTimer <= 3; 51 | 52 | //================================================ 53 | // Which board do we use 54 | 55 | enum class _boards 56 | { 57 | T_LC, T_30, T31_2, T_35, T_36 58 | }; 59 | 60 | #if defined __MKL26Z64__ 61 | constexpr _boards board = _boards::T_LC; 62 | #elif defined __MK20DX128__ 63 | constexpr _boards board = _boards::T_30; 64 | #elif defined __MK20DX256__ 65 | constexpr _boards board = _boards::T31_2; 66 | #elif defined __MK64FX512__ 67 | constexpr _boards board = _boards::T_35; 68 | #elif defined __MK66FX1M0__ 69 | constexpr _boards board = _boards::T_36; 70 | #endif 71 | 72 | 73 | //==================================================================== 74 | // Memory layout of register banks for FTM and TPM timers 75 | 76 | typedef struct // FTM & TPM Channels 77 | { 78 | uint32_t SC; 79 | uint32_t CV; 80 | } FTM_CH_t; 81 | 82 | typedef struct // FTM register block (this layout is compatible to a TPM register block) 83 | { 84 | uint32_t SC; 85 | uint32_t CNT; 86 | uint32_t MOD; 87 | FTM_CH_t CH[8]; 88 | uint32_t CNTIN; 89 | uint32_t STATUS; 90 | uint32_t MODE; 91 | uint32_t SYNC; 92 | uint32_t OUTINIT; 93 | uint32_t OUTMASK; 94 | uint32_t COMBINE; 95 | uint32_t DEADTIME; 96 | uint32_t EXTTRIG; 97 | uint32_t POL; 98 | uint32_t FMS; 99 | uint32_t FILTER; 100 | uint32_t FLTCTRL; 101 | uint32_t QDCTRL; 102 | uint32_t CONF; 103 | uint32_t FLTPOL; 104 | uint32_t SYNCONF; 105 | uint32_t INVCTRL; 106 | uint32_t SWOCTRL; 107 | uint32_t PWMLOAD; 108 | }FTM_t; 109 | 110 | //----------------------------------------------------------------------------------- 111 | // Definition of base address for register block, number of channels and IRQ number 112 | // 113 | 114 | constexpr uintptr_t TimerBaseAddr[][7] = 115 | { // FTM0 FTM1 FTM2 FTM3 TPM0 TMP1 TMP2 116 | { 0, 0, 0, 0, 0, 0, 0 }, // Teensy LC not yet supported 117 | { 0x40038000, 0x40039000, 0, 0, 0, 0, 0, }, // Teensy 3.0 118 | { 0x40038000, 0x40039000, 0x400B8000, 0, 0, 0, 0, }, // Teensy 3.1/3.2 119 | { 0x40038000, 0x40039000, 0x400B8000, 0x400B9000, 0, 0, 0, }, // Teensy 3.5 120 | { 0x40038000, 0x40039000, 0x400B8000, 0x400B9000, 0, 0x400C9000, 0x400CA000 }, // Teensy 3.6 121 | }; 122 | 123 | constexpr int IRQ_Number[][7] 124 | { 125 | // FTM0 FTM1 FTM2 FTM3 TPM0 TPM1 TPM2 126 | { 0, 0, 0, 0, 0, 0, 0 }, // Teensy LC 127 | { 25, 26, 0, 0, 0, 0, 0 }, // Teensy 3.0 128 | { 62, 63, 64, 0, 0, 0, 0 }, // Teensy 3.1/3.2 129 | { 42, 43, 44, 71, 0, 0, 0 }, // Teensy 3.5 130 | { 42, 43, 44, 71, 0, 88, 89 }, // Teensy 3.6 131 | }; 132 | 133 | constexpr int _nrOfChannels[] 134 | { 135 | 8, // FTM0 136 | 2, // FTM1 137 | 2, // FTM2 138 | 8, // FTM3 139 | 6, // TPM0 140 | 2, // TPM1 141 | 2, // TPM2 142 | }; 143 | 144 | constexpr uintptr_t timerAddr = TimerBaseAddr[(int)board][selTimer]; 145 | constexpr volatile FTM_t* timer = __builtin_constant_p((FTM_t*)timerAddr) ? (FTM_t*)timerAddr : (FTM_t*)timerAddr; // base address for register block of selected timer 146 | constexpr unsigned irq = IRQ_Number[(int)board][selTimer]; // IRQ number of selected timer 147 | constexpr unsigned maxChannel = _nrOfChannels[selTimer]; // Number of channels for selected timer 148 | 149 | static_assert(timer != nullptr && irq != 0, "Board does not support choosen timer"); //Generate compiler error in case the board does not support the selected timer 150 | 151 | //----------------------------------------------------------------------------------- 152 | //Frequency dependent settings 153 | 154 | constexpr unsigned _timer_frequency = isFTM ? F_BUS : 16000000; // FTM timers are clocked with F_BUS, the TPM timers are clocked with OSCERCLK (16MHz for all teensies) 155 | 156 | // Choose prescaler such that one timer cycle corresponds to about 1µs 157 | constexpr unsigned prescale = 158 | _timer_frequency > 120000000 ? 0b111 : 159 | _timer_frequency > 60000000 ? 0b110 : 160 | _timer_frequency > 30000000 ? 0b101 : 161 | _timer_frequency > 15000000 ? 0b100 : 162 | _timer_frequency > 8000000 ? 0b011 : 163 | _timer_frequency > 4000000 ? 0b010 : 164 | _timer_frequency > 2000000 ? 0b001 : 0b000; 165 | 166 | // Calculates required reload value to get a delay of mu microseconds. 167 | // this will be completely evaluated by the compiler as long as mu is known at compile time 168 | constexpr int microsToReload(const float mu) 169 | { 170 | return mu * 1E-6 * _timer_frequency / (1 << prescale) + 0.5; 171 | } 172 | } 173 | -------------------------------------------------------------------------------- /src/timer/teensy4/ticktimer.h: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include 3 | //#include 4 | 5 | using callback_t = void (*)(); 6 | //using callback_t = std::function; 7 | 8 | struct channelInfo 9 | { 10 | channelInfo() 11 | { 12 | callback = nullptr; 13 | delta = 0; 14 | run = false; 15 | }; 16 | 17 | unsigned delta; 18 | unsigned start; 19 | bool isPeriodic; 20 | bool run; 21 | callback_t callback; 22 | }; 23 | 24 | class tickTimer 25 | { 26 | static constexpr unsigned maxTimers = 40; 27 | 28 | public: 29 | tickTimer(); 30 | 31 | int attachPeriodicTimer(callback_t callback, unsigned period) { return addTimer(callback, period, true); } 32 | int attachOneShotTimer(callback_t callback, unsigned delay) { return addTimer(callback, delay, false); } 33 | 34 | inline void setFrequency(unsigned f); 35 | inline void trigger(unsigned i) 36 | { 37 | channels[i].start = ARM_DWT_CYCCNT; 38 | channels[i].run = true; 39 | } 40 | 41 | static inline void tick(); 42 | 43 | protected: 44 | int addTimer(callback_t callback, unsigned t, bool periodic); 45 | 46 | static channelInfo *channels; 47 | }; 48 | 49 | void tickTimer::tick() 50 | { 51 | digitalWriteFast(4, HIGH); 52 | for (unsigned i = 0; i < maxTimers; i++) 53 | { 54 | channelInfo *channel = channels + i; 55 | 56 | if (channel->run && (ARM_DWT_CYCCNT - channel->start >= channel->delta)) 57 | { 58 | if (channel->isPeriodic) 59 | channel->start = ARM_DWT_CYCCNT; 60 | else 61 | channel->run = false; 62 | 63 | channel->callback(); 64 | } 65 | } 66 | digitalWriteFast(4, LOW); 67 | } 68 | 69 | int tickTimer::addTimer(callback_t cb, unsigned delay, bool isPeriodic) 70 | { 71 | for (unsigned i = 0; i < maxTimers; i++) 72 | { 73 | channelInfo *channel = channels + i; 74 | if (channel->callback == nullptr) 75 | { 76 | channel->start = ARM_DWT_CYCCNT; 77 | channel->isPeriodic = isPeriodic; 78 | channel->callback = cb; 79 | channel->delta = delay; 80 | channel->run = true; 81 | 82 | return i; 83 | } 84 | } 85 | return -1; 86 | } 87 | 88 | tickTimer::tickTimer() 89 | { 90 | ARM_DEMCR |= ARM_DEMCR_TRCENA; 91 | ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA; 92 | } 93 | 94 | void tickTimer::setFrequency(unsigned f) 95 | { 96 | } 97 | 98 | channelInfo *tickTimer::channels = new channelInfo[tickTimer::maxTimers + 1]; -------------------------------------------------------------------------------- /src/version.h: -------------------------------------------------------------------------------- 1 | namespace TeensyStep 2 | { 3 | 4 | constexpr const char *version = "V2.0.1"; 5 | constexpr const char *branch = "esp32"; 6 | 7 | } // namespace TeensyStep --------------------------------------------------------------------------------