From 73263e54376ac01463b4be62d439089e1675d593 Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Tue, 10 Feb 2026 21:48:40 +0100 Subject: [PATCH] dynamically code arm joints --- MotionCore/Arm.cpp | 232 +++++++++----------------------------- MotionCore/Arm.h | 28 ++--- MotionCore/ArmSegment.cpp | 5 + MotionCore/ArmSegment.h | 1 + MotionCore/MainLoop.cpp | 19 ++-- 5 files changed, 77 insertions(+), 208 deletions(-) diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp index 1ca688a..7b8c4e0 100755 --- a/MotionCore/Arm.cpp +++ b/MotionCore/Arm.cpp @@ -2,176 +2,84 @@ #include #include #include +#include Arm::Arm(const std::wstring& comPort) noexcept: oSerialBus(comPort), - j0( - oSerialBus, - Vector3d::Y, - Vector3d::Forward, - 0.0, //mm - 0, - -135.0, - 45.0, - 0.0, - nullptr - ), - j1( - oSerialBus, - -Vector3d::X, - Vector3d::Up, - 72.0, //mm - 1, - -38.0, - 85.0, - 0.0, - &j0 - ), - j2( - oSerialBus, - -Vector3d::X, - Vector3d::Up, - 97.0, //mm - 2, // index - -90, // min - 90.0, // max - 0.0, // home - &j1 - ), - j3( - oSerialBus, - Vector3d::X, - Vector3d::Up, - 114.0, //mm - 3, - -90.0, - 90.0, - 90.0, - &j2 - ), - j4( - oSerialBus, - Vector3d::Y, - Vector3d::Forward, - 0.0, //mm - 4, - -90.0, - 90.0, - 90.0, - &j3 - ), - j5( - oSerialBus, - -Vector3d::X, - Vector3d::Up, - 56.0, //mm - 5, - -90.0, - 90.0, - 90.0, - &j4 - ) + joints{ + // bus axis ref len idx min max home parent + ArmSegment(oSerialBus, Vector3d::Y, Vector3d::Forward, 0.0, 0, -135.0, 45.0, 0.0, nullptr), + ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 72.0, 1, -38.0, 85.0, 0.0, nullptr), + ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 97.0, 2, -90.0, 90.0, 0.0, nullptr), + ArmSegment(oSerialBus, Vector3d::X, Vector3d::Up, 114.0, 3, -90.0, 90.0, 90.0, nullptr), + ArmSegment(oSerialBus, Vector3d::Y, Vector3d::Forward, 0.0, 4, -90.0, 90.0, 90.0, nullptr), + ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 56.0, 5, -90.0, 90.0, 90.0, nullptr) + } { if (!oSerialBus.isOpen()) { std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl; exit(-1); } + + // Assign parents + ArmSegment* parent = &joints[0]; + for (auto& joint : joints | std::views::drop(1)) { + joint.setParent(parent); + parent = &joint; + } } bool Arm::assumeHomePose() { - if (!j0.goHome(0.5)) { - return false; + for (auto& joint : joints) { + if (!joint.goHome(0.5)) { + return false; + } } - if (!j1.goHome(0.5)) { - return false; - } - if (!j2.goHome(0.5)) { - return false; - } - if (!j3.goHome(0.5)) { - return false; - } - if (!j4.goHome(0.5)) { - return false; - } - if (!j5.goHome(0.5)) { - return false; - } - return true; } void Arm::stagePose( - const double theta_j0, - const double theta_j1, - const double theta_j2, - const double theta_j3, - const double theta_j4, - const double theta_j5 + const std::array& pose ) noexcept { - j0.stageAngle(theta_j0); - j1.stageAngle(theta_j1); - j2.stageAngle(theta_j2); - j3.stageAngle(theta_j3); - j4.stageAngle(theta_j4); - j5.stageAngle(theta_j5); + for (std::size_t i = 0; i < NUM_JOINTS; i++) { + joints[i].stageAngle(pose[i]); + } } void Arm::commitPose() noexcept { - if (j0.isStaged()) { - j0.commitStaging(); - } - if (j1.isStaged()) { - j1.commitStaging(); - } - if (j2.isStaged()) { - j2.commitStaging(); - } - if (j3.isStaged()) { - j3.commitStaging(); - } - if (j4.isStaged()) { - j4.commitStaging(); - } - if (j5.isStaged()) { - j5.commitStaging(); + for (auto& joint : joints) { + if (joint.isStaged()) { + joint.commitStaging(); + } } } void Arm::rollbackPose() noexcept { - if (j0.isStaged()) { - j0.rollbackStaging(); - } - if (j1.isStaged()) { - j1.rollbackStaging(); - } - if (j2.isStaged()) { - j2.rollbackStaging(); - } - if (j3.isStaged()) { - j3.rollbackStaging(); - } - if (j4.isStaged()) { - j4.rollbackStaging(); - } - if (j5.isStaged()) { - j5.rollbackStaging(); + for (auto& joint : joints) { + if (joint.isStaged()) { + joint.rollbackStaging(); + } } } bool Arm::isMoving() const noexcept { - return j0.isMoving() || j1.isMoving() || j2.isMoving() || j3.isMoving() || j4.isMoving() || j5.isMoving(); + for (auto& joint : joints) { + if (joint.isMoving()) { + return true; + } + } + return false; } bool Arm::isEFPositionValid(std::span deadzones) const noexcept { // Otherwise: check for endeffector position - const Vector3d ef = j5.getTargetedGlobalEndpoint(); + const Vector3d ef = joints.back().getTargetedGlobalEndpoint(); // If any deadzone intersects with the EF pos, fai for (const AxisAlignedBoundingBox& aabb : deadzones) { @@ -186,30 +94,14 @@ bool Arm::isEFPositionValid(std::span deadzones) c bool Arm::rollbackJointsBasedOnValidRange() noexcept { bool allGood = true; - if (!j0.isPositionInRange(j0.getTargetPosition())) { - j0.rollbackStaging(); - allGood = false; - } - if (!j1.isPositionInRange(j1.getTargetPosition())) { - j2.rollbackStaging(); - allGood = false; - } - if (!j2.isPositionInRange(j2.getTargetPosition())) { - j2.rollbackStaging(); - allGood = false; - } - if (!j3.isPositionInRange(j3.getTargetPosition())) { - j3.rollbackStaging(); - allGood = false; - } - if (!j4.isPositionInRange(j4.getTargetPosition())) { - j4.rollbackStaging(); - allGood = false; - } - if (!j5.isPositionInRange(j5.getTargetPosition())) { - j5.rollbackStaging(); - allGood = false; + + for (auto& joint : joints) { + if (!joint.isPositionInRange(joint.getTargetPosition())) { + joint.rollbackStaging(); + allGood = false; + } } + return allGood; } @@ -220,23 +112,10 @@ Arm::~Arm() bool Arm::assumeHomePoseImmediate() { - if (!j0.goHomeImmediate()) { - return false; - } - if (!j1.goHomeImmediate()) { - return false; - } - if (!j2.goHomeImmediate()) { - return false; - } - if (!j3.goHomeImmediate()) { - return false; - } - if (!j4.goHomeImmediate()) { - return false; - } - if (!j5.goHomeImmediate()) { - return false; + for (auto& joint : joints) { + if (!joint.goHomeImmediate()) { + return false; + } } return true; @@ -244,10 +123,7 @@ bool Arm::assumeHomePoseImmediate() void Arm::update(double frametime) { - j0.update(frametime); - j1.update(frametime); - j2.update(frametime); - j3.update(frametime); - j4.update(frametime); - j5.update(frametime); + for (auto& joint : joints) { + joint.update(frametime); + } } diff --git a/MotionCore/Arm.h b/MotionCore/Arm.h index 3de111a..b55fab8 100755 --- a/MotionCore/Arm.h +++ b/MotionCore/Arm.h @@ -3,6 +3,9 @@ #include "AxisAlignedBoundingBox.h" #include #include +#include + +constexpr std::size_t NUM_JOINTS = 6; class Arm { @@ -15,14 +18,7 @@ public: bool assumeHomePoseImmediate(); bool assumeHomePose(); - void stagePose( - const double theta_j0, - const double theta_j1, - const double theta_j2, - const double theta_j3, - const double theta_j4, - const double theta_j5 - ) noexcept; + void stagePose(const std::array& pose) noexcept; void commitPose() noexcept; void rollbackPose() noexcept; bool isMoving() const noexcept; @@ -32,22 +28,12 @@ public: // Will roll back individual joints if their angles exceed their limits. Returns true if no rollbacks were made. bool rollbackJointsBasedOnValidRange() noexcept; - const ArmSegment& getJ0() const { return j0; }; - const ArmSegment& getJ1() const { return j1; }; - const ArmSegment& getJ2() const { return j2; }; - const ArmSegment& getJ3() const { return j3; }; - const ArmSegment& getJ4() const { return j4; }; - const ArmSegment& getJ5() const { return j5; }; + const std::array& getJoints() const { return joints; }; + const ArmSegment& getEndEffectorJoint() const { return joints.back(); }; void update(double frametime); private: OSerialBus oSerialBus; - ArmSegment j0; - ArmSegment j1; - ArmSegment j2; - ArmSegment j3; - ArmSegment j4; - ArmSegment j5; + std::array joints; }; - diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp index b78b946..218836c 100755 --- a/MotionCore/ArmSegment.cpp +++ b/MotionCore/ArmSegment.cpp @@ -140,6 +140,11 @@ bool ArmSegment::rollbackStaging() noexcept return true; } +void ArmSegment::setParent(ArmSegment* parent) noexcept +{ + this->parent = parent; +} + void ArmSegment::update(const double frametime) { motor.update(frametime); diff --git a/MotionCore/ArmSegment.h b/MotionCore/ArmSegment.h index 54d83b3..91c141d 100755 --- a/MotionCore/ArmSegment.h +++ b/MotionCore/ArmSegment.h @@ -46,6 +46,7 @@ public: bool commitStaging() noexcept; // Returns false if not currently staged bool rollbackStaging() noexcept; + void setParent(ArmSegment* parent) noexcept; bool isPositionInRange(const double pos) const noexcept { return motor.isPositionInRange(pos); }; double getCurrentPosition() const noexcept { return motor.getCurrentPosition(); }; diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 01f7684..3dcae2a 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -5,6 +5,7 @@ #include #include #include "StateMachine.h" +#include "ArmSegment.h" #include "yz_plotter_win.h" MainLoop::MainLoop() noexcept : @@ -86,12 +87,12 @@ void MainLoop::run() j0Pos += -vkbi.roll * speed * frametime; j1Pos += -vkbi.pitch * speed * frametime; j2Pos += -vkbi.yaw * speed * frametime; - j3Pos = vkbi.wheel * (arm.getJ3().getMaxAngle() - arm.getJ3().getMinAngle()); + j3Pos = vkbi.wheel * (arm.getJoints()[3].getMaxAngle() - arm.getJoints()[3].getMinAngle()); j4Pos += vkbi.thumb_ver * speed * frametime; j5Pos += vkbi.thumb_hor * speed * frametime; // Stage the new pose and keep it if it is good - arm.stagePose(j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos); + arm.stagePose({ j0Pos, j1Pos, j2Pos, j3Pos, j4Pos, j5Pos }); if (arm.isEFPositionValid(deadzones)) { // Roll back individual joints if their angles are out of range arm.rollbackJointsBasedOnValidRange(); @@ -99,19 +100,19 @@ void MainLoop::run() //std::cout << "Pose good!!" << std::endl; arm.commitPose(); - j0Pos = arm.getJ0().getTargetPosition(); - j1Pos = arm.getJ1().getTargetPosition(); - j2Pos = arm.getJ2().getTargetPosition(); - j3Pos = arm.getJ3().getTargetPosition(); - j4Pos = arm.getJ4().getTargetPosition(); - j5Pos = arm.getJ5().getTargetPosition(); + j0Pos = arm.getJoints()[0].getTargetPosition(); + j1Pos = arm.getJoints()[1].getTargetPosition(); + j2Pos = arm.getJoints()[2].getTargetPosition(); + j3Pos = arm.getJoints()[3].getTargetPosition(); + j4Pos = arm.getJoints()[4].getTargetPosition(); + j5Pos = arm.getJoints()[5].getTargetPosition(); } else { arm.rollbackPose(); //std::cout << "Hit deadzone!!" << std::endl; } - const Vector3d ef = arm.getJ5().getCurrentGlobalEndpoint(); + const Vector3d ef = arm.getEndEffectorJoint().getCurrentGlobalEndpoint(); //std::cout << ef << std::endl; //std::cout << arm.getJ3().getCurrentPosition() << std::endl; plot.draw(ef);