From 58b797718bcc4bbb1e3c359ac13ef65ed47f149b Mon Sep 17 00:00:00 2001 From: seifemad Date: Mon, 21 Sep 2026 13:37:32 +0300 Subject: [PATCH] Add FastAccelStepperEngine::moveAllToSync() for synchronized multi-axis moves Starts several independent steppers, each to its own target position, timed so they all reach standstill at approximately the same time. Each axis still runs its own trapezoidal/S-curve ramp independently - this is not interpolated straight-line motion - only the per-axis speed (and, for very short moves, acceleration) is scaled down, never up, so the fastest axes are slowed to match the slowest one. Includes a worked example (examples/SynchronizedMultiMove) that measures and prints per-axis arrival time to verify synchronization on real hardware. --- CHANGELOG.md | 3 + README.md | 1 + .../SynchronizedMultiMove.ino | 142 ++++++++++++++++++ extras/doc/FastAccelStepper_API.md | 32 ++++ keywords.txt | 1 + src/FastAccelStepperEngine.cpp | 82 ++++++++++ src/FastAccelStepperEngine.h | 31 ++++ 7 files changed, 292 insertions(+) create mode 100644 examples/SynchronizedMultiMove/SynchronizedMultiMove.ino diff --git a/CHANGELOG.md b/CHANGELOG.md index ff2863af..23509840 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,6 @@ +Unreleased: +- FastAccelStepperEngine::moveAllToSync(): start several steppers together, each to its own target position, scaling down the per-axis speed/acceleration so all of them reach standstill at approximately the same time (independent ramps, not interpolated motion) + 1.3.3: - moveTimed(0, duration): pause uses last direction XOR prepare_revert (default false does not toggle DIR; true pauses in the opposite direction) diff --git a/README.md b/README.md index c5e9f232..1b115e5c 100644 --- a/README.md +++ b/README.md @@ -72,6 +72,7 @@ FastAccelStepper offers the following features: * No float calculation (log2 representation in range -64..64 with 16bit integer representation and 1/512th resolution) * Provide API to each steppers' command queue. Those commands are tied to timer ticks aka the CPU frequency! * Command queue can be filled with commands and then started. This allows near synchronous start of several steppers for multi axis applications. +* `FastAccelStepperEngine::moveAllToSync()` starts several independent steppers - each to its own target position - scaling down the per-axis speed/acceleration so they all reach standstill at approximately the same time ## Star History diff --git a/examples/SynchronizedMultiMove/SynchronizedMultiMove.ino b/examples/SynchronizedMultiMove/SynchronizedMultiMove.ino new file mode 100644 index 00000000..32cd5d61 --- /dev/null +++ b/examples/SynchronizedMultiMove/SynchronizedMultiMove.ino @@ -0,0 +1,142 @@ +#include "FastAccelStepper.h" + +// Test/demo sketch for two features: +// +// 1) S-curve motion (jerk-limited ramp): setLinearAcceleration() makes the +// acceleration itself ramp up linearly from 0 to the configured value +// over the given number of steps, instead of jumping to it immediately. +// That rounds off the corners of the speed profile into an S-shape, +// instead of a plain trapezoid. +// +// 2) Two (or more) steppers moving to different target positions, started +// together and arriving back at standstill together, via +// engine.moveAllToSync(). Each axis still runs its own independent +// ramp - this is not interpolated straight-line motion - but the +// speed (and, for very short moves, the acceleration) of every axis +// but the slowest is scaled down so all of them take the same time. +// +// HOW TO VERIFY IT WORKS: +// Open the Serial Monitor at 115200 baud. Every cycle prints when the move +// starts and, per stepper, how many ms after the start it reached standstill +// again. Both "reached standstill" lines should be close to each other +// (a few ms apart - one stepper task tick, ~4ms - is expected/fine), +// regardless of the very different distance/speed/acceleration configured +// for the two steppers below. +// +// Wiring: connect two step/dir/enable driver boards, or just probe the pins +// with a logic analyzer/scope - real motors are not required to see the +// timing on the pins. + +// As in StepperDemo for Motor 1+2 on ESP32 +#define dirPinStepper1 18 +#define enablePinStepper1 26 +#define stepPinStepper1 17 + +#define dirPinStepper2 19 +#define enablePinStepper2 26 +#define stepPinStepper2 16 + +// For AVR (e.g. Arduino Nano/Uno), use instead: +// #define dirPinStepper1 5 +// #define enablePinStepper1 6 +// #define stepPinStepper1 9 // OC1A +// #define dirPinStepper2 7 +// #define enablePinStepper2 6 +// #define stepPinStepper2 10 // OC1B + +FastAccelStepperEngine engine = FastAccelStepperEngine(); +FastAccelStepper *stepper1 = NULL; +FastAccelStepper *stepper2 = NULL; + +void setup() { + Serial.begin(115200); + Serial.println("START"); + engine.init(); + + stepper1 = engine.stepperConnectToPin(stepPinStepper1); + stepper2 = engine.stepperConnectToPin(stepPinStepper2); + + if (stepper1 && stepper2) { + stepper1->setDirectionPin(dirPinStepper1); + stepper1->setEnablePin(enablePinStepper1); + stepper1->setAutoEnable(true); + + stepper2->setDirectionPin(dirPinStepper2); + stepper2->setEnablePin(enablePinStepper2); + stepper2->setAutoEnable(true); + + // Each axis' own maximum speed/acceleration. stepper2 is nominally + // much faster than stepper1 - moveAllToSync() will slow it down for + // any move where stepper1 would otherwise be the bottleneck. + stepper1->setSpeedInHz(4000); + stepper1->setAcceleration(8000); + stepper1->setLinearAcceleration(100); // S-curve corners + + stepper2->setSpeedInHz(12000); + stepper2->setAcceleration(30000); + stepper2->setLinearAcceleration(300); // S-curve corners + } else { + while (true) { + Serial.println("NO STEPPER - check pin definitions for your board"); + delay(1000); + } + } +} + +bool going_forward = true; +bool move_active = false; +uint32_t move_start_ms = 0; +bool stepper1_done = false; +bool stepper2_done = false; + +void startNextMove() { + int32_t target1 = going_forward ? 4000 : 0; // long move + int32_t target2 = going_forward ? 1000 : 0; // short move, faster axis + going_forward = !going_forward; + + FastAccelStepper *steppers[2] = {stepper1, stepper2}; + int32_t targets[2] = {target1, target2}; + + move_start_ms = millis(); + stepper1_done = false; + stepper2_done = false; + move_active = true; + + Serial.print("t=0ms starting move -> stepper1:"); + Serial.print(target1); + Serial.print(" stepper2:"); + Serial.println(target2); + + MoveResultCode res = engine.moveAllToSync(steppers, targets, 2); + if (!moveIsOk(res)) { + Serial.print(" moveAllToSync() returned error: "); + Serial.println(toString(res)); + } +} + +void loop() { + if (!move_active) { + startNextMove(); + return; + } + + if (!stepper1_done && !stepper1->isRunning()) { + stepper1_done = true; + Serial.print("t="); + Serial.print(millis() - move_start_ms); + Serial.println("ms stepper1 reached standstill"); + } + if (!stepper2_done && !stepper2->isRunning()) { + stepper2_done = true; + Serial.print("t="); + Serial.print(millis() - move_start_ms); + Serial.println("ms stepper2 reached standstill"); + } + + if (stepper1_done && stepper2_done) { + Serial.println("-- move complete, both arrived --"); + Serial.println(); + delay(1000); // pause so the log is easy to read + move_active = false; + } +} diff --git a/extras/doc/FastAccelStepper_API.md b/extras/doc/FastAccelStepper_API.md index 75ccc19c..a63a3afe 100644 --- a/extras/doc/FastAccelStepper_API.md +++ b/extras/doc/FastAccelStepper_API.md @@ -166,6 +166,38 @@ the engine. The periodic task will let the associated LED blink with 1 Hz ```cpp void setDebugLed(uint8_t ledPin); ``` +## Synchronized Multi-Axis Move + +moveAllToSync() moves several independent steppers - each to its own +target position - so that all moves start together and reach +standstill again at approximately the same time. + +This is not coordinated/interpolated motion: there is no enforced +straight line between axes, each stepper still follows its own +trapezoidal or S-curve ramp (see setLinearAcceleration()). Only the +per-axis speed - and, for very short moves, the acceleration - is +scaled down (never up) so the fastest axes are slowed to match the +slowest one. + +Before calling, every stepper in `steppers` must already have +setSpeedInHz() and setAcceleration() configured: these are used as +each axis' allowed maximum and are reduced only for this move. A +later, unrelated call to moveTo()/move() will keep using the reduced +values, so reconfigure speed/acceleration again if the axis is moved +individually afterwards. + +`steppers[i]` moves from its current position to `targetPositions[i]`, +for i in 0..count-1. A NULL entry in `steppers` is skipped. `count` is +capped to MAX_STEPPER. + +Returns the first non-OK result of the individual moveTo() calls, or +MOVE_OK if all were started successfully. Even on error, moveTo() is +still attempted for every axis - already started axes are not rolled +back. +```cpp + MoveResultCode moveAllToSync(FastAccelStepper* const* steppers, + const int32_t* targetPositions, uint8_t count); +``` ### Return codes of calls to `move()` and `moveTo()` The defined preprocessor macros are MOVE_xxx: diff --git a/keywords.txt b/keywords.txt index 1748168d..202dcf42 100644 --- a/keywords.txt +++ b/keywords.txt @@ -18,6 +18,7 @@ init KEYWORD2 stepperConnectToPin KEYWORD2 setExternalCallForPin KEYWORD2 setDebugLed KEYWORD2 +moveAllToSync KEYWORD2 manageSteppers KEYWORD2 task_rate KEYWORD2 initI2sMux KEYWORD2 diff --git a/src/FastAccelStepperEngine.cpp b/src/FastAccelStepperEngine.cpp index 9e0a55b9..fb75d7ee 100644 --- a/src/FastAccelStepperEngine.cpp +++ b/src/FastAccelStepperEngine.cpp @@ -1,4 +1,5 @@ #include "FastAccelStepperEngine.h" +#include #include "FastAccelStepper.h" #include "fas_queue/stepper_queue.h" #if defined(SUPPORT_ESP32_I2S) @@ -169,6 +170,87 @@ void FastAccelStepperEngine::setDebugLed(uint8_t ledPin) { PIN_OUTPUT(fas_ledPin, LOW); } +MoveResultCode FastAccelStepperEngine::moveAllToSync( + FastAccelStepper* const* steppers, const int32_t* targetPositions, + uint8_t count) { + if (count > MAX_STEPPER) { + count = MAX_STEPPER; + } + + // Pass 1: find how long the slowest axis would take at its own + // currently configured speed/acceleration. That duration becomes the + // common target duration for all axes. + float distance[MAX_STEPPER]; + float target_time = 0; + for (uint8_t i = 0; i < count; i++) { + distance[i] = 0; + FastAccelStepper* s = steppers[i]; + if (s == NULL) { + continue; + } + int32_t d = targetPositions[i] - s->getCurrentPosition(); + distance[i] = (d < 0) ? (float)(-d) : (float)d; + float v = s->getSpeedInMilliHz() / 1000.0f; + float a = (float)s->getAcceleration(); + if ((distance[i] <= 0) || (v <= 0) || (a <= 0)) { + continue; + } + // symmetric ramp up/down: distance covered while not at constant + // speed is v^2/a, taking time 2*v/a + float t_ramp = v / a; + float d_ramp = v * t_ramp; + float t = (distance[i] >= d_ramp) + ? 2.0f * t_ramp + (distance[i] - d_ramp) / v + : 2.0f * sqrt(a * distance[i]) / a; + if (t > target_time) { + target_time = t; + } + } + + // Pass 2: slow every other axis down (speed first, and - for moves too + // short to ever reach that reduced speed - acceleration too) so its own + // move takes target_time, then start the move. + MoveResultCode first_error = MOVE_OK; + for (uint8_t i = 0; i < count; i++) { + FastAccelStepper* s = steppers[i]; + if (s == NULL) { + continue; + } + float v = s->getSpeedInMilliHz() / 1000.0f; + float a = (float)s->getAcceleration(); + if ((distance[i] > 0) && (target_time > 0) && (v > 0) && (a > 0)) { + float v_new = v; + float a_new = a; + float disc = + (a * target_time) * (a * target_time) - 4.0f * a * distance[i]; + bool trapezoid = false; + if (disc >= 0) { + v_new = (a * target_time - sqrt(disc)) / 2.0f; + trapezoid = (distance[i] - (v_new * v_new) / a) >= 0; + } + if (!trapezoid) { + // move too short to ever cruise at a constant speed for + // target_time: shrink acceleration, so the triangular ramp itself + // takes exactly target_time + a_new = 4.0f * distance[i] / (target_time * target_time); + v_new = 2.0f * distance[i] / target_time; + } + // never exceed the axis' own configured maximum + if (v_new > v) v_new = v; + if (v_new < 1.0f) v_new = 1.0f; + if (a_new > a) a_new = a; + if (a_new < 1.0f) a_new = 1.0f; + s->setSpeedInHz((uint32_t)(v_new + 0.5f)); + s->setAcceleration((int32_t)(a_new + 0.5f)); + } + MoveResultCode res = s->moveTo(targetPositions[i]); + if ((first_error == MOVE_OK) && (res != MOVE_OK)) { + first_error = res; + } + } + return first_error; +} + void FastAccelStepperEngine::manageSteppers() { #ifdef DEBUG_LED_HALF_PERIOD if (fas_ledPin != PIN_UNDEFINED) { diff --git a/src/FastAccelStepperEngine.h b/src/FastAccelStepperEngine.h index b7a185a5..084a71cd 100644 --- a/src/FastAccelStepperEngine.h +++ b/src/FastAccelStepperEngine.h @@ -181,6 +181,37 @@ class FastAccelStepperEngine { // the engine. The periodic task will let the associated LED blink with 1 Hz void setDebugLed(uint8_t ledPin); + // ## Synchronized Multi-Axis Move + // + // moveAllToSync() moves several independent steppers - each to its own + // target position - so that all moves start together and reach + // standstill again at approximately the same time. + // + // This is not coordinated/interpolated motion: there is no enforced + // straight line between axes, each stepper still follows its own + // trapezoidal or S-curve ramp (see setLinearAcceleration()). Only the + // per-axis speed - and, for very short moves, the acceleration - is + // scaled down (never up) so the fastest axes are slowed to match the + // slowest one. + // + // Before calling, every stepper in `steppers` must already have + // setSpeedInHz() and setAcceleration() configured: these are used as + // each axis' allowed maximum and are reduced only for this move. A + // later, unrelated call to moveTo()/move() will keep using the reduced + // values, so reconfigure speed/acceleration again if the axis is moved + // individually afterwards. + // + // `steppers[i]` moves from its current position to `targetPositions[i]`, + // for i in 0..count-1. A NULL entry in `steppers` is skipped. `count` is + // capped to MAX_STEPPER. + // + // Returns the first non-OK result of the individual moveTo() calls, or + // MOVE_OK if all were started successfully. Even on error, moveTo() is + // still attempted for every axis - already started axes are not rolled + // back. + MoveResultCode moveAllToSync(FastAccelStepper* const* steppers, + const int32_t* targetPositions, uint8_t count); + /* This should be only called from ISR or stepper task. So do not call it */ void manageSteppers();