From bc853ce4e1661f0f62c136bccc9d9588783b7abb Mon Sep 17 00:00:00 2001
From: Leonetienne <leon@etiennes.de>
Date: Sun, 14 Dec 2025 22:37:39 +0100
Subject: [PATCH] implement regulated j2

---
 MotionCore/ArmSegment.h |  3 +++
 MotionCore/MainLoop.cpp | 29 ++++++++++++++++++++++-------
 2 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/MotionCore/ArmSegment.h b/MotionCore/ArmSegment.h
index 01ba0ac..7934c37 100755
--- a/MotionCore/ArmSegment.h
+++ b/MotionCore/ArmSegment.h
@@ -41,6 +41,9 @@ public:
 	void resumeMoving() noexcept;
 	// Will move the motor to its current target instantly (this is jerky and may damage the hardware long-term)
 	bool assumeTargetPositionImmediately() noexcept;
+	bool isPositionInRange(const double pos) const noexcept { return motor.isPositionInRange(pos); };
+	double getCurrentPosition() const noexcept { return motor.getCurrentPosition(); };
+	double getTargetPosition() const noexcept { return motor.getTargetPosition(); };
 
 	void update(const double frametime);
 
diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp
index 855e090..bf675bb 100755
--- a/MotionCore/MainLoop.cpp
+++ b/MotionCore/MainLoop.cpp
@@ -45,6 +45,11 @@ void MainLoop::run()
 
    //YZConsolePlotter plot(-200, 200, 20, 2);
 
+    double j0Pos = 0;
+    double j1Pos = 0;
+    double j2Pos = 0;
+    double j2Off = 0;
+
     while (isRunning)
     {
         std::chrono::steady_clock::time_point begin =
@@ -61,16 +66,26 @@ void MainLoop::run()
         constexpr double speed = 3000;
         VkbInputs vkbi = leftVKB.get();
 
-        const double moveJ0By = -vkbi.roll * speed * frametime;
-        const double moveJ1By = -vkbi.pitch * speed * frametime;
-        const double moveJ2By = -vkbi.yaw * speed * frametime;
+        // Calculate new joint positions
+        j0Pos += -vkbi.roll * speed * frametime;
+        const double newJ1Pos = j1Pos + -vkbi.pitch * speed * frametime;
+        const double newJ2Off = j2Off + -vkbi.yaw * speed * frametime;
+        const double newJ2Pos = newJ2Off - newJ1Pos;
 
-        arm.getJ0().moveBy(moveJ0By, std::nullopt);
-        arm.getJ1().moveBy(moveJ1By, std::nullopt);
-        arm.getJ2().moveBy(moveJ2By, std::nullopt);
+        arm.getJ0().moveTo(j0Pos, std::nullopt);
+        // J1 and J2 are coupled. Only apply their position if they both are valid
+        if (
+           arm.getJ1().isPositionInRange(newJ1Pos) &&
+           arm.getJ2().isPositionInRange(newJ2Pos)) {
+            j1Pos = newJ1Pos;
+            j2Pos = newJ2Pos;
+            j2Off = newJ2Off;
+            arm.getJ1().moveTo(j1Pos, std::nullopt);
+            arm.getJ2().moveTo(j2Pos, std::nullopt);
+        }
        
         const Vector3d ef = arm.getJ2().getGlobalEndpoint();
-        std::cout << ef << std::endl;
+        std::cout << j2Pos << std::endl;
         //plot.draw(ef);
 
         arm.update(frametime);