diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp new file mode 100755 index 0000000..0ffe2f8 --- /dev/null +++ b/MotionCore/Arm.cpp @@ -0,0 +1,89 @@ +#include "Arm.h" +#include + +Arm::Arm(const std::wstring& comPort) noexcept: + oSerialBus(comPort), + j0( + oSerialBus, + Vector3d::Y, + Vector3d::Up, + 100.0, //mm + 0.0, + 125.0, + 30.0, + 180.0, + 0.0, + nullptr + ), + j1( + oSerialBus, + Vector3d::X, + Vector3d::Forward, + 72.0, //mm + 1.0, + 48.0, + 0.0, + 120.0, + 0.0, + &j0 + ), + j2( + oSerialBus, + Vector3d::X, + Vector3d::Forward, + 95.0, //mm + 2.0, + 115.0, + 0.0, + 180.0, + 0.0, + &j1 + ) +{ + if (!oSerialBus.isOpen()) { + std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl; + exit(-1); + } +} + +bool Arm::assumeHomePose() +{ + if (!j0.goHome(0.5)) { + return false; + } + if (!j1.goHome(0.5)) { + return false; + } + if (!j2.goHome(0.5)) { + return false; + } + + return true; +} + +Arm::~Arm() +{ + oSerialBus.close(); +} + +bool Arm::assumeHomePoseImmediate() +{ + if (!j0.goHomeImmediate()) { + return false; + } + if (!j1.goHomeImmediate()) { + return false; + } + if (!j2.goHomeImmediate()) { + return false; + } + + return true; +} + +void Arm::update(double frametime) +{ + j0.update(frametime); + j1.update(frametime); + j2.update(frametime); +} diff --git a/MotionCore/Arm.h b/MotionCore/Arm.h new file mode 100755 index 0000000..c945f5d --- /dev/null +++ b/MotionCore/Arm.h @@ -0,0 +1,24 @@ +#pragma once +#include "ArmSegment.h" +#include + +class Arm +{ +public: + Arm(const std::wstring& comPort) noexcept; + Arm(const Arm& other) = delete; + Arm(Arm&& other) noexcept = default; + ~Arm(); + + bool assumeHomePoseImmediate(); + bool assumeHomePose(); + + void update(double frametime); + +private: + OSerialBus oSerialBus; + ArmSegment j0; + ArmSegment j1; + ArmSegment j2; +}; + diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp new file mode 100755 index 0000000..86ffb7d --- /dev/null +++ b/MotionCore/ArmSegment.cpp @@ -0,0 +1,113 @@ +#include "ArmSegment.h" +#include "OSerialBus.h" + +ArmSegment::ArmSegment( + OSerialBus& serialBus, + const Vector3d& rotationAxis, + const Vector3d& longitudinalAxis, + const double length, + const uint8_t motorIndex, + const double mechanicalToVirtualAngleOffset, + const double mechanicalMinAngle, + const double mechanicalMaxAngle, + const double homeAngle, + ArmSegment* parent +) noexcept : + rotationAxis(rotationAxis), + longitudinalAxis(longitudinalAxis), + length(length), + motor( + motorIndex, + serialBus, + virtualToMechanicalAngle(homeAngle), + mechanicalMinAngle, + mechanicalMaxAngle, + 0.5 + ), + parent(parent) +{ + +} + +bool ArmSegment::goHome(const std::optional speed) noexcept +{ + return motor.moveTo( + virtualToMechanicalAngle(homeAngle), + speed + ); +} + + +bool ArmSegment::goHomeImmediate() noexcept +{ + return motor.moveToImmediate( + virtualToMechanicalAngle(homeAngle) + ); +} + +bool ArmSegment::moveTo(const double theta, const std::optional speed) noexcept +{ + return motor.moveTo( + virtualToMechanicalAngle(theta), + speed + ); +} + + +bool ArmSegment::moveToImmediate(const double theta) noexcept +{ + return motor.moveToImmediate( + virtualToMechanicalAngle(theta) + ); +} + +bool ArmSegment::moveBy(const double theta, const std::optional speed) noexcept +{ + return motor.moveBy( + virtualToMechanicalAngle(theta), + speed + ); +} + + +bool ArmSegment::moveByImmediate(const double theta) noexcept +{ + return motor.moveByImmediate( + virtualToMechanicalAngle(theta) + ); +} + +bool ArmSegment::stopMoving() noexcept +{ + return motor.stopMoving(); +} + +void ArmSegment::pauseMoving() noexcept +{ + motor.pauseMoving(); +} + +void ArmSegment::resumeMoving() noexcept +{ + motor.resumeMoving(); +} + +bool ArmSegment::assumeTargetPositionImmediately() noexcept +{ + return motor.assumeTargetPositionImmediately(); +} + +void ArmSegment::update(const double frametime) +{ + motor.update(frametime); +} + +double ArmSegment::virtualToMechanicalAngle(const double virtualAngle) +{ + return virtualAngle + mechanicalToVirtualAngleOffset; +} + +double ArmSegment::mechanicalToVirtualAngle(const double hardwareAngle) +{ + return hardwareAngle - mechanicalToVirtualAngleOffset; +} diff --git a/MotionCore/ArmSegment.h b/MotionCore/ArmSegment.h new file mode 100755 index 0000000..aa6e436 --- /dev/null +++ b/MotionCore/ArmSegment.h @@ -0,0 +1,66 @@ +#pragma once +#include "Motor.h" +#include "Vector3d.h" +#include + +class OSerialBus; + +class ArmSegment +{ +public: + ArmSegment( + OSerialBus& serialBus, + const Vector3d& rotationAxis, + const Vector3d& longitudinalAxis, + const double length, + const uint8_t motorIndex, + const double mechanicalToVirtualAngleOffset, // Mechanical angle at which this segment is neutral to its parent (assuming 0 degrees in virtual space) + const double mechanicalMinAngle, + const double mechanicalMaxAngle, + const double homeAngle, + ArmSegment* parent // May be nullptr + ) noexcept; + + ArmSegment(const ArmSegment& other) = delete; + ArmSegment(ArmSegment&& other) noexcept = default; + + // Will return the motor to its home angle + bool goHome(const std::optional speed) noexcept; + // Will return the motor to its home angle immediately (this is jerky and may damage the hardware long-term) + bool goHomeImmediate() noexcept; + + // Will move the motor to the desired angle with ramp-up and controlled speed + bool moveTo(const double theta, const std::optional speed) noexcept; + // Will move the motor instantly to the desired angle (this is jerky and may damage the hardware long-term) + bool moveToImmediate(const double theta) noexcept; + // Will move the motor by a specified angle with ramp-up and controlled speed + bool moveBy(const double theta, const std::optional speed) noexcept; + // Will move the motor by a specified angle instantly (this is jerky and may damage the hardware long-term) + bool moveByImmediate(const double theta) noexcept; + bool stopMoving() noexcept; + void pauseMoving() noexcept; + void resumeMoving() noexcept; + // Will move the motor to its current target instantly (this is jerky and may damage the hardware long-term) + bool assumeTargetPositionImmediately() noexcept; + + 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 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); + + Motor motor; + Vector3d rotationAxis; + Vector3d longitudinalAxis; + double length; + double mechanicalToVirtualAngleOffset; + double homeAngle; + ArmSegment* parent; +}; diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index bb86ac0..b833564 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -4,40 +4,9 @@ #include 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 - ) + arm(L"COM4") { - - if (!oSerialBus.isOpen()) { - std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl; - } - - j0.moveHomeImmediate(); - j1.moveHomeImmediate(); - j2.moveHomeImmediate(); + arm.assumeHomePoseImmediate(); } MainLoop& MainLoop::getInstance() @@ -48,7 +17,6 @@ MainLoop& MainLoop::getInstance() MainLoop::~MainLoop() noexcept { - oSerialBus.close(); } void MainLoop::run() @@ -59,9 +27,7 @@ void MainLoop::run() 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); + arm.update(frametime); lastBegin = std::chrono::high_resolution_clock::now(); } diff --git a/MotionCore/MainLoop.h b/MotionCore/MainLoop.h index a437db5..f67fac8 100755 --- a/MotionCore/MainLoop.h +++ b/MotionCore/MainLoop.h @@ -1,6 +1,5 @@ #pragma once -#include "OSerialBus.h" -#include "Motor.h" +#include "Arm.h" // Singleton-class, use MainLoop::getInstance() to get the only instance of the runtime class MainLoop @@ -15,10 +14,7 @@ public: private: MainLoop() noexcept; - OSerialBus oSerialBus; - Motor j0; - Motor j1; - Motor j2; - bool isRunning = true; + + Arm arm; }; diff --git a/MotionCore/MotionCore.vcxproj b/MotionCore/MotionCore.vcxproj index 0e37b7e..3546016 100755 --- a/MotionCore/MotionCore.vcxproj +++ b/MotionCore/MotionCore.vcxproj @@ -143,15 +143,21 @@ + + + + + + diff --git a/MotionCore/MotionCore.vcxproj.filters b/MotionCore/MotionCore.vcxproj.filters index 7b89b20..074d291 100755 --- a/MotionCore/MotionCore.vcxproj.filters +++ b/MotionCore/MotionCore.vcxproj.filters @@ -27,6 +27,15 @@ Quelldateien + + Quelldateien + + + Quelldateien + + + Quelldateien + @@ -38,5 +47,14 @@ Headerdateien + + Headerdateien + + + Headerdateien + + + Headerdateien + \ No newline at end of file diff --git a/MotionCore/Motor.cpp b/MotionCore/Motor.cpp index 894067f..ed6e10f 100755 --- a/MotionCore/Motor.cpp +++ b/MotionCore/Motor.cpp @@ -12,7 +12,6 @@ Motor::Motor( const double initialPosition, const double mechanicalMinPosition, const double mechanicalMaxPosition, - const double homePosition, const double initialSpeed ) noexcept: index(index), @@ -21,8 +20,7 @@ Motor::Motor( targetPosition(currentPosition), moveStartPosition(currentPosition), mechanicalMinPosition(mechanicalMinPosition), - mechanicalMaxPosition(mechanicalMaxPosition), - homePosition(homePosition) + mechanicalMaxPosition(mechanicalMaxPosition) { setSpeed(initialSpeed); } @@ -35,7 +33,6 @@ Motor::Motor(Motor&& other) noexcept: moveStartPosition(other.moveStartPosition), mechanicalMinPosition(other.mechanicalMinPosition), mechanicalMaxPosition(other.mechanicalMaxPosition), - homePosition(other.homePosition), targetSpeed(other.targetSpeed), speedLow(other.speedLow) { @@ -130,26 +127,6 @@ bool Motor::moveByImmediate(const double theta) noexcept return result; } -bool Motor::moveHome(const std::optional speed) noexcept -{ - 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) { @@ -202,7 +179,7 @@ void Motor::update(const double frametime) return; } - handleRampUp(frametime); + handleMovement(frametime); } void Motor::setSpeed(const double speed) noexcept @@ -221,7 +198,7 @@ bool Motor::isPositionInMechanicalRange(const double pos) const noexcept return pos >= mechanicalMinPosition && pos <= mechanicalMaxPosition; } -void Motor::handleRampUp(const double frametime) +void Motor::handleMovement(const double frametime) { constexpr double epsilon = 0.1; if (std::abs(currentPosition - targetPosition) > epsilon) { diff --git a/MotionCore/Motor.h b/MotionCore/Motor.h index 3fc3b12..4b4838d 100755 --- a/MotionCore/Motor.h +++ b/MotionCore/Motor.h @@ -11,23 +11,24 @@ public: const double initialPosition, const double mechanicalMinPosition, const double mechanicalMaxPosition, - const double homePosition, const double initialSpeed = 0.5 ) noexcept; Motor(const Motor& other) = delete; Motor(Motor&& other) noexcept; - // Will move the motor immediately to the desired position (don't do that) - bool moveToImmediate(const double theta) noexcept; + // Will move the motor to the desired position with ramp-up and controlled speed bool moveTo(const double theta, const std::optional speed) noexcept; + // Will move the motor instantly to the desired position (this is jerky and may damage the hardware long-term) + bool moveToImmediate(const double theta) noexcept; + // Will move the motor by a specified angle with ramp-up and controlled speed bool moveBy(const double theta, const std::optional speed) noexcept; + // Will move the motor by a specified angle instantly (this is jerky and may damage the hardware long-term) bool moveByImmediate(const double theta) noexcept; - bool moveHome(const std::optional speed) noexcept; - bool moveHomeImmediate() noexcept; bool stopMoving() noexcept; void pauseMoving() noexcept; void resumeMoving() noexcept; + // Will move the motor to its current target instantly (this is jerky and may damage the hardware long-term) bool assumeTargetPositionImmediately() noexcept; void update(const double frametime); @@ -39,12 +40,13 @@ public: 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: - void handleRampUp(const double frametime); + // Update-handler for moving + void handleMovement(const double frametime); + // Sends a move absolute command to the serial busses MCU bool sendMACommand(const double targetPos) noexcept; // Will calculate the current speed for rampup based on currentPosition, moveStartPosition, and targetPosition double calculateCurrentSpeed() const noexcept; @@ -58,7 +60,6 @@ private: double speedLow; // End- and startpoint of ramp-up phase const double mechanicalMinPosition; const double mechanicalMaxPosition; - const double homePosition; bool disabled = false; bool paused = false; }; diff --git a/MotionCore/Vector3d.cpp b/MotionCore/Vector3d.cpp new file mode 100755 index 0000000..80df308 --- /dev/null +++ b/MotionCore/Vector3d.cpp @@ -0,0 +1,160 @@ +#include "Vector3d.h" +#include + +const Vector3d Vector3d::Zero { 0.0, 0.0, 0.0 }; +const Vector3d Vector3d::One { 1.0, 1.0, 1.0 }; +const Vector3d Vector3d::Forward { 0.0, 0.0, 1.0 }; +const Vector3d Vector3d::Backward { 0.0, 0.0, -1.0 }; +const Vector3d Vector3d::Up { 0.0, 1.0, 0.0 }; +const Vector3d Vector3d::Down { 0.0, -1.0, 0.0 }; +const Vector3d Vector3d::Left { -1.0, 0.0, 0.0 }; +const Vector3d Vector3d::Right { 1.0, 0.0, 0.0 }; +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() + : x(0.0), y(0.0), z(0.0) +{ +} + +Vector3d::Vector3d(double x, double y, double z) + : x(x), y(y), z(z) +{ +} + +Vector3d& Vector3d::operator=(const Vector3d& other) +{ + if (this != &other) + { + x = other.x; + y = other.y; + z = other.z; + } + return *this; +} + +Vector3d& Vector3d::operator=(Vector3d&& other) noexcept +{ + if (this != &other) + { + x = other.x; + y = other.y; + z = other.z; + } + return *this; +} + +Vector3d Vector3d::operator+(const Vector3d& rhs) const +{ + return { x + rhs.x, y + rhs.y, z + rhs.z }; +} + +Vector3d Vector3d::operator-(const Vector3d& rhs) const +{ + return { x - rhs.x, y - rhs.y, z - rhs.z }; +} + +Vector3d Vector3d::operator*(double scalar) const +{ + return { x * scalar, y * scalar, z * scalar }; +} + +Vector3d Vector3d::operator/(double scalar) const +{ + return { x / scalar, y / scalar, z / scalar }; +} + +Vector3d& Vector3d::operator+=(const Vector3d& rhs) +{ + x += rhs.x; + y += rhs.y; + z += rhs.z; + return *this; +} + +Vector3d& Vector3d::operator-=(const Vector3d& rhs) +{ + x -= rhs.x; + y -= rhs.y; + z -= rhs.z; + return *this; +} + +Vector3d& Vector3d::operator*=(double scalar) +{ + x *= scalar; + y *= scalar; + z *= scalar; + return *this; +} + +Vector3d& Vector3d::operator/=(double scalar) +{ + x /= scalar; + y /= scalar; + z /= scalar; + return *this; +} + +bool Vector3d::operator==(const Vector3d& rhs) const +{ + return x == rhs.x && y == rhs.y && z == rhs.z; +} + +bool Vector3d::operator!=(const Vector3d& rhs) const +{ + return !(*this == rhs); +} + +std::ostream& operator<<(std::ostream& os, const Vector3d& v) +{ + os << "Vector3d(" << v.x << ", " << v.y << ", " << v.z << ")"; + return os; +} + +double Vector3d::dot(const Vector3d& rhs) const +{ + return x * rhs.x + y * rhs.y + z * rhs.z; +} + +double Vector3d::magnitude() const +{ + return std::sqrt(x * x + y * y + z * z); +} + +double Vector3d::magnitudeSquared() const +{ + return x * x + y * y + z * z; +} + +Vector3d Vector3d::cross(const Vector3d& rhs) const +{ + return { + y * rhs.z - z * rhs.y, + z * rhs.x - x * rhs.z, + x * rhs.y - y * rhs.x + }; +} + +Vector3d Vector3d::normalized() const +{ + double m = magnitude(); + if (m == 0.0) + return Vector3d::Zero; + + double inv = 1.0 / m; + return { x * inv, y * inv, z * inv }; +} + +void Vector3d::normalize() +{ + double m = magnitude(); + if (m == 0.0) + return; + + double inv = 1.0 / m; + x *= inv; + y *= inv; + z *= inv; +} diff --git a/MotionCore/Vector3d.h b/MotionCore/Vector3d.h new file mode 100755 index 0000000..ce7e713 --- /dev/null +++ b/MotionCore/Vector3d.h @@ -0,0 +1,59 @@ +#pragma once + +#include + +class Vector3d +{ +public: + double x; + double y; + double z; + +public: + Vector3d(); + Vector3d(double x, double y, double z); + + 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(); + + Vector3d& operator=(const Vector3d& other); + Vector3d& operator=(Vector3d&& other) noexcept; + + Vector3d operator+(const Vector3d& rhs) const; + Vector3d operator-(const Vector3d& rhs) const; + + Vector3d operator*(double scalar) const; + Vector3d operator/(double scalar) const; + + Vector3d& operator+=(const Vector3d& rhs); + Vector3d& operator-=(const Vector3d& rhs); + + Vector3d& operator*=(double scalar); + Vector3d& operator/=(double scalar); + + bool operator==(const Vector3d& rhs) const; + bool operator!=(const Vector3d& rhs) const; + +public: + static const Vector3d Zero; + static const Vector3d One; + static const Vector3d Forward; + static const Vector3d Backward; + static const Vector3d Up; + static const Vector3d Down; + static const Vector3d Left; + static const Vector3d Right; + static const Vector3d X; + static const Vector3d Y; + static const Vector3d Z; +}; + +std::ostream& operator<<(std::ostream& os, const Vector3d& v);