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

4
src/Functions.cpp
View File

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

338
src/Helper_Functions.cpp
View File

@@ -1,138 +1,147 @@
#include "EggDuino.h"
#include "motion_planner.h"
namespace
{
#ifdef ESP32
constexpr char kDeviceNamePrefix[] = "EggBot_";
constexpr char kDeviceNamePrefix[] = "EggBot_";
#endif
int clampServoAngle(int angle)
{
if (angle < 0)
{
return 0;
}
if (angle > 180)
{
return 180;
}
return angle;
}
int clampServoAngle(int angle)
{
if (angle < 0)
{
return 0;
}
if (angle > 180)
{
return 180;
}
return angle;
}
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)
{
return 70;
}
int speed = 20 + (rate / 2);
if (speed < 10)
{
speed = 10;
}
if (speed > 360)
{
speed = 360;
}
return (uint_fast16_t)speed;
}
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)
{
return 70;
}
int speed = 20 + (rate / 2);
if (speed < 10)
{
speed = 10;
}
if (speed > 360)
{
speed = 360;
}
return (uint_fast16_t)speed;
}
}
#ifdef ESP32
void buildDeviceName(char *nameBuffer, size_t bufferSize)
{
if ((nameBuffer == NULL) || (bufferSize == 0))
{
return;
}
if ((nameBuffer == NULL) || (bufferSize == 0))
{
return;
}
// 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);
snprintf(nameBuffer, bufferSize, "%s%06X", kDeviceNamePrefix, chipId);
// 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);
snprintf(nameBuffer, bufferSize, "%s%06X", kDeviceNamePrefix, chipId);
}
#endif
void updateStepCorrectionFactors()
{
if (g_iRotMicrostep <= 0)
{
g_iRotMicrostep = kDefaultRotMicrostep;
}
if (g_iPenMicrostep <= 0)
{
g_iPenMicrostep = kDefaultPenMicrostep;
}
fROT_STEP_CORRECTION = 16.0f / (float)g_iRotMicrostep;
fPEN_STEP_CORRECTION = 16.0f / (float)g_iPenMicrostep;
if (g_iRotMicrostep <= 0)
{
g_iRotMicrostep = kDefaultRotMicrostep;
}
if (g_iPenMicrostep <= 0)
{
g_iPenMicrostep = kDefaultPenMicrostep;
}
fROT_STEP_CORRECTION = 16.0f / (float)g_iRotMicrostep;
fPEN_STEP_CORRECTION = 16.0f / (float)g_iPenMicrostep;
}
XYMotionPlanner *g_pPlanner = nullptr;
void initHardware()
{
if (!initConfigStore())
{
g_iPenUpPos = 5;
g_iPenDownPos = 20;
}
updateStepCorrectionFactors();
g_iPenState = g_iPenDownPos;
if (!initConfigStore())
{
g_iPenUpPos = 5;
g_iPenDownPos = 20;
}
updateStepCorrectionFactors();
g_iPenState = g_iPenDownPos;
g_stepEngine.init();
g_pStepperRotate = g_stepEngine.stepperConnectToPin(g_iRotStepPin);
if (g_pStepperRotate)
{
// rotMotor.setMaxSpeed(2000.0);
// rotMotor.setAcceleration(10000.0);
g_stepEngine.init();
g_pStepperRotate = g_stepEngine.stepperConnectToPin(g_iRotStepPin);
if (g_pStepperRotate)
{
// rotMotor.setMaxSpeed(2000.0);
// rotMotor.setAcceleration(10000.0);
g_pStepperRotate->setDirectionPin(g_iRotDirPin);
g_pStepperRotate->setEnablePin(g_iRotEnablePin);
g_pStepperRotate->setAcceleration(g_iMaxAcclSpeed);
g_pStepperRotate->setAutoEnable(false);
}
g_pStepperRotate->setDirectionPin(g_iRotDirPin);
g_pStepperRotate->setEnablePin(g_iRotEnablePin);
g_pStepperRotate->setAcceleration(g_iMaxAcclSpeed);
g_pStepperRotate->setAutoEnable(false);
}
// Stepper pen init
g_pStepperPen = g_stepEngine.stepperConnectToPin(g_iPenStepPin);
if (g_pStepperPen)
{
// penMotor.setMaxSpeed(2000.0);
// penMotor.setAcceleration(10000.0);
g_pStepperPen->setDirectionPin(g_iPenDirPin);
g_pStepperPen->setEnablePin(g_iPenEnablePin);
g_pStepperPen->setAcceleration(g_iMaxAcclSpeed);
g_pStepperPen->setAutoEnable(false);
}
// Stepper pen init
g_pStepperPen = g_stepEngine.stepperConnectToPin(g_iPenStepPin);
if (g_pStepperPen)
{
// penMotor.setMaxSpeed(2000.0);
// penMotor.setAcceleration(10000.0);
g_pStepperPen->setDirectionPin(g_iPenDirPin);
g_pStepperPen->setEnablePin(g_iPenEnablePin);
g_pStepperPen->setAcceleration(g_iMaxAcclSpeed);
g_pStepperPen->setAutoEnable(false);
}
motorsOff();
g_iPenState = clampServoAngle(g_iPenState);
penServo.attach(g_iServoPin, g_iPenState);
penServo.setEasingType(EASE_QUADRATIC_IN_OUT);
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);
}
void movePenServoTo(int targetPos)
{
targetPos = clampServoAngle(targetPos);
int currentPos = clampServoAngle(g_iPenState);
if (currentPos == targetPos)
{
return;
}
targetPos = clampServoAngle(targetPos);
int currentPos = clampServoAngle(g_iPenState);
if (currentPos == targetPos)
{
return;
}
uint_fast16_t speed = 0;
speed = servoSpeedFromRate(g_iServoRateDown);
penServo.easeTo(targetPos, speed);
uint_fast16_t speed = 0;
speed = servoSpeedFromRate(g_iServoRateDown);
penServo.easeTo(targetPos, speed);
g_iPenState = targetPos;
g_iPenState = targetPos;
}
void storePenUpPosInEE()
{
saveConfigToFile();
saveConfigToFile();
}
void storePenDownPosInEE()
{
saveConfigToFile();
saveConfigToFile();
}
void sendAck()
@@ -149,118 +158,93 @@ void sendError()
void motorsOff()
{
Log(__FUNCTION__);
g_pStepperPen->disableOutputs();
g_pStepperRotate->disableOutputs();
g_bMotorsEnabled = 0;
Log(__FUNCTION__);
g_pStepperPen->disableOutputs();
g_pStepperRotate->disableOutputs();
g_bMotorsEnabled = 0;
}
void motorsOn()
{
Log(__FUNCTION__);
g_pStepperPen->enableOutputs();
g_pStepperRotate->enableOutputs();
g_bMotorsEnabled = 1;
Log(__FUNCTION__);
g_pStepperPen->enableOutputs();
g_pStepperRotate->enableOutputs();
g_bMotorsEnabled = 1;
}
void toggleMotors()
{
Log(__FUNCTION__);
if (g_bMotorsEnabled)
{
motorsOff();
}
else
{
motorsOn();
}
Log(__FUNCTION__);
if (g_bMotorsEnabled)
{
motorsOff();
}
else
{
motorsOn();
}
}
bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB)
{
char *arg1 = NULL;
char *arg2 = NULL;
char *arg3 = NULL;
arg1 = nextCommandArg();
if (arg1 != NULL)
{
*duration = atoi(arg1);
arg2 = nextCommandArg();
}
if (arg2 != NULL)
{
*penStepsEBB = atoi(arg2);
arg3 = nextCommandArg();
}
if (arg3 != NULL)
{
*rotStepsEBB = atoi(arg3);
char *arg1 = NULL;
char *arg2 = NULL;
char *arg3 = NULL;
arg1 = nextCommandArg();
if (arg1 != NULL)
{
*duration = atoi(arg1);
arg2 = nextCommandArg();
}
if (arg2 != NULL)
{
*penStepsEBB = atoi(arg2);
arg3 = nextCommandArg();
}
if (arg3 != NULL)
{
*rotStepsEBB = atoi(arg3);
return true;
}
return true;
}
return false;
return false;
}
void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB)
{
if (!g_bMotorsEnabled)
{
motorsOn();
}
if (!g_bMotorsEnabled)
{
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());
Log("done");
Log("moveOneStep");
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());
Log("done");
Log("moveToDestination");
while (g_pStepperPen->isRunning() || g_pStepperRotate->isRunning() || g_pPlanner->isRunning())
{
g_pPlanner->update();
}
g_pPlanner->update();
Log("done");
}
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);
}