From 2bd1cc0bfd10742b12cd3309cb08180aad975f79 Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Tue, 10 Feb 2026 20:54:09 +0100 Subject: [PATCH] feat: add j3,j4,j5 --- MotionCore/Arm.cpp | 116 ++++++++++++++++++++++++++++++++++---- MotionCore/Arm.h | 15 ++++- MotionCore/ArmSegment.cpp | 2 +- MotionCore/MainLoop.cpp | 30 +++++++--- MotionCore/MainLoop.h | 2 +- 5 files changed, 143 insertions(+), 22 deletions(-) diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp index 588a33a..1ca688a 100755 --- a/MotionCore/Arm.cpp +++ b/MotionCore/Arm.cpp @@ -31,12 +31,45 @@ Arm::Arm(const std::wstring& comPort) noexcept: oSerialBus, -Vector3d::X, Vector3d::Up, - 95.0, //mm - 2, - -123, - 57.0, - 0.0, + 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 ) { if (!oSerialBus.isOpen()) { @@ -56,15 +89,34 @@ bool Arm::assumeHomePose() 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) noexcept +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 +) 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); } void Arm::commitPose() noexcept @@ -78,6 +130,15 @@ void Arm::commitPose() noexcept if (j2.isStaged()) { j2.commitStaging(); } + if (j3.isStaged()) { + j3.commitStaging(); + } + if (j4.isStaged()) { + j4.commitStaging(); + } + if (j5.isStaged()) { + j5.commitStaging(); + } } void Arm::rollbackPose() noexcept @@ -91,19 +152,28 @@ void Arm::rollbackPose() noexcept if (j2.isStaged()) { j2.rollbackStaging(); } + if (j3.isStaged()) { + j3.rollbackStaging(); + } + if (j4.isStaged()) { + j4.rollbackStaging(); + } + if (j5.isStaged()) { + j5.rollbackStaging(); + } } bool Arm::isMoving() const noexcept { - return j0.isMoving() || j1.isMoving() || j2.isMoving(); + return j0.isMoving() || j1.isMoving() || j2.isMoving() || j3.isMoving() || j4.isMoving() || j5.isMoving(); } bool Arm::isEFPositionValid(std::span deadzones) const noexcept { // Otherwise: check for endeffector position - const Vector3d ef = j2.getTargetedGlobalEndpoint(); + const Vector3d ef = j5.getTargetedGlobalEndpoint(); - // If any deadzone intersects with the EF pos, fail + // If any deadzone intersects with the EF pos, fai for (const AxisAlignedBoundingBox& aabb : deadzones) { if (aabb.doesIntersect(ef)) { return false; @@ -120,14 +190,26 @@ bool Arm::rollbackJointsBasedOnValidRange() noexcept j0.rollbackStaging(); allGood = false; } - if (!j1.isPositionInRange(j0.getTargetPosition())) { + if (!j1.isPositionInRange(j1.getTargetPosition())) { j2.rollbackStaging(); allGood = false; } - if (!j2.isPositionInRange(j0.getTargetPosition())) { + 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; + } return allGood; } @@ -147,6 +229,15 @@ bool Arm::assumeHomePoseImmediate() if (!j2.goHomeImmediate()) { return false; } + if (!j3.goHomeImmediate()) { + return false; + } + if (!j4.goHomeImmediate()) { + return false; + } + if (!j5.goHomeImmediate()) { + return false; + } return true; } @@ -156,4 +247,7 @@ void Arm::update(double frametime) j0.update(frametime); j1.update(frametime); j2.update(frametime); + j3.update(frametime); + j4.update(frametime); + j5.update(frametime); } diff --git a/MotionCore/Arm.h b/MotionCore/Arm.h index 21b7e5f..3de111a 100755 --- a/MotionCore/Arm.h +++ b/MotionCore/Arm.h @@ -15,7 +15,14 @@ public: bool assumeHomePoseImmediate(); bool assumeHomePose(); - void stagePose(const double theta_j0, const double theta_j1, const double theta_j2) noexcept; + 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 commitPose() noexcept; void rollbackPose() noexcept; bool isMoving() const noexcept; @@ -28,6 +35,9 @@ public: 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; }; void update(double frametime); @@ -36,5 +46,8 @@ private: ArmSegment j0; ArmSegment j1; ArmSegment j2; + ArmSegment j3; + ArmSegment j4; + ArmSegment j5; }; diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp index ae5c4ae..b78b946 100755 --- a/MotionCore/ArmSegment.cpp +++ b/MotionCore/ArmSegment.cpp @@ -149,7 +149,7 @@ Vector3d ArmSegment::getCurrentGlobalEndpoint() const { // Collect the chain from this segment up to the root std::vector chain; - chain.reserve(3); + chain.reserve(4); const ArmSegment* s = this; while (s != nullptr) { chain.push_back(s); diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index bccd837..01f7684 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -11,8 +11,12 @@ MainLoop::MainLoop() noexcept : arm(L"COM4"), deadzones({ AxisAlignedBoundingBox( - Vector3d(-125, -125, -20), - Vector3d(65, 100, 120) + Vector3d(-125, -125, -20), + Vector3d(65, 100, 120) + ), + AxisAlignedBoundingBox( + Vector3d(0, -160, 0), + Vector3d(20000, 100, 20000) ) }), leftVKB(L" VKBsim Gladiator EVO L ") @@ -50,11 +54,14 @@ void MainLoop::run() static std::chrono::steady_clock::time_point lastBegin = std::chrono::high_resolution_clock::now(); - //YZConsolePlotter plot(-200, 200, 20, 2); + YZConsolePlotter plot(-200, 200, 20, 2); double j0Pos = 0; double j1Pos = 0; double j2Pos = 0; + double j3Pos = 0; + double j4Pos = 0; + double j5Pos = 0; while (isRunning) { @@ -79,28 +86,35 @@ 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()); + 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); + 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(); - std::cout << "Pose good!!" << std::endl; + //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(); } else { arm.rollbackPose(); - std::cout << "Hit deadzone!!" << std::endl; + //std::cout << "Hit deadzone!!" << std::endl; } - const Vector3d ef = arm.getJ2().getCurrentGlobalEndpoint(); + const Vector3d ef = arm.getJ5().getCurrentGlobalEndpoint(); //std::cout << ef << std::endl; - //plot.draw(ef); + //std::cout << arm.getJ3().getCurrentPosition() << std::endl; + plot.draw(ef); arm.update(frametime); diff --git a/MotionCore/MainLoop.h b/MotionCore/MainLoop.h index be181e0..3c4f3ff 100755 --- a/MotionCore/MainLoop.h +++ b/MotionCore/MainLoop.h @@ -20,6 +20,6 @@ private: bool isRunning = true; Arm arm; - std::array deadzones; + std::array deadzones; VKBSimController leftVKB; };