From c457910bfa57eb007be1794a2b74184a8a6cdd9a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?PROFERIS=20-=20Mi=C2=B3osz=20Stocki?=
 <milosz.stocki@proferis.pl>
Date: Wed, 8 Jul 2026 12:51:45 +0200
Subject: [PATCH] more kinematics

---
 src/main.cpp         | 73 +++++++++++++++++++++++---------------------
 web/js/kinematics.js |  2 +-
 2 files changed, 40 insertions(+), 35 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 0c5d274..8029531 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -260,70 +260,75 @@ void loop() {
 // ─── Homing Routines ──────────────────────────────────────────────
 
 void performHomingAll() {
-  // 1. Move against limits (M1 CCW, M2 CCW)
+  Serial.println("[SYS] Homing all: Phase 1 (Stall UP)");
+  
+  // 1. Swing BOTH motors UP to hit the mechanism housing (stall point)
+  // M1 (Left) swings UP by moving CW (negative)
+  // M2 (Right) swings UP by moving CCW (positive)
+  stepper1.move(-2048); 
+  stepper2.move(2048);
+  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
+    runSteppersWithNotify();
+  }
+  
+  // 2. At this point, BOTH are stalled against the mechanism housing.
+  // We know the physical angles of these stall points!
+  // M1 stall is at +474 steps (83.3 degrees).
+  // M2 stall is at +530 steps (93.1 degrees).
+  stepper1.setCurrentPosition(474);
+  stepper2.setCurrentPosition(530);
+
+  Serial.println("[SYS] Homing all: Phase 2 (Move to Outward Home)");
+
+  // 3. Move BOTH motors to their outward home positions
+  // M1 goes to +1024 (180 degrees, pointing Left)
+  // M2 goes to 0 (0 degrees, pointing Right)
   stepper1.moveTo(1024);
-  stepper2.moveTo(2048);
-  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
-    runSteppersWithNotify();
-  }
-  stepper2.setCurrentPosition(0);
-
-  // 2. Move to negative limits (CW)
-  stepper1.moveTo(-1050);
-  stepper2.move(-1300);
-  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
-    runSteppersWithNotify();
-  }
-  stepper1.setCurrentPosition(0);
-
-  // 3. Move to final home position
-  stepper1.moveTo(550);
-  stepper2.moveTo(-530);
+  stepper2.moveTo(0);
   while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
     runSteppersWithNotify();
   }
 
-  // 4. Set origin relative to this final position
-  stepper1.setCurrentPosition(-1024);
-  stepper2.setCurrentPosition(0);
-
+  // 4. Disable outputs to rest
   stepper1.disableOutputs();
   stepper2.disableOutputs();
   Serial.println("[SYS] Homing all complete.");
 }
 
 void performHoming1() {
-  // M1 (Left) stalls going negative (CW)
+  Serial.println("[S1] Homing Motor 1: Phase 1 (Stall UP)");
   stepper1.move(-2048);
   while (stepper1.distanceToGo() != 0) {
     runSteppersWithNotify();
   }
-  stepper1.setCurrentPosition(0);
-
-  // Move to final home position (CCW)
-  stepper1.moveTo(550);
+  
+  stepper1.setCurrentPosition(474);
+  
+  Serial.println("[S1] Homing Motor 1: Phase 2 (Move Outward)");
+  stepper1.moveTo(1024);
   while (stepper1.distanceToGo() != 0) {
     runSteppersWithNotify();
   }
-  stepper1.setCurrentPosition(-1024);
+  
   stepper1.disableOutputs();
   Serial.println("[S1] Homing complete.");
 }
 
 void performHoming2() {
-  // M2 (Right) stalls going positive (CCW)
+  Serial.println("[S2] Homing Motor 2: Phase 1 (Stall UP)");
   stepper2.move(2048);
   while (stepper2.distanceToGo() != 0) {
     runSteppersWithNotify();
   }
-  stepper2.setCurrentPosition(0);
-
-  // Move to final home position (CW)
-  stepper2.moveTo(-530);
+  
+  stepper2.setCurrentPosition(530);
+  
+  Serial.println("[S2] Homing Motor 2: Phase 2 (Move Outward)");
+  stepper2.moveTo(0);
   while (stepper2.distanceToGo() != 0) {
     runSteppersWithNotify();
   }
-  stepper2.setCurrentPosition(0);
+  
   stepper2.disableOutputs();
   Serial.println("[S2] Homing complete.");
 }
diff --git a/web/js/kinematics.js b/web/js/kinematics.js
index cb28915..f016ce2 100644
--- a/web/js/kinematics.js
+++ b/web/js/kinematics.js
@@ -40,7 +40,7 @@ const ARM = {
   l2: 110.0,  // distal  link length (mm)
   STEPS_PER_REV: 2048,
   STEP_ANGLE_DEG: 360 / 2048,  // ≈ 0.17578125 °/step
-  HOME_STEPS: { m1: -1024, m2: 0 }, // M1 (Left) at -180°, M2 (Right) at 0° (arms folded outward)
+  HOME_STEPS: { m1: 1024, m2: 0 }, // M1 (Left) at +180°, M2 (Right) at 0° (arms folded outward)
 };
 
 // ── WORKSPACE CONSTRAINTS ─────────────────────────────────────────