leonetienne/MotionCore
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix issues in motor driver and and modify movement example

Browse Source
This commit is contained in:
Leonetienne
2025-12-08 19:52:27 +01:00
parent 885ef00b27
commit e48068b15f
9 changed files with 164 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotionCore/Arm.cpp
+10 -25
View File
@@ -8,10 +8,10 @@ Arm::Arm(const std::wstring& comPort) noexcept:
Vector3d::Y,
Vector3d::Up,
100.0, //mm
0.0,
125.0,
30.0,
180.0,
0,
186.0,
10.0,
270.0,
0.0,
nullptr
),
@@ -20,10 +20,10 @@ Arm::Arm(const std::wstring& comPort) noexcept:
Vector3d::X,
Vector3d::Forward,
72.0, //mm
1.0,
48.0,
1,
71.0,
0.0,
120.0,
181.0,
0.0,
&j0
),
@@ -32,10 +32,10 @@ Arm::Arm(const std::wstring& comPort) noexcept:
Vector3d::X,
Vector3d::Forward,
95.0, //mm
2.0,
115.0,
2,
171.0,
0.0,
180.0,
280.0,
0.0,
&j1
)
@@ -61,21 +61,6 @@ bool Arm::assumeHomePose()
return true;
}
bool Arm::moveJ0(double theta)
{
return j0.moveTo(theta, std::nullopt);
}
bool Arm::moveJ1(double theta)
{
return j1.moveTo(theta, std::nullopt);
}
bool Arm::moveJ2(double theta)
{
return j2.moveTo(theta, std::nullopt);
}
Arm::~Arm()
{
oSerialBus.close();
MotionCore/Arm.h
+8 -3
View File
@@ -13,9 +13,14 @@ public:
bool assumeHomePoseImmediate();
bool assumeHomePose();
bool moveJ0(double theta);
bool moveJ1(double theta);
bool moveJ2(double theta);
ArmSegment& getJ0() { return j0; };
const ArmSegment& getJ0() const { return j0; };
ArmSegment& getJ1() { return j1; };
const ArmSegment& getJ1() const { return j1; };
ArmSegment& getJ2() { return j2; };
const ArmSegment& getJ2() const { return j2; };
void update(double frametime);
MotionCore/ArmSegment.cpp
+3 -3
View File
@@ -24,7 +24,7 @@ ArmSegment::ArmSegment(
virtualToMechanicalAngle(homeAngle),
mechanicalMinAngle,
mechanicalMaxAngle,
0.5
1.0
),
parent(parent)
{
@@ -104,12 +104,12 @@ void ArmSegment::update(const double frametime)
motor.update(frametime);
}
double ArmSegment::virtualToMechanicalAngle(const double virtualAngle)
double ArmSegment::virtualToMechanicalAngle(const double virtualAngle) const noexcept
{
return virtualAngle + mechanicalToVirtualAngleOffset;
}
double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle)
double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle) const noexcept
{
return hardwareAngle - mechanicalToVirtualAngleOffset;
}
MotionCore/ArmSegment.h
+5 -5
View File
@@ -46,16 +46,16 @@ public:
void update(const double frametime);
void setSpeed(const double speed) noexcept { motor.setSpeed(speed); };
double getMechanicalMinAngle() const { return motor.getMechanicalMinPosition(); };
double getMechanicalMaxAngle() const { return motor.getMechanicalMaxPosition(); };
double getHomeANgle() const noexcept { return homeAngle; };
double getMinAngle() const noexcept { return mechanicalToVirtualAngle(motor.getMechanicalMinPosition()); };
double getMaxAngle() const noexcept { return mechanicalToVirtualAngle(motor.getMechanicalMaxPosition()); };
double getHomeAngle() const noexcept { return homeAngle; };
double getSpeed() const noexcept { return motor.getSpeed(); };
ArmSegment* getParent() { return parent; };
ArmSegment const* getParent() const { return parent; };
private:
double virtualToMechanicalAngle(const double virtualAngle);
double mechanicalToVirtualAngle(const double hardwareAngle);
double virtualToMechanicalAngle(const double virtualAngle) const noexcept;
double mechanicalToVirtualAngle(const double hardwareAngle) const noexcept;
Motor motor;
Vector3d rotationAxis;
MotionCore/MainLoop.cpp
+50 -28
View File
@@ -3,6 +3,11 @@
#include <thread>
#include <chrono>
#include <Windows.h>
#include <algorithm>
// Bullshit macros from windows lib
#undef max
#undef min
MainLoop::MainLoop() noexcept:
arm(L"COM4")
@@ -11,6 +16,8 @@ MainLoop::MainLoop() noexcept:
std::cerr << "Unable to move arm to home!" << std::endl;
exit(-1);
}
SetProcessDPIAware();
}
MainLoop& MainLoop::getInstance()
@@ -23,17 +30,15 @@ MainLoop::~MainLoop() noexcept
{
}
static std::chrono::steady_clock::time_point lastDisplay =
std::chrono::high_resolution_clock::now();
void MainLoop::run()
{
static std::chrono::steady_clock::time_point lastBegin =
std::chrono::high_resolution_clock::now();
double j1Position = 0.0; // start at 0
const double speed = 1000.0; // units per second
std::cout << "Main loop started\n";
std::cout << "Use UP and DOWN arrows to move J1\n";
std::cout << "Press ESC to exit\n\n";
const Vector3d armPos(0, 500, 2560);
while (isRunning)
{
@@ -43,37 +48,54 @@ void MainLoop::run()
const double frametime =
(begin - lastBegin).count() * 10e-7; // milliseconds
// -------- INPUT --------
if (GetAsyncKeyState(VK_UP) & 0x8000)
POINT p;
if (GetCursorPos(&p))
{
j1Position += speed * frametime;
arm.moveJ1(j1Position);
Vector3d cursor(
p.x,
-p.y + 1440.0,
0.0
);
// Vector from arm to cursor
Vector3d toCursor = cursor - armPos;
double vx = std::max(toCursor.x, 0.0);
double vy = toCursor.y;
double vz = toCursor.z; // will be negative (0 - 2560)
// Horizontal distance (XZ plane)
double horiz = std::sqrt(vx * vx + vz * vz);
// Yaw (Y): rotation in XZ plane
double yawRad = std::atan2(vx, vz);
double yawDeg = (yawRad * 180.0 / 3.14159) - 150;
// Pitch (X): rotation in vertical plane
double pitchRad = std::atan2(vy, horiz) * 3 - 1;
double pitchDegTotal = (pitchRad * 180.0 / 3.14159);
double pitchDegj1 = 90 - (pitchDegTotal / 2.0) - 45;
double pitchDegj2 = -(pitchDegTotal / 2.0) + 45;
std::cout << "cursor: " << cursor
<< " yaw: " << yawDeg
<< " pitch j1: " << pitchDegj1 << std::endl
<< " pitch j2: " << pitchDegj2 << std::endl;
// Send yaw/pitch to your arm here...
// arm.setYaw(yawRad);
// arm.setPitch(pitchRad);
arm.getJ0().moveTo(yawDeg, std::nullopt);
arm.getJ1().moveTo(pitchDegj1, std::nullopt);
arm.getJ2().moveTo(pitchDegj2, std::nullopt);
}
if (GetAsyncKeyState(VK_DOWN) & 0x8000)
{
j1Position -= speed * frametime;
arm.moveJ1(j1Position);
}
if (GetAsyncKeyState(VK_ESCAPE) & 0x8000)
{
std::cout << "\nExit requested\n";
if (GetAsyncKeyState(VK_ESCAPE)) {
isRunning = false;
}
arm.update(frametime);
// -------- STATUS OUTPUT --------
std::cout << "\rJ1: " << std::fixed << std::setprecision(3)
<< j1Position
<< " | Frame time (ms): "
<< std::setprecision(3)
<< frametime
<< std::flush;
lastBegin = std::chrono::high_resolution_clock::now();
}
std::cout << "\nMain loop stopped\n";
}
MotionCore/Motor.cpp
+39 -17
View File
@@ -4,7 +4,8 @@
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
constexpr double SPEED_CORRECTION_FAC = 100000.0;
constexpr double MOVEMENT_COMPARISON_EPSILON = 0.01;
Motor::Motor(
const uint8_t index,
@@ -55,10 +56,10 @@ bool Motor::moveTo(const double theta, const std::optional<double> speed) noexce
return false;
}
moveStartPosition = currentPosition;
targetPosition = theta;
moveStartPosition = currentPosition;
return false;
return true;
}
bool Motor::moveToImmediate(const double theta) noexcept
@@ -99,10 +100,10 @@ bool Motor::moveBy(const double theta, const std::optional<double> speed) noexce
return false;
}
moveStartPosition = moveStartPosition;
targetPosition = acuteTarget;
moveStartPosition = currentPosition;
return false;
return true;
}
bool Motor::moveByImmediate(const double theta) noexcept
@@ -200,16 +201,27 @@ bool Motor::isPositionInMechanicalRange(const double pos) const noexcept
void Motor::handleMovement(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;
}
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, mechanicalMinPosition, mechanicalMaxPosition);
if (sendMACommand(nextPositionStep)) {
currentPosition = nextPositionStep;
}
}
@@ -223,14 +235,14 @@ bool Motor::sendMACommand(const double targetPos) noexcept
double Motor::calculateCurrentSpeed() const noexcept
{
if (targetPosition == moveStartPosition) {
return 0.0;
return speedLow;
}
auto smoothstep = [](double x) noexcept {
return x * x * (3.0 - 2.0 * x);
};
const double t = ((currentPosition - moveStartPosition) / (targetPosition - moveStartPosition));
const double t = getCurrentMovementProgress();
// Accelleration
if (t < RAMPUP_RECOVERY_SPEED) {
@@ -244,3 +256,13 @@ double Motor::calculateCurrentSpeed() const noexcept
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;
}
MotionCore/Motor.h
+9 -7
View File
@@ -33,14 +33,14 @@ public:
void update(const double frametime);
std::size_t getIndex() const { return index; };
double getCurrentPosition() const { return currentPosition; };
double getTargetPosition() const { return targetPosition; };
double getSpeed() const { return targetSpeed; };
std::size_t getIndex() const noexcept { return index; };
double getCurrentPosition() const noexcept { return currentPosition; };
double getTargetPosition() const noexcept { return targetPosition; };
double getSpeed() const noexcept { return targetSpeed; };
void setSpeed(const double speedTarget) noexcept;
double getMechanicalMinPosition() const { return mechanicalMinPosition; };
double getMechanicalMaxPosition() const { return mechanicalMaxPosition; };
double getPaused() const { return paused; };
double getMechanicalMinPosition() const noexcept { return mechanicalMinPosition; };
double getMechanicalMaxPosition() const noexcept { return mechanicalMaxPosition; };
double getPaused() const noexcept { return paused; };
bool isPositionInMechanicalRange(const double pos) const noexcept;
private:
@@ -50,6 +50,8 @@ private:
bool sendMACommand(const double targetPos) noexcept;
// Will calculate the current speed for rampup based on currentPosition, moveStartPosition, and targetPosition
double calculateCurrentSpeed() const noexcept;
double getCurrentMovementProgress() const noexcept;
bool isCurrentlyMoving() const noexcept;
const uint8_t index;
OSerialBus& serialBus;
MotionCore/Vector3d.cpp
+19 -19
View File
@@ -13,17 +13,17 @@ const Vector3d Vector3d::X { 1.0, 0.0, 0.0 };
const Vector3d Vector3d::Y { 0.0, 1.0, 0.0 };
const Vector3d Vector3d::Z { 0.0, 0.0, 1.0 };
Vector3d::Vector3d()
Vector3d::Vector3d() noexcept
: x(0.0), y(0.0), z(0.0)
{
}
Vector3d::Vector3d(double x, double y, double z)
Vector3d::Vector3d(double x, double y, double z) noexcept
: x(x), y(y), z(z)
{
}
Vector3d& Vector3d::operator=(const Vector3d& other)
Vector3d& Vector3d::operator=(const Vector3d& other) noexcept
{
if (this != &other)
{
@@ -45,27 +45,27 @@ Vector3d& Vector3d::operator=(Vector3d&& other) noexcept
return *this;
}
Vector3d Vector3d::operator+(const Vector3d& rhs) const
Vector3d Vector3d::operator+(const Vector3d& rhs) const noexcept
{
return { x + rhs.x, y + rhs.y, z + rhs.z };
}
Vector3d Vector3d::operator-(const Vector3d& rhs) const
Vector3d Vector3d::operator-(const Vector3d& rhs) const noexcept
{
return { x - rhs.x, y - rhs.y, z - rhs.z };
}
Vector3d Vector3d::operator*(double scalar) const
Vector3d Vector3d::operator*(double scalar) const noexcept
{
return { x * scalar, y * scalar, z * scalar };
}
Vector3d Vector3d::operator/(double scalar) const
Vector3d Vector3d::operator/(double scalar) const noexcept
{
return { x / scalar, y / scalar, z / scalar };
}
Vector3d& Vector3d::operator+=(const Vector3d& rhs)
Vector3d& Vector3d::operator+=(const Vector3d& rhs) noexcept
{
x += rhs.x;
y += rhs.y;
@@ -73,7 +73,7 @@ Vector3d& Vector3d::operator+=(const Vector3d& rhs)
return *this;
}
Vector3d& Vector3d::operator-=(const Vector3d& rhs)
Vector3d& Vector3d::operator-=(const Vector3d& rhs) noexcept
{
x -= rhs.x;
y -= rhs.y;
@@ -81,7 +81,7 @@ Vector3d& Vector3d::operator-=(const Vector3d& rhs)
return *this;
}
Vector3d& Vector3d::operator*=(double scalar)
Vector3d& Vector3d::operator*=(double scalar) noexcept
{
x *= scalar;
y *= scalar;
@@ -89,7 +89,7 @@ Vector3d& Vector3d::operator*=(double scalar)
return *this;
}
Vector3d& Vector3d::operator/=(double scalar)
Vector3d& Vector3d::operator/=(double scalar) noexcept
{
x /= scalar;
y /= scalar;
@@ -97,12 +97,12 @@ Vector3d& Vector3d::operator/=(double scalar)
return *this;
}
bool Vector3d::operator==(const Vector3d& rhs) const
bool Vector3d::operator==(const Vector3d& rhs) const noexcept
{
return x == rhs.x && y == rhs.y && z == rhs.z;
}
bool Vector3d::operator!=(const Vector3d& rhs) const
bool Vector3d::operator!=(const Vector3d& rhs) const noexcept
{
return !(*this == rhs);
}
@@ -113,22 +113,22 @@ std::ostream& operator<<(std::ostream& os, const Vector3d& v)
return os;
}
double Vector3d::dot(const Vector3d& rhs) const
double Vector3d::dot(const Vector3d& rhs) const noexcept
{
return x * rhs.x + y * rhs.y + z * rhs.z;
}
double Vector3d::magnitude() const
double Vector3d::magnitude() const noexcept
{
return std::sqrt(x * x + y * y + z * z);
}
double Vector3d::magnitudeSquared() const
double Vector3d::magnitudeSquared() const noexcept
{
return x * x + y * y + z * z;
}
Vector3d Vector3d::cross(const Vector3d& rhs) const
Vector3d Vector3d::cross(const Vector3d& rhs) const noexcept
{
return {
y * rhs.z - z * rhs.y,
@@ -137,7 +137,7 @@ Vector3d Vector3d::cross(const Vector3d& rhs) const
};
}
Vector3d Vector3d::normalized() const
Vector3d Vector3d::normalized() const noexcept
{
double m = magnitude();
if (m == 0.0)
@@ -147,7 +147,7 @@ Vector3d Vector3d::normalized() const
return { x * inv, y * inv, z * inv };
}
void Vector3d::normalize()
void Vector3d::normalize() noexcept
{
double m = magnitude();
if (m == 0.0)
MotionCore/Vector3d.h
+19 -19
View File
@@ -10,37 +10,37 @@ public:
double z;
public:
Vector3d();
Vector3d(double x, double y, double z);
Vector3d() noexcept;
Vector3d(double x, double y, double z) noexcept;
Vector3d(const Vector3d& other) = default;
Vector3d(Vector3d&& other) noexcept = default;
double dot(const Vector3d& rhs) const;
double magnitude() const;
double magnitudeSquared() const;
Vector3d cross(const Vector3d& rhs) const;
Vector3d normalized() const;
void normalize();
double dot(const Vector3d& rhs) const noexcept;
double magnitude() const noexcept;
double magnitudeSquared() const noexcept;
Vector3d cross(const Vector3d& rhs) const noexcept;
Vector3d normalized() const noexcept;
void normalize() noexcept;
Vector3d& operator=(const Vector3d& other);
Vector3d& operator=(const Vector3d& other) noexcept;
Vector3d& operator=(Vector3d&& other) noexcept;
Vector3d operator+(const Vector3d& rhs) const;
Vector3d operator-(const Vector3d& rhs) const;
Vector3d operator+(const Vector3d& rhs) const noexcept;
Vector3d operator-(const Vector3d& rhs) const noexcept;
Vector3d operator*(double scalar) const;
Vector3d operator/(double scalar) const;
Vector3d operator*(double scalar) const noexcept;
Vector3d operator/(double scalar) const noexcept;
Vector3d& operator+=(const Vector3d& rhs);
Vector3d& operator-=(const Vector3d& rhs);
Vector3d& operator+=(const Vector3d& rhs) noexcept;
Vector3d& operator-=(const Vector3d& rhs) noexcept;
Vector3d& operator*=(double scalar);
Vector3d& operator/=(double scalar);
Vector3d& operator*=(double scalar) noexcept;
Vector3d& operator/=(double scalar) noexcept;
bool operator==(const Vector3d& rhs) const;
bool operator!=(const Vector3d& rhs) const;
bool operator==(const Vector3d& rhs) const noexcept;
bool operator!=(const Vector3d& rhs) const noexcept;
public:
static const Vector3d Zero;