Use simple motion planner for movement.

include/motion_planner.h

@@ -0,0 +1,51 @@
+#include <Arduino.h>
+#include <FastAccelStepper.h>
+
+class XYMotionPlanner {
+public:
+  XYMotionPlanner(FastAccelStepper* stepperX, FastAccelStepper* stepperY);
+
+  // Basisparameter der "dominanten" Achse
+  void setBaseLimits(uint32_t maxSpeedHz, uint32_t maxAccelStepsPerS2);
+
+  // Minimale Werte vermeiden, dass eine kurze Achse mit 0 endet
+  void setMinimums(uint32_t minSpeedHz, uint32_t minAccelStepsPerS2);
+
+  // Aktuelle Positionen der Planner-Sicht setzen, z.B. nach Homing
+  void setCurrentPosition(int32_t x, int32_t y);
+
+  int32_t currentX() const;
+  int32_t currentY() const;
+
+  bool isRunning() const;
+
+  // Optional blockierend warten
+  void waitUntilFinished();
+
+  // Startet eine koordinierte lineare P2P-Bewegung
+  // Rückgabe false = nichts zu tun oder ungültig
+  bool moveTo(int32_t targetX, int32_t targetY);
+
+  // Convenience: relative Bewegung
+  bool moveBy(int32_t deltaX, int32_t deltaY);
+
+  // Muss regelmäßig aufgerufen werden, wenn du die Planner-Position
+  // nach abgeschlossener Bewegung sauber nachführen willst
+  void update();
+
+private:
+  FastAccelStepper* m_x = nullptr;
+  FastAccelStepper* m_y = nullptr;
+
+  int32_t m_currentX = 0;
+  int32_t m_currentY = 0;
+  int32_t m_targetX = 0;
+  int32_t m_targetY = 0;
+
+  uint32_t m_baseSpeedHz = 2000;
+  uint32_t m_baseAccel = 4000;
+  uint32_t m_minSpeedHz = 1;
+  uint32_t m_minAccel = 1;
+
+  static uint32_t scaledValue(uint32_t base, float scale, uint32_t distance);
+};

src/Functions.cpp

@@ -102,13 +102,13 @@ void setNodeCount()
 
 void nodeCountIncrement()
 {
-	g_uiNodeCount = g_uiNodeCount++;
+	g_uiNodeCount++;
 	sendAck();
 }
 
 void nodeCountDecrement()
 {
-	g_uiNodeCount = g_uiNodeCount--;
+	g_uiNodeCount--;
 	sendAck();
 }
 

src/Helper_Functions.cpp

@@ -1,13 +1,14 @@
 #include "EggDuino.h"
+#include "motion_planner.h"
 
 namespace
 {
 #ifdef ESP32
-constexpr char kDeviceNamePrefix[] = "EggBot_";
+   constexpr char kDeviceNamePrefix[] = "EggBot_";
 #endif
 
-int clampServoAngle(int angle)
-{
+   int clampServoAngle(int angle)
+   {
       if (angle < 0)
       {
          return 0;
@@ -17,10 +18,10 @@ int clampServoAngle(int angle)
          return 180;
       }
       return angle;
-}
+   }
 
-uint_fast16_t servoSpeedFromRate(int rate)
-{
+   uint_fast16_t servoSpeedFromRate(int rate)
+   {
       // EBB rate values are implementation-specific. We map them to ServoEasing degrees/second.
       // Higher rate means faster movement.
       if (rate <= 0)
@@ -37,7 +38,7 @@ uint_fast16_t servoSpeedFromRate(int rate)
          speed = 360;
       }
       return (uint_fast16_t)speed;
-}
+   }
 }
 
 #ifdef ESP32
@@ -68,6 +69,8 @@ void updateStepCorrectionFactors()
    fPEN_STEP_CORRECTION = 16.0f / (float)g_iPenMicrostep;
 }
 
+XYMotionPlanner *g_pPlanner = nullptr;
+
 void initHardware()
 {
    if (!initConfigStore())
@@ -104,6 +107,12 @@ void initHardware()
    }
 
    motorsOff();
+
+   g_pPlanner = new XYMotionPlanner(g_pStepperPen, g_pStepperRotate);
+   g_pPlanner->setBaseLimits(4000, g_iMaxAcclSpeed);
+   g_pPlanner->setMinimums(50, 100);
+   g_pPlanner->setCurrentPosition(0, 0);
+
    g_iPenState = clampServoAngle(g_iPenState);
    penServo.attach(g_iServoPin, g_iPenState);
    penServo.setEasingType(EASE_QUADRATIC_IN_OUT);
@@ -164,6 +173,7 @@ void motorsOn()
 }
 
 void toggleMotors()
+
 {
    Log(__FUNCTION__);
    if (g_bMotorsEnabled)
@@ -209,54 +219,28 @@ void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB)
       motorsOn();
    }
 
-	if ((1 == fROT_STEP_CORRECTION) && (1 == fPEN_STEP_CORRECTION))
-	{ // if coordinatessystems are identical
-		// set Coordinates and Speed
-		g_pStepperRotate->setSpeedInTicks(abs((float)rotStepsEBB * (float)1000 / (float)duration));
-		g_pStepperRotate->move(rotStepsEBB);
-
-		g_pStepperPen->setSpeedInTicks(abs((float)penStepsEBB * (float)1000 / (float)duration));
-		g_pStepperPen->move(penStepsEBB);
-	}
-	else
-	{
-		// incoming EBB-Steps will be multiplied by 16, then Integer-maths is done, result will be divided by 16
-		//  This make thinks here really complicated, but floating point-math kills performance and memory, believe me... I tried...
-		long rotSteps = ((long)rotStepsEBB * 16 / fROT_STEP_CORRECTION) + (long)g_iRotStepError; // correct incoming EBB-Steps to our microstep-Setting and multiply  by 16 to avoid floatingpoint...
-		long penSteps = ((long)penStepsEBB * 16 / fPEN_STEP_CORRECTION) + (long)g_iPenStepError;
-
-		int rotStepsToGo = (int)(rotSteps / 16); // Calc Steps to go, which are possible on our machine
-		int penStepsToGo = (int)(penSteps / 16);
-
-		g_iRotStepError = (long)rotSteps - ((long)rotStepsToGo * (long)16); // calc Position-Error, if there is one
-		g_iPenStepError = (long)penSteps - ((long)penStepsToGo * (long)16);
-
-		long temp_rotSpeed = ((long)rotStepsToGo * (long)1000 / (long)duration); // calc Speed in Integer Math
-		long temp_penSpeed = ((long)penStepsToGo * (long)1000 / (long)duration);
-
-		float rotSpeed = (float)abs(temp_rotSpeed); // type cast
-		float penSpeed = (float)abs(temp_penSpeed);
-
-		// set Coordinates and Speed
-		g_pStepperRotate->setSpeedInTicks(rotSpeed);
-		g_pStepperRotate->move(rotStepsToGo);
-
-		g_pStepperPen->setSpeedInTicks(penSpeed);
-		g_pStepperPen->move(penStepsToGo);
-	}
+   g_pPlanner->moveBy(penStepsEBB / fPEN_STEP_CORRECTION, rotStepsEBB / fROT_STEP_CORRECTION);
 }
 
 void moveOneStep()
 {
    Log("moveOneStep");
-	while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning());
+   while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning() || g_pPlanner->isRunning())
+   {
+      g_pPlanner->update();
+   }
+   g_pPlanner->update();
    Log("done");
 }
 
 void moveToDestination()
 {
    Log("moveToDestination");
-	while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning());
+   while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning() || g_pPlanner->isRunning())
+   {
+      g_pPlanner->update();
+   }
+   g_pPlanner->update();
    Log("done");
 }
 

src/motion_planner.cpp

@@ -0,0 +1,141 @@
+#include "motion_planner.h"
+
+XYMotionPlanner::XYMotionPlanner(FastAccelStepper* stepperX, FastAccelStepper* stepperY)
+    : m_x(stepperX), m_y(stepperY) {}
+
+void XYMotionPlanner::setBaseLimits(uint32_t maxSpeedHz, uint32_t maxAccelStepsPerS2) {
+  m_baseSpeedHz = max<uint32_t>(1, maxSpeedHz);
+  m_baseAccel = max<uint32_t>(1, maxAccelStepsPerS2);
+}
+
+void XYMotionPlanner::setMinimums(uint32_t minSpeedHz, uint32_t minAccelStepsPerS2) {
+  m_minSpeedHz = minSpeedHz;
+  m_minAccel = minAccelStepsPerS2;
+}
+
+void XYMotionPlanner::setCurrentPosition(int32_t x, int32_t y) {
+  m_currentX = x;
+  m_currentY = y;
+  m_targetX = x;
+  m_targetY = y;
+  if (m_x) {
+    m_x->setCurrentPosition(x);
+  }
+  if (m_y) {
+    m_y->setCurrentPosition(y);
+  }
+}
+
+int32_t XYMotionPlanner::currentX() const { return m_currentX; }
+
+int32_t XYMotionPlanner::currentY() const { return m_currentY; }
+
+bool XYMotionPlanner::isRunning() const {
+  bool rx = m_x ? m_x->isRunning() : false;
+  bool ry = m_y ? m_y->isRunning() : false;
+  return rx || ry;
+}
+
+void XYMotionPlanner::waitUntilFinished() {
+  while (isRunning()) {
+    delay(1);
+  }
+  m_currentX = m_targetX;
+  m_currentY = m_targetY;
+}
+
+bool XYMotionPlanner::moveTo(int32_t targetX, int32_t targetY) {
+  if ((m_x == nullptr) || (m_y == nullptr)) {
+    return false;
+  }
+
+  // Keep planner state in sync when the previous move already ended,
+  // but update() has not been called by the caller yet.
+  update();
+
+  m_targetX = targetX;
+  m_targetY = targetY;
+
+  int32_t dxSigned = m_targetX - m_currentX;
+  int32_t dySigned = m_targetY - m_currentY;
+
+  uint32_t dx = (dxSigned >= 0) ? dxSigned : -dxSigned;
+  uint32_t dy = (dySigned >= 0) ? dySigned : -dySigned;
+
+  if ((dx == 0) && (dy == 0)) {
+    return false;
+  }
+
+  uint32_t dmax = max(dx, dy);
+
+  // Skalierungsfaktoren relativ zur dominanten Achse
+  float sx = (float)dx / (float)dmax;
+  float sy = (float)dy / (float)dmax;
+
+  // Dominante Achse bekommt die Basiswerte
+  uint32_t speedX = scaledValue(m_baseSpeedHz, sx, dx);
+  uint32_t speedY = scaledValue(m_baseSpeedHz, sy, dy);
+  uint32_t accelX = scaledValue(m_baseAccel, sx, dx);
+  uint32_t accelY = scaledValue(m_baseAccel, sy, dy);
+
+  // Kurze Achsen mit Bewegung sollen nicht auf 0 fallen
+  if (dx > 0) {
+    speedX = max(speedX, m_minSpeedHz);
+    accelX = max(accelX, m_minAccel);
+  }
+  if (dy > 0) {
+    speedY = max(speedY, m_minSpeedHz);
+    accelY = max(accelY, m_minAccel);
+  }
+
+  // FastAccelStepper: speed in steps/s, accel in steps/s²
+  if (dx > 0) {
+    m_x->setSpeedInHz(speedX);
+    m_x->setAcceleration(accelX);
+  }
+  if (dy > 0) {
+    m_y->setSpeedInHz(speedY);
+    m_y->setAcceleration(accelY);
+  }
+
+  // Bewegungen anstoßen
+  // Reihenfolge ist hier meist ausreichend.
+  // Für "noch synchroner" müsste man tiefer in die Queue-API gehen.
+  if (dx > 0) {
+    m_x->moveTo(m_targetX);
+  }
+  if (dy > 0) {
+    m_y->moveTo(m_targetY);
+  }
+
+  return true;
+}
+
+bool XYMotionPlanner::moveBy(int32_t deltaX, int32_t deltaY) {
+  update();
+
+  // Chain relative moves reliably:
+  // - if running, append relative to the current planned target
+  // - if idle, use the current position
+  const int32_t baseX = isRunning() ? m_targetX : m_currentX;
+  const int32_t baseY = isRunning() ? m_targetY : m_currentY;
+  return moveTo(baseX + deltaX, baseY + deltaY);
+}
+
+void XYMotionPlanner::update() {
+  if (!isRunning()) {
+    m_currentX = m_targetX;
+    m_currentY = m_targetY;
+  }
+}
+
+uint32_t XYMotionPlanner::scaledValue(uint32_t base, float scale, uint32_t distance) {
+  if (distance == 0) {
+    return 0;
+  }
+  float v = (float)base * scale;
+  if (v < 1.0f) {
+    v = 1.0f;
+  }
+  return (uint32_t)(v + 0.5f);
+}