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