test kinematics

web/js/kinematics.js

@@ -141,7 +141,8 @@ function solve(x, y) {
   if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false };
   const r = Math.atan2(y, xpd);
   const w2 = Math.acos(cosW2);
-  const theta1 = r + w2;
+  const t1A = r + w2; // Outward
+  const t1B = r - w2; // Inward
 
   // ── Motor 2 (Right, pivot at +d2, 0 = +12.9 mm) ──────────────
   // xmd = x - d2 is the X component of (target – M2_pivot).
@@ -152,9 +153,51 @@ function solve(x, y) {
   if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
   const q = Math.atan2(y, xmd);
   const w1 = Math.acos(cosW1);
-  const theta2 = q - w1;
+  const t2A = q - w1; // Outward
+  const t2B = q + w1; // Inward
 
-  return { theta1, theta2, reachable: true };
+  // 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 };
 }
 
 // ── Forward kinematics ───────────────────────────────────────────