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::Y,
Vector3d::Up, Vector3d::Up,
100.0, //mm 100.0, //mm
0.0, 0,
125.0, 186.0,
30.0, 10.0,
180.0, 270.0,
0.0, 0.0,
nullptr nullptr
), ),
@@ -20,10 +20,10 @@ Arm::Arm(const std::wstring& comPort) noexcept:
Vector3d::X, Vector3d::X,
Vector3d::Forward, Vector3d::Forward,
72.0, //mm 72.0, //mm
1.0, 1,
48.0, 71.0,
0.0, 0.0,
120.0, 181.0,
0.0, 0.0,
&j0 &j0
), ),
@@ -32,10 +32,10 @@ Arm::Arm(const std::wstring& comPort) noexcept:
Vector3d::X, Vector3d::X,
Vector3d::Forward, Vector3d::Forward,
95.0, //mm 95.0, //mm
2.0, 2,
115.0, 171.0,
0.0, 0.0,
180.0, 280.0,
0.0, 0.0,
&j1 &j1
) )
@@ -61,21 +61,6 @@ bool Arm::assumeHomePose()
return true; 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() Arm::~Arm()
{ {
oSerialBus.close(); oSerialBus.close();
MotionCore/Arm.h
+9 -4
View File
@@ -9,13 +9,18 @@ public:
Arm(const Arm& other) = delete; Arm(const Arm& other) = delete;
Arm(Arm&& other) noexcept = default; Arm(Arm&& other) noexcept = default;
~Arm(); ~Arm();
bool assumeHomePoseImmediate(); bool assumeHomePoseImmediate();
bool assumeHomePose(); bool assumeHomePose();
bool moveJ0(double theta); ArmSegment& getJ0() { return j0; };
bool moveJ1(double theta); const ArmSegment& getJ0() const { return j0; };
bool moveJ2(double theta);
ArmSegment& getJ1() { return j1; };
const ArmSegment& getJ1() const { return j1; };
ArmSegment& getJ2() { return j2; };
const ArmSegment& getJ2() const { return j2; };
void update(double frametime); void update(double frametime);
MotionCore/ArmSegment.cpp
+3 -3
View File
@@ -24,7 +24,7 @@ ArmSegment::ArmSegment(
virtualToMechanicalAngle(homeAngle), virtualToMechanicalAngle(homeAngle),
mechanicalMinAngle, mechanicalMinAngle,
mechanicalMaxAngle, mechanicalMaxAngle,
0.5 1.0
), ),
parent(parent) parent(parent)
{ {
@@ -104,12 +104,12 @@ void ArmSegment::update(const double frametime)
motor.update(frametime); motor.update(frametime);
} }
double ArmSegment::virtualToMechanicalAngle(const double virtualAngle) double ArmSegment::virtualToMechanicalAngle(const double virtualAngle) const noexcept
{ {
return virtualAngle + mechanicalToVirtualAngleOffset; return virtualAngle + mechanicalToVirtualAngleOffset;
} }
double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle) double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle) const noexcept
{ {
return hardwareAngle - mechanicalToVirtualAngleOffset; return hardwareAngle - mechanicalToVirtualAngleOffset;
} }
MotionCore/ArmSegment.h
+5 -5
View File
@@ -46,16 +46,16 @@ public:
void update(const double frametime); void update(const double frametime);
void setSpeed(const double speed) noexcept { motor.setSpeed(speed); }; void setSpeed(const double speed) noexcept { motor.setSpeed(speed); };
double getMechanicalMinAngle() const { return motor.getMechanicalMinPosition(); }; double getMinAngle() const noexcept { return mechanicalToVirtualAngle(motor.getMechanicalMinPosition()); };
double getMechanicalMaxAngle() const { return motor.getMechanicalMaxPosition(); }; double getMaxAngle() const noexcept { return mechanicalToVirtualAngle(motor.getMechanicalMaxPosition()); };
double getHomeANgle() const noexcept { return homeAngle; }; double getHomeAngle() const noexcept { return homeAngle; };
double getSpeed() const noexcept { return motor.getSpeed(); }; double getSpeed() const noexcept { return motor.getSpeed(); };
ArmSegment* getParent() { return parent; }; ArmSegment* getParent() { return parent; };
ArmSegment const* getParent() const { return parent; }; ArmSegment const* getParent() const { return parent; };
private: private:
double virtualToMechanicalAngle(const double virtualAngle); double virtualToMechanicalAngle(const double virtualAngle) const noexcept;
double mechanicalToVirtualAngle(const double hardwareAngle); double mechanicalToVirtualAngle(const double hardwareAngle) const noexcept;
Motor motor; Motor motor;
Vector3d rotationAxis; Vector3d rotationAxis;
MotionCore/MainLoop.cpp
+51 -29
View File
@@ -3,6 +3,11 @@
#include <thread> #include <thread>
#include <chrono> #include <chrono>
#include <Windows.h> #include <Windows.h>
#include <algorithm>
// Bullshit macros from windows lib
#undef max
#undef min
MainLoop::MainLoop() noexcept: MainLoop::MainLoop() noexcept:
arm(L"COM4") arm(L"COM4")
@@ -11,6 +16,8 @@ MainLoop::MainLoop() noexcept:
std::cerr << "Unable to move arm to home!" << std::endl; std::cerr << "Unable to move arm to home!" << std::endl;
exit(-1); exit(-1);
} }
SetProcessDPIAware();
} }
MainLoop& MainLoop::getInstance() MainLoop& MainLoop::getInstance()
@@ -23,18 +30,16 @@ MainLoop::~MainLoop() noexcept
{ {
} }
static std::chrono::steady_clock::time_point lastDisplay =
std::chrono::high_resolution_clock::now();
void MainLoop::run() void MainLoop::run()
{ {
static std::chrono::steady_clock::time_point lastBegin = static std::chrono::steady_clock::time_point lastBegin =
std::chrono::high_resolution_clock::now(); std::chrono::high_resolution_clock::now();
double j1Position = 0.0; // start at 0 const Vector3d armPos(0, 500, 2560);
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";
while (isRunning) while (isRunning)
{ {
std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::time_point begin =
@@ -43,37 +48,54 @@ void MainLoop::run()
const double frametime = const double frametime =
(begin - lastBegin).count() * 10e-7; // milliseconds (begin - lastBegin).count() * 10e-7; // milliseconds
// -------- INPUT -------- POINT p;
if (GetAsyncKeyState(VK_UP) & 0x8000) if (GetCursorPos(&p))
{ {
j1Position += speed * frametime; Vector3d cursor(
arm.moveJ1(j1Position); 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) if (GetAsyncKeyState(VK_ESCAPE)) {
{
j1Position -= speed * frametime;
arm.moveJ1(j1Position);
}
if (GetAsyncKeyState(VK_ESCAPE) & 0x8000)
{
std::cout << "\nExit requested\n";
isRunning = false; isRunning = false;
} }
arm.update(frametime); 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(); 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_REDUCTION_FAC = 0.1;
constexpr double RAMPUP_RECOVERY_SPEED = 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( Motor::Motor(
const uint8_t index, const uint8_t index,
@@ -55,10 +56,10 @@ bool Motor::moveTo(const double theta, const std::optional<double> speed) noexce
return false; return false;
} }
moveStartPosition = currentPosition;
targetPosition = theta; targetPosition = theta;
moveStartPosition = currentPosition;
return false; return true;
} }
bool Motor::moveToImmediate(const double theta) noexcept 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; return false;
} }
moveStartPosition = moveStartPosition;
targetPosition = acuteTarget; targetPosition = acuteTarget;
moveStartPosition = currentPosition;
return false; return true;
} }
bool Motor::moveByImmediate(const double theta) noexcept 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) void Motor::handleMovement(const double frametime)
{ {
constexpr double epsilon = 0.1; const double remaining = targetPosition - currentPosition;
if (std::abs(currentPosition - targetPosition) > epsilon) { if (std::abs(remaining) <= MOVEMENT_COMPARISON_EPSILON) {
const double nextPositionStep = currentPosition + (targetPosition - moveStartPosition) * calculateCurrentSpeed() * SPEED_CORRECTION_FAC; currentPosition = targetPosition;
if (!isPositionInMechanicalRange(nextPositionStep)) { return;
return; }
}
const bool result = sendMACommand(nextPositionStep); const double speed = calculateCurrentSpeed();
if (result) { const double maxStep = speed * frametime * SPEED_CORRECTION_FAC;
currentPosition = nextPositionStep; 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 double Motor::calculateCurrentSpeed() const noexcept
{ {
if (targetPosition == moveStartPosition) { if (targetPosition == moveStartPosition) {
return 0.0; return speedLow;
} }
auto smoothstep = [](double x) noexcept { auto smoothstep = [](double x) noexcept {
return x * x * (3.0 - 2.0 * x); return x * x * (3.0 - 2.0 * x);
}; };
const double t = ((currentPosition - moveStartPosition) / (targetPosition - moveStartPosition)); const double t = getCurrentMovementProgress();
// Accelleration // Accelleration
if (t < RAMPUP_RECOVERY_SPEED) { if (t < RAMPUP_RECOVERY_SPEED) {
@@ -244,3 +256,13 @@ double Motor::calculateCurrentSpeed() const noexcept
return targetSpeed; 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); void update(const double frametime);
std::size_t getIndex() const { return index; }; std::size_t getIndex() const noexcept { return index; };
double getCurrentPosition() const { return currentPosition; }; double getCurrentPosition() const noexcept { return currentPosition; };
double getTargetPosition() const { return targetPosition; }; double getTargetPosition() const noexcept { return targetPosition; };
double getSpeed() const { return targetSpeed; }; double getSpeed() const noexcept { return targetSpeed; };
void setSpeed(const double speedTarget) noexcept; void setSpeed(const double speedTarget) noexcept;
double getMechanicalMinPosition() const { return mechanicalMinPosition; }; double getMechanicalMinPosition() const noexcept { return mechanicalMinPosition; };
double getMechanicalMaxPosition() const { return mechanicalMaxPosition; }; double getMechanicalMaxPosition() const noexcept { return mechanicalMaxPosition; };
double getPaused() const { return paused; }; double getPaused() const noexcept { return paused; };
bool isPositionInMechanicalRange(const double pos) const noexcept; bool isPositionInMechanicalRange(const double pos) const noexcept;
private: private:
@@ -50,6 +50,8 @@ private:
bool sendMACommand(const double targetPos) noexcept; bool sendMACommand(const double targetPos) noexcept;
// Will calculate the current speed for rampup based on currentPosition, moveStartPosition, and targetPosition // Will calculate the current speed for rampup based on currentPosition, moveStartPosition, and targetPosition
double calculateCurrentSpeed() const noexcept; double calculateCurrentSpeed() const noexcept;
double getCurrentMovementProgress() const noexcept;
bool isCurrentlyMoving() const noexcept;
const uint8_t index; const uint8_t index;
OSerialBus& serialBus; 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::Y { 0.0, 1.0, 0.0 };
const Vector3d Vector3d::Z { 0.0, 0.0, 1.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) : 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) : x(x), y(y), z(z)
{ {
} }
Vector3d& Vector3d::operator=(const Vector3d& other) Vector3d& Vector3d::operator=(const Vector3d& other) noexcept
{ {
if (this != &other) if (this != &other)
{ {
@@ -45,27 +45,27 @@ Vector3d& Vector3d::operator=(Vector3d&& other) noexcept
return *this; 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 }; 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 }; 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 }; 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 }; return { x / scalar, y / scalar, z / scalar };
} }
Vector3d& Vector3d::operator+=(const Vector3d& rhs) Vector3d& Vector3d::operator+=(const Vector3d& rhs) noexcept
{ {
x += rhs.x; x += rhs.x;
y += rhs.y; y += rhs.y;
@@ -73,7 +73,7 @@ Vector3d& Vector3d::operator+=(const Vector3d& rhs)
return *this; return *this;
} }
Vector3d& Vector3d::operator-=(const Vector3d& rhs) Vector3d& Vector3d::operator-=(const Vector3d& rhs) noexcept
{ {
x -= rhs.x; x -= rhs.x;
y -= rhs.y; y -= rhs.y;
@@ -81,7 +81,7 @@ Vector3d& Vector3d::operator-=(const Vector3d& rhs)
return *this; return *this;
} }
Vector3d& Vector3d::operator*=(double scalar) Vector3d& Vector3d::operator*=(double scalar) noexcept
{ {
x *= scalar; x *= scalar;
y *= scalar; y *= scalar;
@@ -89,7 +89,7 @@ Vector3d& Vector3d::operator*=(double scalar)
return *this; return *this;
} }
Vector3d& Vector3d::operator/=(double scalar) Vector3d& Vector3d::operator/=(double scalar) noexcept
{ {
x /= scalar; x /= scalar;
y /= scalar; y /= scalar;
@@ -97,12 +97,12 @@ Vector3d& Vector3d::operator/=(double scalar)
return *this; 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; 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); return !(*this == rhs);
} }
@@ -113,22 +113,22 @@ std::ostream& operator<<(std::ostream& os, const Vector3d& v)
return os; 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; 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); 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; return x * x + y * y + z * z;
} }
Vector3d Vector3d::cross(const Vector3d& rhs) const Vector3d Vector3d::cross(const Vector3d& rhs) const noexcept
{ {
return { return {
y * rhs.z - z * rhs.y, 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(); double m = magnitude();
if (m == 0.0) if (m == 0.0)
@@ -147,7 +147,7 @@ Vector3d Vector3d::normalized() const
return { x * inv, y * inv, z * inv }; return { x * inv, y * inv, z * inv };
} }
void Vector3d::normalize() void Vector3d::normalize() noexcept
{ {
double m = magnitude(); double m = magnitude();
if (m == 0.0) if (m == 0.0)
MotionCore/Vector3d.h
+19 -19
View File
@@ -10,37 +10,37 @@ public:
double z; double z;
public: public:
Vector3d(); Vector3d() noexcept;
Vector3d(double x, double y, double z); Vector3d(double x, double y, double z) noexcept;
Vector3d(const Vector3d& other) = default; Vector3d(const Vector3d& other) = default;
Vector3d(Vector3d&& other) noexcept = default; Vector3d(Vector3d&& other) noexcept = default;
double dot(const Vector3d& rhs) const; double dot(const Vector3d& rhs) const noexcept;
double magnitude() const; double magnitude() const noexcept;
double magnitudeSquared() const; double magnitudeSquared() const noexcept;
Vector3d cross(const Vector3d& rhs) const; Vector3d cross(const Vector3d& rhs) const noexcept;
Vector3d normalized() const; Vector3d normalized() const noexcept;
void normalize(); void normalize() noexcept;
Vector3d& operator=(const Vector3d& other); Vector3d& operator=(const Vector3d& other) noexcept;
Vector3d& operator=(Vector3d&& other) noexcept; Vector3d& operator=(Vector3d&& other) noexcept;
Vector3d operator+(const Vector3d& rhs) const; Vector3d operator+(const Vector3d& rhs) const noexcept;
Vector3d operator-(const Vector3d& rhs) const; Vector3d operator-(const Vector3d& rhs) const noexcept;
Vector3d operator*(double scalar) const; Vector3d operator*(double scalar) const noexcept;
Vector3d operator/(double scalar) const; Vector3d operator/(double scalar) const noexcept;
Vector3d& operator+=(const Vector3d& rhs); Vector3d& operator+=(const Vector3d& rhs) noexcept;
Vector3d& operator-=(const Vector3d& rhs); Vector3d& operator-=(const Vector3d& rhs) noexcept;
Vector3d& operator*=(double scalar); Vector3d& operator*=(double scalar) noexcept;
Vector3d& operator/=(double scalar); Vector3d& operator/=(double scalar) noexcept;
bool operator==(const Vector3d& rhs) const; bool operator==(const Vector3d& rhs) const noexcept;
bool operator!=(const Vector3d& rhs) const; bool operator!=(const Vector3d& rhs) const noexcept;
public: public:
static const Vector3d Zero; static const Vector3d Zero;