feat: abstractrize scene

MotionCore/MainLoop.cpp

@@ -1,39 +1,17 @@
 #include "MainLoop.h"
-#include <iostream>
 #include <thread>
 #include <chrono>
 #include <algorithm>
 #include <numbers>
 #include "StateMachine.h"
 #include "ArmSegment.h"
-#include "yz_plotter_win.h"
+#include "Scene_JointSpaceControl.h"
 
 MainLoop::MainLoop() noexcept :
-    arm(L"COM4"),
+    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  ")
 {
     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);
+    Scene_JointSpaceControl sceneJointSpaceControl(arm);
-
-    double j0Pos = 0;
-    double j1Pos = 0;
-    double j2Pos = 0;
-    double j3Pos = 0;
-    double j4Pos = 0;
-    double j5Pos = 0;
 
     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;
+                // HERE SCENE UPDATE
-                VkbInputs vkbi = leftVKB.get();
+                sceneJointSpaceControl.update(frametime);
-
-                // 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);
 
                 // Unset MOVING to IDLING state if no arm is moving
                 if (StateMachine::getInstance().getState() == StateMachine::STATE::MOVING && !arm.isMoving()) {

MotionCore/MainLoop.h

@@ -1,7 +1,5 @@
 #pragma once
 #include "Arm.h"
-#include "AxisAlignedBoundingBox.h"
-#include "vkb_controller.h"
 #include <array>
 
 // 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<AxisAlignedBoundingBox, 2> deadzones;
-	VKBSimController leftVKB;
 };

MotionCore/MotionCore.vcxproj

@@ -151,6 +151,7 @@
     <ClCompile Include="Matrix3x3.cpp" />
     <ClCompile Include="Motor.cpp" />
     <ClCompile Include="OSerialBus.cpp" />
+    <ClCompile Include="Scene_JointSpaceControl.cpp" />
     <ClCompile Include="StateMachine.cpp" />
     <ClCompile Include="Vector3d.cpp" />
     <ClCompile Include="vkb_controllerh.cpp" />
@@ -163,6 +164,7 @@
     <ClInclude Include="Matrix3x3.h" />
     <ClInclude Include="Motor.h" />
     <ClInclude Include="OSerialBus.h" />
+    <ClInclude Include="Scene_JointSpaceControl.h" />
     <ClInclude Include="StateMachine.h" />
     <ClInclude Include="Vector3d.h" />
     <ClInclude Include="vkb_controller.h" />

MotionCore/MotionCore.vcxproj.filters

@@ -48,6 +48,9 @@
     <ClCompile Include="StateMachine.cpp">
       <Filter>Quelldateien</Filter>
     </ClCompile>
+    <ClCompile Include="Scene_JointSpaceControl.cpp">
+      <Filter>Quelldateien</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="MainLoop.h">
@@ -83,5 +86,8 @@
     <ClInclude Include="StateMachine.h">
       <Filter>Headerdateien</Filter>
     </ClInclude>
+    <ClInclude Include="Scene_JointSpaceControl.h">
+      <Filter>Headerdateien</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

MotionCore/Scene_JointSpaceControl.cpp

@@ -0,0 +1,71 @@
+#include "Scene_JointSpaceControl.h"
+#include <iostream>
+
+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);
+}

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<AxisAlignedBoundingBox, 2> deadzones;
+	VKBSimController leftVKB;
+
+	YZConsolePlotter plot;
+
+	double j0Pos = 0;
+	double j1Pos = 0;
+	double j2Pos = 0;
+	double j3Pos = 0;
+	double j4Pos = 0;
+	double j5Pos = 0;
+};