From 4048ad0fe6c9ede27233ebd5def7a93fe11aa473 Mon Sep 17 00:00:00 2001
From: osiu97 <osiu97@gmail.com>
Date: Wed, 8 Jul 2026 18:23:49 +0200
Subject: [PATCH] upgrade motion pathing

---
 web/js/motion.js | 83 ++++++++++++++++++++++++++++++++++--------------
 1 file changed, 59 insertions(+), 24 deletions(-)

diff --git a/web/js/motion.js b/web/js/motion.js
index 925b9e6..5d67122 100644
--- a/web/js/motion.js
+++ b/web/js/motion.js
@@ -118,6 +118,35 @@ async function executeQueue() {
   }
 }
 
+function buildSegments(startX, startY, endX, endY, mode) {
+  const dx = endX - startX;
+  const dy = endY - startY;
+  const distance = Math.sqrt(dx * dx + dy * dy);
+
+  let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
+  if (numSegments === 0) numSegments = 1;
+
+  let segs = [];
+
+  for (let i = 1; i <= numSegments; i++) {
+    const fraction = i / numSegments;
+    const px = startX + dx * fraction;
+    const py = startY + dy * fraction;
+
+    const wsCheck = IK.checkWorkspace(px, py);
+    if (!wsCheck.ok) return null;
+
+    const res = IK.solve(px, py);
+    if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) return null;
+
+    const s1 = IK.radToStepsAbsolute(res.theta1, 1);
+    const s2 = IK.radToStepsAbsolute(res.theta2, 2);
+    
+    segs.push({ s1, s2, mode });
+  }
+  return segs;
+}
+
 /**
  * Moves the arm to Cartesian targetX, targetY.
  * Dynamically slices the path into segments to prevent out-of-bounds arcs.
@@ -129,6 +158,13 @@ async function goto(targetX, targetY, mode = 'direct') {
     return false;
   }
 
+  // Quick check on final destination
+  const destCheck = IK.checkWorkspace(targetX, targetY);
+  if (!destCheck.ok) {
+    UI.log(`⛔ Destination blocked: ${destCheck.reason}`, 'error');
+    return false;
+  }
+
   // Calculate current Cartesian position
   const t1 = IK.stepsToRad(steps1);
   const t2 = IK.stepsToRad(steps2);
@@ -139,38 +175,37 @@ async function goto(targetX, targetY, mode = 'direct') {
     return false;
   }
 
-  const dx = targetX - currentPos.endX;
-  const dy = targetY - currentPos.endY;
-  const distance = Math.sqrt(dx * dx + dy * dy);
+  let segments = buildSegments(currentPos.endX, currentPos.endY, targetX, targetY, mode);
 
-  let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
-  if (numSegments === 0) numSegments = 1;
+  if (!segments) {
+    // Attempt rerouting through safe overhead point
+    const SAFE_X = 0;
+    const SAFE_Y = 75;
 
-  motionQueue = [];
-
-  for (let i = 1; i <= numSegments; i++) {
-    const fraction = i / numSegments;
-    const px = currentPos.endX + dx * fraction;
-    const py = currentPos.endY + dy * fraction;
-
-    const wsCheck = IK.checkWorkspace(px, py);
-    if (!wsCheck.ok) {
-      UI.log(`⛔ Waypoint blocked: ${wsCheck.reason}`, 'error');
-      return false; // abort full move
+    // Prevent infinite loop if already at/near safe point
+    const distToSafe = Math.sqrt(Math.pow(currentPos.endX - SAFE_X, 2) + Math.pow(currentPos.endY - SAFE_Y, 2));
+    if (distToSafe < 5) {
+       UI.log(`⛔ Path completely blocked even via safe point`, 'error');
+       return false;
     }
 
-    const res = IK.solve(px, py);
-    if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
-      UI.log(`Waypoint geometrically unreachable.`, 'error');
+    const leg1 = buildSegments(currentPos.endX, currentPos.endY, SAFE_X, SAFE_Y, mode);
+    if (leg1) {
+      const leg2 = buildSegments(SAFE_X, SAFE_Y, targetX, targetY, mode);
+      if (leg2) {
+        segments = leg1.concat(leg2);
+        UI.log(`Rerouting around exclusion zone via (0, 75)`, 'info');
+      }
+    }
+
+    if (!segments) {
+      UI.log(`⛔ Target unreachable (path blocked by mechanism)`, 'error');
       return false;
     }
-
-    const s1 = IK.radToStepsAbsolute(res.theta1, 1);
-    const s2 = IK.radToStepsAbsolute(res.theta2, 2);
-    
-    motionQueue.push({ s1, s2, mode });
   }
 
+  motionQueue = segments;
+
   return new Promise((resolve) => {
     currentResolve = resolve;
     executeQueue();