diff --git a/include/motion_planner.h b/include/motion_planner.h new file mode 100644 index 0000000..7a25e18 --- /dev/null +++ b/include/motion_planner.h @@ -0,0 +1,51 @@ +#include +#include + +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); +}; diff --git a/src/Functions.cpp b/src/Functions.cpp index 975f9e9..527980b 100644 --- a/src/Functions.cpp +++ b/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(); } diff --git a/src/Helper_Functions.cpp b/src/Helper_Functions.cpp index 445489f..06f6b62 100644 --- a/src/Helper_Functions.cpp +++ b/src/Helper_Functions.cpp @@ -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(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(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; } diff --git a/src/motion_planner.cpp b/src/motion_planner.cpp new file mode 100644 index 0000000..e84da00 --- /dev/null +++ b/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(1, maxSpeedHz); + m_baseAccel = max(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); +}