implement simgle joint level control for debugging

MotionCore/Arm.cpp

@@ -61,6 +61,21 @@ bool Arm::assumeHomePose()
 	return true;
 }
 
+bool Arm::moveJ0(double theta)
+{
+	return j0.moveTo(theta, std::nullopt);
+}
+
+bool Arm::moveJ1(double theta)
+{
+	return j1.moveTo(theta, std::nullopt);
+}
+
+bool Arm::moveJ2(double theta)
+{
+	return j2.moveTo(theta, std::nullopt);
+}
+
 Arm::~Arm()
 {
 	oSerialBus.close();

MotionCore/Arm.h

@@ -13,6 +13,10 @@ public:
 	bool assumeHomePoseImmediate();
 	bool assumeHomePose();
 
+	bool moveJ0(double theta);
+	bool moveJ1(double theta);
+	bool moveJ2(double theta);
+
 	void update(double frametime);
 
 private:

MotionCore/ArmSegment.cpp

@@ -16,6 +16,8 @@ ArmSegment::ArmSegment(
 	rotationAxis(rotationAxis),
 	longitudinalAxis(longitudinalAxis),
 	length(length),
+	mechanicalToVirtualAngleOffset(mechanicalToVirtualAngleOffset),
+	homeAngle(homeAngle),
 	motor(
 		motorIndex,
 		serialBus,

MotionCore/ArmSegment.h

@@ -48,6 +48,7 @@ public:
 	void setSpeed(const double speed) noexcept { motor.setSpeed(speed); };
 	double getMechanicalMinAngle() const { return motor.getMechanicalMinPosition(); };
 	double getMechanicalMaxAngle() const { return motor.getMechanicalMaxPosition(); };
+	double getHomeANgle() const noexcept { return homeAngle; };
 	double getSpeed() const noexcept { return motor.getSpeed(); };
 	ArmSegment* getParent() { return parent; };
 	ArmSegment const* getParent() const { return parent; };

MotionCore/MainLoop.cpp

@@ -2,11 +2,15 @@
 #include <iostream>
 #include <thread>
 #include <chrono>
+#include <Windows.h>
 
 MainLoop::MainLoop() noexcept:
     arm(L"COM4")
 {
-    arm.assumeHomePoseImmediate();
+    if (!arm.assumeHomePoseImmediate()) {
+        std::cerr << "Unable to move arm to home!" << std::endl;
+        exit(-1);
+    }
 }
 
 MainLoop& MainLoop::getInstance()
@@ -21,14 +25,55 @@ MainLoop::~MainLoop() noexcept
 
 void MainLoop::run()
 {
-    static std::chrono::steady_clock::time_point lastBegin = std::chrono::high_resolution_clock::now();
+    static std::chrono::steady_clock::time_point lastBegin =
+        std::chrono::high_resolution_clock::now();
 
-    while (isRunning) {
+    double j1Position = 0.0;     // start at 0
-        std::chrono::steady_clock::time_point begin = std::chrono::high_resolution_clock::now();
+    const double speed = 1000.0;  // units per second
-        const double frametime = (begin - lastBegin).count() * 10e-7; // in milliseconds
+
+    std::cout << "Main loop started\n";
+    std::cout << "Use UP and DOWN arrows to move J1\n";
+    std::cout << "Press ESC to exit\n\n";
+
+    while (isRunning)
+    {
+        std::chrono::steady_clock::time_point begin =
+            std::chrono::high_resolution_clock::now();
+
+        const double frametime =
+            (begin - lastBegin).count() * 10e-7; // milliseconds
+
+        // -------- INPUT --------
+        if (GetAsyncKeyState(VK_UP) & 0x8000)
+        {
+            j1Position += speed * frametime;
+            arm.moveJ1(j1Position);
+        }
+
+        if (GetAsyncKeyState(VK_DOWN) & 0x8000)
+        {
+            j1Position -= speed * frametime;
+            arm.moveJ1(j1Position);
+        }
+
+        if (GetAsyncKeyState(VK_ESCAPE) & 0x8000)
+        {
+            std::cout << "\nExit requested\n";
+            isRunning = false;
+        }
 
         arm.update(frametime);
 
+        // -------- STATUS OUTPUT --------
+        std::cout << "\rJ1: " << std::fixed << std::setprecision(3)
+            << j1Position
+            << " | Frame time (ms): "
+            << std::setprecision(3)
+            << frametime
+            << std::flush;
+
         lastBegin = std::chrono::high_resolution_clock::now();
     }
+
+    std::cout << "\nMain loop stopped\n";
 }