diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 3dcae2a..3f21a3e 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -1,39 +1,17 @@ #include "MainLoop.h" -#include #include #include #include #include #include "StateMachine.h" #include "ArmSegment.h" -#include "yz_plotter_win.h" +#include "Scene_JointSpaceControl.h" MainLoop::MainLoop() noexcept : - arm(L"COM4"), - deadzones({ - AxisAlignedBoundingBox( - Vector3d(-125, -125, -20), - Vector3d(65, 100, 120) - ), - AxisAlignedBoundingBox( - Vector3d(0, -160, 0), - Vector3d(20000, 100, 20000) - ) - }), - leftVKB(L" VKBsim Gladiator EVO L ") + arm(L"COM4") { StateMachine::getInstance(); - if (!arm.assumeHomePoseImmediate()) { - std::cerr << "Unable to move arm to home!" << std::endl; - exit(-1); - } - - if (!leftVKB.connect()) { - std::cerr << "Unable to connect to left VKB controller!" << std::endl; - exit(-1); - } - StateMachine::getInstance().setState(StateMachine::STATE::IDLING); } @@ -55,14 +33,7 @@ void MainLoop::run() static std::chrono::steady_clock::time_point lastBegin = std::chrono::high_resolution_clock::now(); - YZConsolePlotter plot(-200, 200, 20, 2); - - double j0Pos = 0; - double j1Pos = 0; - double j2Pos = 0; - double j3Pos = 0; - double j4Pos = 0; - double j5Pos = 0; + Scene_JointSpaceControl sceneJointSpaceControl(arm); while (isRunning) { @@ -80,44 +51,8 @@ void MainLoop::run() switch (StateMachine::getInstance().getState()) { case StateMachine::STATE::IDLING: case StateMachine::STATE::MOVING: { - constexpr double speed = 5000; - VkbInputs vkbi = leftVKB.get(); - - // Calculate new joint positions - j0Pos += -vkbi.roll * speed * frametime; - j1Pos += -vkbi.pitch * speed * frametime; - j2Pos += -vkbi.yaw * speed * frametime; - 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 }); - if (arm.isEFPositionValid(deadzones)) { - // Roll back individual joints if their angles are out of range - arm.rollbackJointsBasedOnValidRange(); - - //std::cout << "Pose good!!" << std::endl; - arm.commitPose(); - - 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.getEndEffectorJoint().getCurrentGlobalEndpoint(); - //std::cout << ef << std::endl; - //std::cout << arm.getJ3().getCurrentPosition() << std::endl; - plot.draw(ef); - - arm.update(frametime); + // HERE SCENE UPDATE + sceneJointSpaceControl.update(frametime); // Unset MOVING to IDLING state if no arm is moving if (StateMachine::getInstance().getState() == StateMachine::STATE::MOVING && !arm.isMoving()) { diff --git a/MotionCore/MainLoop.h b/MotionCore/MainLoop.h index 3c4f3ff..a976fbe 100755 --- a/MotionCore/MainLoop.h +++ b/MotionCore/MainLoop.h @@ -1,7 +1,5 @@ #pragma once #include "Arm.h" -#include "AxisAlignedBoundingBox.h" -#include "vkb_controller.h" #include // Singleton-class, use MainLoop::getInstance() to get the only instance of the runtime @@ -18,8 +16,5 @@ private: MainLoop() noexcept; bool isRunning = true; - Arm arm; - std::array deadzones; - VKBSimController leftVKB; }; diff --git a/MotionCore/MotionCore.vcxproj b/MotionCore/MotionCore.vcxproj index 8f8f2eb..7c8a050 100755 --- a/MotionCore/MotionCore.vcxproj +++ b/MotionCore/MotionCore.vcxproj @@ -151,6 +151,7 @@ + @@ -163,6 +164,7 @@ + diff --git a/MotionCore/MotionCore.vcxproj.filters b/MotionCore/MotionCore.vcxproj.filters index 4703f20..b59b3f8 100755 --- a/MotionCore/MotionCore.vcxproj.filters +++ b/MotionCore/MotionCore.vcxproj.filters @@ -48,6 +48,9 @@ Quelldateien + + Quelldateien + @@ -83,5 +86,8 @@ Headerdateien + + Headerdateien + \ No newline at end of file diff --git a/MotionCore/Scene_JointSpaceControl.cpp b/MotionCore/Scene_JointSpaceControl.cpp new file mode 100755 index 0000000..bf07b92 --- /dev/null +++ b/MotionCore/Scene_JointSpaceControl.cpp @@ -0,0 +1,71 @@ +#include "Scene_JointSpaceControl.h" +#include + +Scene_JointSpaceControl::Scene_JointSpaceControl(Arm& arm): + arm{arm}, + deadzones({ + AxisAlignedBoundingBox( + Vector3d(-125, -125, -20), + Vector3d(65, 100, 120) + ), + AxisAlignedBoundingBox( + Vector3d(0, -160, 0), + Vector3d(20000, 100, 20000) + ) + }), + leftVKB(L" VKBsim Gladiator EVO L "), + plot(-200, 200, 20, 2) +{ + + if (!arm.assumeHomePoseImmediate()) { + std::cerr << "Unable to move arm to home!" << std::endl; + exit(-1); + } + + if (!leftVKB.connect()) { + std::cerr << "Unable to connect to left VKB controller!" << std::endl; + exit(-1); + } +} + +void Scene_JointSpaceControl::update(double frametime) +{ + constexpr double speed = 5000; + VkbInputs vkbi = leftVKB.get(); + + // Calculate new joint positions + j0Pos += -vkbi.roll * speed * frametime; + j1Pos += -vkbi.pitch * speed * frametime; + j2Pos += -vkbi.yaw * speed * frametime; + 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 }); + if (arm.isEFPositionValid(deadzones)) { + // Roll back individual joints if their angles are out of range + arm.rollbackJointsBasedOnValidRange(); + + //std::cout << "Pose good!!" << std::endl; + arm.commitPose(); + + 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.getEndEffectorJoint().getCurrentGlobalEndpoint(); + //std::cout << ef << std::endl; + //std::cout << arm.getJ3().getCurrentPosition() << std::endl; + plot.draw(ef); + + arm.update(frametime); +} diff --git a/MotionCore/Scene_JointSpaceControl.h b/MotionCore/Scene_JointSpaceControl.h new file mode 100755 index 0000000..1db3fbb --- /dev/null +++ b/MotionCore/Scene_JointSpaceControl.h @@ -0,0 +1,27 @@ +#pragma once +#include "Arm.h" +#include "AxisAlignedBoundingBox.h" +#include "vkb_controller.h" +#include "yz_plotter_win.h" + +class Scene_JointSpaceControl +{ +public: + Scene_JointSpaceControl(Arm& arm); + + void update(double frametime); + +private: + Arm& arm; + std::array deadzones; + VKBSimController leftVKB; + + YZConsolePlotter plot; + + double j0Pos = 0; + double j1Pos = 0; + double j2Pos = 0; + double j3Pos = 0; + double j4Pos = 0; + double j5Pos = 0; +};