Sternenlabor/EggDuino
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Use simple motion planner for movement.

Browse Source
This commit is contained in:
Holger Weber
2026-03-08 17:38:23 +01:00
parent 4e749580d9
commit f22ded6423
4 changed files with 355 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
include/motion_planner.h Normal file
View File

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

4
src/Functions.cpp
View File

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

338
src/Helper_Functions.cpp
View File

@@ -1,138 +1,147 @@
#include "EggDuino.h" #include "EggDuino.h"
#include "motion_planner.h"
namespace namespace
{ {
#ifdef ESP32 #ifdef ESP32
constexpr char kDeviceNamePrefix[] = "EggBot_"; constexpr char kDeviceNamePrefix[] = "EggBot_";
#endif #endif
int clampServoAngle(int angle) int clampServoAngle(int angle)
{ {
if (angle < 0) if (angle < 0)
{ {
return 0; return 0;
} }
if (angle > 180) if (angle > 180)
{ {
return 180; return 180;
} }
return angle; 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. // EBB rate values are implementation-specific. We map them to ServoEasing degrees/second.
// Higher rate means faster movement. // Higher rate means faster movement.
if (rate <= 0) if (rate <= 0)
{ {
return 70; return 70;
} }
int speed = 20 + (rate / 2); int speed = 20 + (rate / 2);
if (speed < 10) if (speed < 10)
{ {
speed = 10; speed = 10;
} }
if (speed > 360) if (speed > 360)
{ {
speed = 360; speed = 360;
} }
return (uint_fast16_t)speed; return (uint_fast16_t)speed;
} }
} }
#ifdef ESP32 #ifdef ESP32
void buildDeviceName(char *nameBuffer, size_t bufferSize) void buildDeviceName(char *nameBuffer, size_t bufferSize)
{ {
if ((nameBuffer == NULL) || (bufferSize == 0)) if ((nameBuffer == NULL) || (bufferSize == 0))
{ {
return; return;
} }
// Tasmota-style chip ID uses the lower 24 bits of the ESP32 efuse MAC. // Tasmota-style chip ID uses the lower 24 bits of the ESP32 efuse MAC.
const uint32_t chipId = static_cast<uint32_t>(ESP.getEfuseMac() & 0xFFFFFFULL); const uint32_t chipId = static_cast<uint32_t>(ESP.getEfuseMac() & 0xFFFFFFULL);
snprintf(nameBuffer, bufferSize, "%s%06X", kDeviceNamePrefix, chipId); snprintf(nameBuffer, bufferSize, "%s%06X", kDeviceNamePrefix, chipId);
} }
#endif #endif
void updateStepCorrectionFactors() void updateStepCorrectionFactors()
{ {
if (g_iRotMicrostep <= 0) if (g_iRotMicrostep <= 0)
{ {
g_iRotMicrostep = kDefaultRotMicrostep; g_iRotMicrostep = kDefaultRotMicrostep;
} }
if (g_iPenMicrostep <= 0) if (g_iPenMicrostep <= 0)
{ {
g_iPenMicrostep = kDefaultPenMicrostep; g_iPenMicrostep = kDefaultPenMicrostep;
} }
fROT_STEP_CORRECTION = 16.0f / (float)g_iRotMicrostep; fROT_STEP_CORRECTION = 16.0f / (float)g_iRotMicrostep;
fPEN_STEP_CORRECTION = 16.0f / (float)g_iPenMicrostep; fPEN_STEP_CORRECTION = 16.0f / (float)g_iPenMicrostep;
} }
XYMotionPlanner *g_pPlanner = nullptr;
void initHardware() void initHardware()
{ {
if (!initConfigStore()) if (!initConfigStore())
{ {
g_iPenUpPos = 5; g_iPenUpPos = 5;
g_iPenDownPos = 20; g_iPenDownPos = 20;
} }
updateStepCorrectionFactors(); updateStepCorrectionFactors();
g_iPenState = g_iPenDownPos; g_iPenState = g_iPenDownPos;
g_stepEngine.init(); g_stepEngine.init();
g_pStepperRotate = g_stepEngine.stepperConnectToPin(g_iRotStepPin); g_pStepperRotate = g_stepEngine.stepperConnectToPin(g_iRotStepPin);
if (g_pStepperRotate) if (g_pStepperRotate)
{ {
// rotMotor.setMaxSpeed(2000.0); // rotMotor.setMaxSpeed(2000.0);
// rotMotor.setAcceleration(10000.0); // rotMotor.setAcceleration(10000.0);
g_pStepperRotate->setDirectionPin(g_iRotDirPin); g_pStepperRotate->setDirectionPin(g_iRotDirPin);
g_pStepperRotate->setEnablePin(g_iRotEnablePin); g_pStepperRotate->setEnablePin(g_iRotEnablePin);
g_pStepperRotate->setAcceleration(g_iMaxAcclSpeed); g_pStepperRotate->setAcceleration(g_iMaxAcclSpeed);
g_pStepperRotate->setAutoEnable(false); g_pStepperRotate->setAutoEnable(false);
} }
// Stepper pen init // Stepper pen init
g_pStepperPen = g_stepEngine.stepperConnectToPin(g_iPenStepPin); g_pStepperPen = g_stepEngine.stepperConnectToPin(g_iPenStepPin);
if (g_pStepperPen) if (g_pStepperPen)
{ {
// penMotor.setMaxSpeed(2000.0); // penMotor.setMaxSpeed(2000.0);
// penMotor.setAcceleration(10000.0); // penMotor.setAcceleration(10000.0);
g_pStepperPen->setDirectionPin(g_iPenDirPin); g_pStepperPen->setDirectionPin(g_iPenDirPin);
g_pStepperPen->setEnablePin(g_iPenEnablePin); g_pStepperPen->setEnablePin(g_iPenEnablePin);
g_pStepperPen->setAcceleration(g_iMaxAcclSpeed); g_pStepperPen->setAcceleration(g_iMaxAcclSpeed);
g_pStepperPen->setAutoEnable(false); g_pStepperPen->setAutoEnable(false);
} }
motorsOff(); motorsOff();
g_iPenState = clampServoAngle(g_iPenState);
penServo.attach(g_iServoPin, g_iPenState); g_pPlanner = new XYMotionPlanner(g_pStepperPen, g_pStepperRotate);
penServo.setEasingType(EASE_QUADRATIC_IN_OUT); 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);
} }
void movePenServoTo(int targetPos) void movePenServoTo(int targetPos)
{ {
targetPos = clampServoAngle(targetPos); targetPos = clampServoAngle(targetPos);
int currentPos = clampServoAngle(g_iPenState); int currentPos = clampServoAngle(g_iPenState);
if (currentPos == targetPos) if (currentPos == targetPos)
{ {
return; return;
} }
uint_fast16_t speed = 0; uint_fast16_t speed = 0;
speed = servoSpeedFromRate(g_iServoRateDown); speed = servoSpeedFromRate(g_iServoRateDown);
penServo.easeTo(targetPos, speed); penServo.easeTo(targetPos, speed);
g_iPenState = targetPos; g_iPenState = targetPos;
} }
void storePenUpPosInEE() void storePenUpPosInEE()
{ {
saveConfigToFile(); saveConfigToFile();
} }
void storePenDownPosInEE() void storePenDownPosInEE()
{ {
saveConfigToFile(); saveConfigToFile();
} }
void sendAck() void sendAck()
@@ -149,118 +158,93 @@ void sendError()
void motorsOff() void motorsOff()
{ {
Log(__FUNCTION__); Log(__FUNCTION__);
g_pStepperPen->disableOutputs(); g_pStepperPen->disableOutputs();
g_pStepperRotate->disableOutputs(); g_pStepperRotate->disableOutputs();
g_bMotorsEnabled = 0; g_bMotorsEnabled = 0;
} }
void motorsOn() void motorsOn()
{ {
Log(__FUNCTION__); Log(__FUNCTION__);
g_pStepperPen->enableOutputs(); g_pStepperPen->enableOutputs();
g_pStepperRotate->enableOutputs(); g_pStepperRotate->enableOutputs();
g_bMotorsEnabled = 1; g_bMotorsEnabled = 1;
} }
void toggleMotors() void toggleMotors()
{ {
Log(__FUNCTION__); Log(__FUNCTION__);
if (g_bMotorsEnabled) if (g_bMotorsEnabled)
{ {
motorsOff(); motorsOff();
} }
else else
{ {
motorsOn(); motorsOn();
} }
} }
bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB) bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB)
{ {
char *arg1 = NULL; char *arg1 = NULL;
char *arg2 = NULL; char *arg2 = NULL;
char *arg3 = NULL; char *arg3 = NULL;
arg1 = nextCommandArg(); arg1 = nextCommandArg();
if (arg1 != NULL) if (arg1 != NULL)
{ {
*duration = atoi(arg1); *duration = atoi(arg1);
arg2 = nextCommandArg(); arg2 = nextCommandArg();
} }
if (arg2 != NULL) if (arg2 != NULL)
{ {
*penStepsEBB = atoi(arg2); *penStepsEBB = atoi(arg2);
arg3 = nextCommandArg(); arg3 = nextCommandArg();
} }
if (arg3 != NULL) if (arg3 != NULL)
{ {
*rotStepsEBB = atoi(arg3); *rotStepsEBB = atoi(arg3);
return true; return true;
} }
return false; return false;
} }
void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB) void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB)
{ {
if (!g_bMotorsEnabled) if (!g_bMotorsEnabled)
{ {
motorsOn(); motorsOn();
} }
if ((1 == fROT_STEP_CORRECTION) && (1 == fPEN_STEP_CORRECTION)) g_pPlanner->moveBy(penStepsEBB / fPEN_STEP_CORRECTION, rotStepsEBB / fROT_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);
}
} }
void moveOneStep() void moveOneStep()
{ {
Log("moveOneStep"); Log("moveOneStep");
while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning()); while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning() || g_pPlanner->isRunning())
Log("done"); {
g_pPlanner->update();
}
g_pPlanner->update();
Log("done");
} }
void moveToDestination() void moveToDestination()
{ {
Log("moveToDestination"); Log("moveToDestination");
while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning()); while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning() || g_pPlanner->isRunning())
Log("done"); {
g_pPlanner->update();
}
g_pPlanner->update();
Log("done");
} }
void setprgButtonState() void setprgButtonState()
{ {
g_bPrgButtonState = 1; g_bPrgButtonState = 1;
} }

141
src/motion_planner.cpp Normal file
View File

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