MotionCore/MotionCore/Motor.cpp

#include "Motor.h"
#include <iostream>
#include <algorithm>
#include <array>

constexpr double RAMPUP_REDUCTION_FAC = 0.1;
constexpr double RAMPUP_RECOVERY_SPEED = 0.1;
constexpr double SPEED_CORRECTION_FAC = 100000.0;
constexpr double MOVEMENT_COMPARISON_EPSILON = 0.01;

Motor::Motor(
	const uint8_t index,
	OSerialBus& serialBus,
	const double initialPosition,
	const double safeMinPos,
	const double safeMaxPos,
	const double initialSpeed
) noexcept:
	index(index),
	serialBus(serialBus),
	currentPosition(initialPosition),
	targetPosition(currentPosition),
	moveStartPosition(currentPosition),
	safeMinPos(safeMinPos),
	safeMaxPos(safeMaxPos)
{
	setSpeed(initialSpeed);
}

Motor::Motor(Motor&& other) noexcept:
	index(other.index),
	serialBus(other.serialBus),
	currentPosition(other.currentPosition),
	targetPosition(other.targetPosition),
	moveStartPosition(other.moveStartPosition),
	safeMinPos(other.safeMinPos),
	safeMaxPos(other.safeMaxPos),
	targetSpeed(other.targetSpeed),
	speedLow(other.speedLow)
{
	// Make moved object inert
	other.disabled = true;
}

bool Motor::moveTo(const double theta, const std::optional<double> speed) noexcept
{
	if (disabled) {
		std::cerr << "Trying to act on disabled motor object!" << std::endl;
		return false;
	}

	if (speed.has_value()) {
		setSpeed(speed.value());
	}

	if (!isPositionInRange(theta)) {
		return false;
	}

	targetPosition = theta;
	moveStartPosition = currentPosition;

	return true;
}

bool Motor::moveToImmediate(const double theta) noexcept
{
	if (disabled) {
		std::cerr << "Trying to act on disabled motor object!" << std::endl;
		return false;
	}

	if (!isPositionInRange(theta)) {
		return false;
	}

	const bool result = sendMACommand(theta);
	if (result) {
		currentPosition = theta;
		targetPosition = theta;
		moveStartPosition = theta;
	}
	return result;
}


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

	if (speed.has_value()) {
		setSpeed(speed.value());
	}

	const double acuteTarget = currentPosition + theta;

	if (!isPositionInRange(acuteTarget)) {
		return false;
	}

	targetPosition = acuteTarget;
	moveStartPosition = currentPosition;

	return true;
}

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 (!isPositionInRange(acuteTarget)) {
		return false;
	}

	const bool result = sendMACommand(acuteTarget);
	if (result) {
		moveStartPosition = currentPosition;
		currentPosition = acuteTarget;
		targetPosition = acuteTarget;
	}
	return result;
}

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

	handleMovement(frametime);
}

void Motor::setSpeed(const double speed) noexcept
{
	this->targetSpeed = speed;
	this->speedLow = speed * RAMPUP_REDUCTION_FAC;
}

bool Motor::isPositionInRange(const double pos) const noexcept
{
	if (disabled) {
		std::cerr << "Trying to act on disabled motor object!" << std::endl;
		return false;
	}

	return pos >= safeMinPos && pos <= safeMaxPos;
}

void Motor::handleMovement(const double frametime)
{
	const double remaining = targetPosition - currentPosition;
	if (std::abs(remaining) <= MOVEMENT_COMPARISON_EPSILON) {
		currentPosition = targetPosition;
		return;
	}

	const double speed = calculateCurrentSpeed();
	const double maxStep = speed * frametime * SPEED_CORRECTION_FAC;
	double step;

	if (std::abs(remaining) <= std::abs(maxStep)) {
		step = remaining;
	}
	else {
		step = (remaining > 0 ? 1.0 : -1.0) * std::abs(maxStep);
	}

	const double nextPositionStep = std::clamp(currentPosition + step, safeMinPos, safeMaxPos);

	if (sendMACommand(nextPositionStep)) {
		currentPosition = nextPositionStep;
	}
}

bool Motor::sendMACommand(const double targetPos) noexcept
{
	std::array<std::uint8_t, 5> buffer; // 5-byte packets

	buffer[0] = 0x00; // Instruction: Instruction to MOVE ABSOLUTE

	// Send motor index as u16
	buffer[1] = 0x00; // Motor index MSB: Won't have more than 255 motors
	buffer[2] = index; // Motor index LSB, take as-is

	// Send motor degrees as i16
	const std::int16_t deciDegrees = (std::int16_t)(10.0 * targetPos);
	buffer[3] = (deciDegrees >> 8) & 0xFF; // Motor degree MSB
	buffer[4] = deciDegrees & 0xFF; // Motor degree LSB

	return serialBus.send(buffer.data(), buffer.size());
}

double Motor::calculateCurrentSpeed() const noexcept
{
	if (targetPosition == moveStartPosition) {
		return speedLow;
	}

	auto smoothstep = [](double x) noexcept {
		return x * x * (3.0 - 2.0 * x);
	};

	const double t = getCurrentMovementProgress();
	
	// 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;
	}
}

double Motor::getCurrentMovementProgress() const noexcept
{
	return std::clamp((currentPosition - moveStartPosition) / (targetPosition - moveStartPosition), 0.0, 1.0);
}

bool Motor::isCurrentlyMoving() const noexcept
{
	return std::abs(currentPosition - targetPosition) > MOVEMENT_COMPARISON_EPSILON;
}