implement ramp-ups and ramp-downs for motor
This commit is contained in:
Leonetienne
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
+226
-24
@@ -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 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<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];
|
||||
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<double> 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<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;
|
||||
}
|
||||
}
|
||||
|
||||
+41
-13
@@ -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 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<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 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;
|
||||
};
|
||||
|
||||
+2
-45
@@ -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");
|
||||
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();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user