implement ramp-ups and ramp-downs for motor

MotionCore/MainLoop.cpp

@@ -1,5 +1,68 @@
 #include "MainLoop.h"
+#include <iostream>
+#include <thread>
+#include <chrono>
+
+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();
+    }
 }

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

MotionCore/Motor.cpp

@@ -1,67 +1,269 @@
 #include "Motor.h"
+#include <iostream>
 #include <algorithm>
 
+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 mechanicalMinPosition,
-	const double mechanicalMaxDeg,
+	const double mechanicalMaxPosition,
-	const double homePosition
+	const double homePosition,
+	const double initialSpeed
 ) noexcept:
 	index(index),
 	serialBus(serialBus),
 	currentPosition(initialPosition),
-	mechanicalMinDeg(mechanicalMinDeg),
+	targetPosition(currentPosition),
-	mechanicalMaxDeg(mechanicalMaxDeg),
+	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),
+	targetPosition(other.targetPosition),
-	mechanicalMaxDeg(other.mechanicalMaxDeg),
+	moveStartPosition(other.moveStartPosition),
-	homePosition(other.homePosition)
+	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<double> 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];
+	if (speed.has_value()) {
-	snprintf(buffer, sizeof(buffer), "MA %d %d", (int)index, (int)theta);
+		setSpeed(speed.value());
-	bool result = serialBus.sendl(buffer);
+	}
+
+	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<double> speed) noexcept
+{
+	if (disabled) {
+		std::cerr << "Trying to act on disabled motor object!" << std::endl;
 		return false;
 	}
 
-	char buffer[32];
+	if (speed.has_value()) {
-	snprintf(buffer, sizeof(buffer), "MA %d %d", (int)index, (int)acuteTarget);
+		setSpeed(speed.value());
-	bool result = serialBus.sendl(buffer);
+	}
+
+	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<double> 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;
+	}
 }

MotionCore/Motor.h

@@ -1,36 +1,64 @@
 #pragma once
 #include "OSerialBus.h"
+#include <optional>
 
 class Motor
 {
 public:
 	Motor(
-		const std::size_t index,
+		const uint8_t index,
 		OSerialBus& serialBus,
 		const double initialPosition,
-		const double mechanicalMinDeg,
+		const double mechanicalMinPosition,
-		const double mechanicalMaxDeg,
+		const double mechanicalMaxPosition,
-		const double homePosition
+		const double homePosition,
+		const double initialSpeed = 0.5
 	) noexcept;
 
 	Motor(const Motor& other) = delete;
 	Motor(Motor&& other) noexcept;
 
-	bool moveTo(const double theta) noexcept;
+	// Will move the motor immediately to the desired position (don't do that)
-	bool moveBy(const double theta) noexcept;
+	bool moveToImmediate(const double theta) noexcept;
-	bool moveHome() noexcept;
+	bool moveTo(const double theta, const std::optional<double> speed) noexcept;
+	bool moveBy(const double theta, const std::optional<double> speed) noexcept;
+	bool moveByImmediate(const double theta) noexcept;
+	bool moveHome(const std::optional<double> 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 getTargetPosition() const { return targetPosition; };
-	double getMechanicalMaxDeg() const { return mechanicalMaxDeg; };
+	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;
+	double currentPosition; // we are here
-	const double mechanicalMinDeg;
+	double moveStartPosition; // last move instruction started here
-	const double mechanicalMaxDeg;
+	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;
 };

MotionCore/main.cpp

@@ -1,51 +1,8 @@
-#include <iostream>
+
 #include "MainLoop.h"
-#include "OSerialBus.h"
-#include "Motor.h"
-#include <thread>
-#include <chrono>
 
 int main()
 {
-    OSerialBus bus(L"COM4");
+    MainLoop& mainloop = MainLoop::getInstance();
-    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.run();
 }