Add FastAccelStepperEngine::moveAllToSync() for synchronized multi-axis moves

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)
 

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
 

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;
+  }
+}

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:

keywords.txt

@@ -18,6 +18,7 @@ init	KEYWORD2
 stepperConnectToPin	KEYWORD2
 setExternalCallForPin	KEYWORD2
 setDebugLed	KEYWORD2
+moveAllToSync	KEYWORD2
 manageSteppers	KEYWORD2
 task_rate	KEYWORD2
 initI2sMux	KEYWORD2

src/FastAccelStepperEngine.cpp

@@ -1,4 +1,5 @@
 #include "FastAccelStepperEngine.h"
+#include <math.h>
 #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) {

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();