From d9f81e7c680244ae748aafccb0ab6091972d5d6c Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Sun, 7 Dec 2025 23:28:04 +0100 Subject: [PATCH] implement ramp-ups and ramp-downs for motor --- MotionCore/MainLoop.cpp | 63 ++++++++++ MotionCore/MainLoop.h | 17 +++ MotionCore/Motor.cpp | 250 ++++++++++++++++++++++++++++++++++++---- MotionCore/Motor.h | 54 ++++++--- MotionCore/main.cpp | 47 +------- 5 files changed, 349 insertions(+), 82 deletions(-) diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 5d98500..bb86ac0 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -1,5 +1,68 @@ #include "MainLoop.h" +#include +#include +#include + +MainLoop::MainLoop() noexcept: + oSerialBus(L"COM4"), + j0( + 0, + oSerialBus, + 0, + 30.0, + 180.0, + 125.0 + ), + j1( + 1, + oSerialBus, + 0.0, + 0.0, + 120.0, + 51.0 + ), + j2( + 2, + oSerialBus, + 0.0, + 0.0, + 180.0, + 115.0 + ) +{ + + if (!oSerialBus.isOpen()) { + std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl; + } + + j0.moveHomeImmediate(); + j1.moveHomeImmediate(); + j2.moveHomeImmediate(); +} + +MainLoop& MainLoop::getInstance() +{ + static MainLoop instance; + return instance; +} + +MainLoop::~MainLoop() noexcept +{ + oSerialBus.close(); +} void MainLoop::run() { + static std::chrono::steady_clock::time_point lastBegin = std::chrono::high_resolution_clock::now(); + + while (isRunning) { + std::chrono::steady_clock::time_point begin = std::chrono::high_resolution_clock::now(); + const double frametime = (begin - lastBegin).count() * 10e-7; // in milliseconds + + j0.update(frametime); + j1.update(frametime); + j2.update(frametime); + + lastBegin = std::chrono::high_resolution_clock::now(); + } } diff --git a/MotionCore/MainLoop.h b/MotionCore/MainLoop.h index d4ab0da..a437db5 100755 --- a/MotionCore/MainLoop.h +++ b/MotionCore/MainLoop.h @@ -1,7 +1,24 @@ #pragma once +#include "OSerialBus.h" +#include "Motor.h" +// Singleton-class, use MainLoop::getInstance() to get the only instance of the runtime class MainLoop { public: + static MainLoop& getInstance(); + MainLoop(const MainLoop& other) = delete; + MainLoop(MainLoop&& other) = delete; + ~MainLoop() noexcept; void run(); + +private: + MainLoop() noexcept; + + OSerialBus oSerialBus; + Motor j0; + Motor j1; + Motor j2; + + bool isRunning = true; }; diff --git a/MotionCore/Motor.cpp b/MotionCore/Motor.cpp index 7bc5bee..894067f 100755 --- a/MotionCore/Motor.cpp +++ b/MotionCore/Motor.cpp @@ -1,67 +1,269 @@ #include "Motor.h" +#include #include +constexpr double RAMPUP_REDUCTION_FAC = 0.1; +constexpr double RAMPUP_RECOVERY_SPEED = 0.1; +constexpr double SPEED_CORRECTION_FAC = 0.01; // Otherwhise speed=1.0 would be way too fast + Motor::Motor( - const std::size_t index, + const uint8_t index, OSerialBus& serialBus, const double initialPosition, - const double mechanicalMinDeg, - const double mechanicalMaxDeg, - const double homePosition + const double mechanicalMinPosition, + const double mechanicalMaxPosition, + const double homePosition, + const double initialSpeed ) noexcept: index(index), serialBus(serialBus), currentPosition(initialPosition), - mechanicalMinDeg(mechanicalMinDeg), - mechanicalMaxDeg(mechanicalMaxDeg), + targetPosition(currentPosition), + moveStartPosition(currentPosition), + mechanicalMinPosition(mechanicalMinPosition), + mechanicalMaxPosition(mechanicalMaxPosition), homePosition(homePosition) { + setSpeed(initialSpeed); } Motor::Motor(Motor&& other) noexcept: index(other.index), serialBus(other.serialBus), currentPosition(other.currentPosition), - mechanicalMinDeg(other.mechanicalMinDeg), - mechanicalMaxDeg(other.mechanicalMaxDeg), - homePosition(other.homePosition) + targetPosition(other.targetPosition), + moveStartPosition(other.moveStartPosition), + mechanicalMinPosition(other.mechanicalMinPosition), + mechanicalMaxPosition(other.mechanicalMaxPosition), + homePosition(other.homePosition), + targetSpeed(other.targetSpeed), + speedLow(other.speedLow) { - + // Make moved object inert + other.disabled = true; } -bool Motor::moveTo(const double theta) noexcept +bool Motor::moveTo(const double theta, const std::optional speed) noexcept { - if (theta < mechanicalMinDeg || theta > mechanicalMaxDeg) { + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; return false; } - char buffer[32]; - snprintf(buffer, sizeof(buffer), "MA %d %d", (int)index, (int)theta); - bool result = serialBus.sendl(buffer); + if (speed.has_value()) { + setSpeed(speed.value()); + } + + if (!isPositionInMechanicalRange(theta)) { + return false; + } + + moveStartPosition = currentPosition; + targetPosition = theta; + + return false; +} + +bool Motor::moveToImmediate(const double theta) noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return false; + } + + if (!isPositionInMechanicalRange(theta)) { + return false; + } + + const bool result = sendMACommand(theta); if (result) { currentPosition = theta; + targetPosition = theta; + moveStartPosition = theta; } return result; } -bool Motor::moveBy(const double theta) noexcept -{ - const double acuteTarget = currentPosition + theta; - if (acuteTarget < mechanicalMinDeg || acuteTarget > mechanicalMaxDeg) { +bool Motor::moveBy(const double theta, const std::optional speed) noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; return false; } - char buffer[32]; - snprintf(buffer, sizeof(buffer), "MA %d %d", (int)index, (int)acuteTarget); - bool result = serialBus.sendl(buffer); + if (speed.has_value()) { + setSpeed(speed.value()); + } + + const double acuteTarget = currentPosition + theta; + + if (!isPositionInMechanicalRange(acuteTarget)) { + return false; + } + + moveStartPosition = moveStartPosition; + targetPosition = acuteTarget; + + return false; +} + +bool Motor::moveByImmediate(const double theta) noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return false; + } + + const double acuteTarget = currentPosition + theta; + + if (!isPositionInMechanicalRange(acuteTarget)) { + return false; + } + + const bool result = sendMACommand(acuteTarget); if (result) { + moveStartPosition = currentPosition; currentPosition = acuteTarget; + targetPosition = acuteTarget; } return result; } -bool Motor::moveHome() noexcept +bool Motor::moveHome(const std::optional speed) noexcept { - return moveTo(homePosition); + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return false; + } + + return moveTo(homePosition, speed); +} + +bool Motor::moveHomeImmediate() noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return false; + } + + return moveToImmediate(homePosition); +} + +bool Motor::stopMoving() noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return false; + } + + targetPosition = currentPosition; +} + +void Motor::pauseMoving() noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return; + } + + paused = true; +} + +void Motor::resumeMoving() noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return; + } + + paused = false; +} + +bool Motor::assumeTargetPositionImmediately() noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return false; + } + + return moveToImmediate(targetPosition); +} + + +void Motor::update(const double frametime) +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return; + } + + if (paused) { + return; + } + + handleRampUp(frametime); +} + +void Motor::setSpeed(const double speed) noexcept +{ + this->targetSpeed = speed; + this->speedLow = speed * RAMPUP_REDUCTION_FAC; +} + +bool Motor::isPositionInMechanicalRange(const double pos) const noexcept +{ + if (disabled) { + std::cerr << "Trying to act on disabled motor object!" << std::endl; + return false; + } + + return pos >= mechanicalMinPosition && pos <= mechanicalMaxPosition; +} + +void Motor::handleRampUp(const double frametime) +{ + constexpr double epsilon = 0.1; + if (std::abs(currentPosition - targetPosition) > epsilon) { + const double nextPositionStep = currentPosition + (targetPosition - moveStartPosition) * calculateCurrentSpeed() * SPEED_CORRECTION_FAC; + if (!isPositionInMechanicalRange(nextPositionStep)) { + return; + } + const bool result = sendMACommand(nextPositionStep); + if (result) { + currentPosition = nextPositionStep; + } + } +} + +bool Motor::sendMACommand(const double targetPos) noexcept +{ + char buffer[32]; + snprintf(buffer, sizeof(buffer), "MA %d %d", index, (int)targetPos); + return serialBus.sendl(buffer); +} + +double Motor::calculateCurrentSpeed() const noexcept +{ + if (targetPosition == moveStartPosition) { + return 0.0; + } + + auto smoothstep = [](double x) noexcept { + return x * x * (3.0 - 2.0 * x); + }; + + const double t = ((currentPosition - moveStartPosition) / (targetPosition - moveStartPosition)); + + // Accelleration + if (t < RAMPUP_RECOVERY_SPEED) { + return speedLow + (targetSpeed - speedLow) * smoothstep(t / RAMPUP_RECOVERY_SPEED); + } + // Decceleration + else if (t > 1.0 - RAMPUP_RECOVERY_SPEED) { + return speedLow + (targetSpeed - speedLow) * smoothstep((1.0 - t) / RAMPUP_RECOVERY_SPEED); + } + else { + return targetSpeed; + } } diff --git a/MotionCore/Motor.h b/MotionCore/Motor.h index 8ce8730..3fc3b12 100755 --- a/MotionCore/Motor.h +++ b/MotionCore/Motor.h @@ -1,36 +1,64 @@ #pragma once #include "OSerialBus.h" +#include class Motor { public: Motor( - const std::size_t index, + const uint8_t index, OSerialBus& serialBus, const double initialPosition, - const double mechanicalMinDeg, - const double mechanicalMaxDeg, - const double homePosition + const double mechanicalMinPosition, + const double mechanicalMaxPosition, + const double homePosition, + const double initialSpeed = 0.5 ) noexcept; Motor(const Motor& other) = delete; Motor(Motor&& other) noexcept; - bool moveTo(const double theta) noexcept; - bool moveBy(const double theta) noexcept; - bool moveHome() noexcept; + // Will move the motor immediately to the desired position (don't do that) + bool moveToImmediate(const double theta) noexcept; + bool moveTo(const double theta, const std::optional speed) noexcept; + bool moveBy(const double theta, const std::optional speed) noexcept; + bool moveByImmediate(const double theta) noexcept; + bool moveHome(const std::optional speed) noexcept; + bool moveHomeImmediate() noexcept; + bool stopMoving() noexcept; + void pauseMoving() noexcept; + void resumeMoving() noexcept; + bool assumeTargetPositionImmediately() noexcept; + + void update(const double frametime); std::size_t getIndex() const { return index; }; double getCurrentPosition() const { return currentPosition; }; - double getMechanicalMinDeg() const { return mechanicalMinDeg; }; - double getMechanicalMaxDeg() const { return mechanicalMaxDeg; }; + double getTargetPosition() const { return targetPosition; }; + double getSpeed() const { return targetSpeed; }; + void setSpeed(const double speedTarget) noexcept; + double getMechanicalMinPosition() const { return mechanicalMinPosition; }; + double getMechanicalMaxPosition() const { return mechanicalMaxPosition; }; double getHomePosition() const { return homePosition; }; + double getPaused() const { return paused; }; + bool isPositionInMechanicalRange(const double pos) const noexcept; private: - const std::size_t index; + void handleRampUp(const double frametime); + bool sendMACommand(const double targetPos) noexcept; + // Will calculate the current speed for rampup based on currentPosition, moveStartPosition, and targetPosition + double calculateCurrentSpeed() const noexcept; + + const uint8_t index; OSerialBus& serialBus; - double currentPosition = 0; - const double mechanicalMinDeg; - const double mechanicalMaxDeg; + double currentPosition; // we are here + double moveStartPosition; // last move instruction started here + double targetPosition; // movement should end here + double targetSpeed; // Motor should ideally move at this speed + double speedLow; // End- and startpoint of ramp-up phase + const double mechanicalMinPosition; + const double mechanicalMaxPosition; const double homePosition; + bool disabled = false; + bool paused = false; }; diff --git a/MotionCore/main.cpp b/MotionCore/main.cpp index af6fd8a..1f5521a 100755 --- a/MotionCore/main.cpp +++ b/MotionCore/main.cpp @@ -1,51 +1,8 @@ -#include + #include "MainLoop.h" -#include "OSerialBus.h" -#include "Motor.h" -#include -#include int main() { - OSerialBus bus(L"COM4"); - if (!bus.isOpen()) { - std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl; - return -1; - } - - Motor j0( - 0, - bus, - 125.0, - 30.0, - 180.0, - 125.0 - ); - j0.moveHome(); - - Motor j1( - 1, - bus, - 51.0, - 0.0, - 120.0, - 51.0 - ); - j1.moveHome(); - - Motor j2( - 2, - bus, - 115.0, - 0.0, - 180.0, - 115.0 - ); - j2.moveHome(); - - - bus.close(); - - MainLoop mainloop; + MainLoop& mainloop = MainLoop::getInstance(); mainloop.run(); }