diff --git a/web/js/kinematics.js b/web/js/kinematics.js index 9ff855f..0f2bc1d 100644 --- a/web/js/kinematics.js +++ b/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 ───────────────────────────────────────────