From ab5dc7b6787a2a15b3dc589d1902c626b70e54a2 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 16:12:27 +0200
Subject: [PATCH] new kin

---
 web/js/kinematics.js | 62 ++++++++++----------------------------------
 1 file changed, 13 insertions(+), 49 deletions(-)

diff --git a/web/js/kinematics.js b/web/js/kinematics.js
index 0f2bc1d..daa1944 100644
--- a/web/js/kinematics.js
+++ b/web/js/kinematics.js
@@ -139,65 +139,29 @@ function solve(x, y) {
   if (t < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
   const cosW2 = (l2 * l2 - t * t - l1 * l1) / (-2 * l1 * t);
   if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false };
-  const r = Math.atan2(y, xpd);
+  let r = Math.atan2(y, xpd);
+  // Fix atan2 wrap-around: shift the cut from -180° (which crosses our left workspace) 
+  // to -90° (straight down, which is safely outside our workspace).
+  if (r < -Math.PI / 2) r += 2 * Math.PI;
+  
   const w2 = Math.acos(cosW2);
-  const t1A = r + w2; // Outward
-  const t1B = r - w2; // Inward
+  const theta1 = r + w2; // Outward elbow
 
   // ── Motor 2 (Right, pivot at +d2, 0 = +12.9 mm) ──────────────
-  // xmd = x - d2 is the X component of (target – M2_pivot).
   const xmd = x - d2;
   const s = Math.sqrt(xmd * xmd + y * y);
   if (s < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
   const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
   if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
-  const q = Math.atan2(y, xmd);
+  
+  let q = Math.atan2(y, xmd);
+  // Fix atan2 wrap-around for the right motor as well
+  if (q < -Math.PI / 2) q += 2 * Math.PI;
+  
   const w1 = Math.acos(cosW1);
-  const t2A = q - w1; // Outward
-  const t2B = q + w1; // Inward
+  const theta2 = q - w1; // Outward elbow
 
-  // Evaluate all 4 possible elbow configurations
-  const combos = [
-    { t1: t1A, t2: t2A }, // Out/Out
-    { t1: t1B, t2: t2A }, // In/Out
-    { t1: t1A, t2: t2B }, // Out/In
-    { t1: t1B, t2: t2B }, // In/In
-  ];
-
-  let bestCombo = null;
-
-  for (const c of combos) {
-    if (armsCrossed(c.t1, c.t2)) continue; // Rejected by physical constraints (housing/bottom)
-    
-    // Calculate distance between elbows to avoid physical joint collision
-    const e1x = -d2 + l1 * Math.cos(c.t1);
-    const e1y = l1 * Math.sin(c.t1);
-    const e2x = +d2 + l1 * Math.cos(c.t2);
-    const e2y = l1 * Math.sin(c.t2);
-    const edist = Math.sqrt((e2x - e1x)**2 + (e2y - e1y)**2);
-    
-    // If elbows are absurdly close (< 10mm), the physical joints will collide
-    // and the FK will glitch (since intersections become highly sensitive).
-    if (edist < 10) continue;
-    
-    // We prefer the combination where elbows point UP (y > 0)
-    // Score based on how far above the bottom housing they are
-    c.score = e1y + e2y; 
-    
-    // Penalize configurations that point downwards heavily
-    if (e1y < 0) c.score -= 1000;
-    if (e2y < 0) c.score -= 1000;
-    
-    if (!bestCombo || c.score > bestCombo.score) {
-      bestCombo = c;
-    }
-  }
-
-  if (!bestCombo) {
-    return { theta1: t1A, theta2: t2A, reachable: false };
-  }
-
-  return { theta1: bestCombo.t1, theta2: bestCombo.t2, reachable: true };
+  return { theta1, theta2, reachable: true };
 }
 
 // ── Forward kinematics ───────────────────────────────────────────