From 57ea3f7b4ae15930a095028a6538550462948ecb Mon Sep 17 00:00:00 2001
From: osiu97 <osiu97@gmail.com>
Date: Wed, 8 Jul 2026 18:14:17 +0200
Subject: [PATCH] kinematic motion controller

---
 gemini.md                        |   8 ++
 web/js/motion.js                 | 186 +++++++++++++++++++++++++++++++
 web/js/sections/board-control.js |  53 +--------
 web/js/sections/input-text.js    |  41 +------
 web/js/sections/stepper-test.js  |  31 ++----
 web/js/sequence-runner.js        |  39 ++-----
 6 files changed, 220 insertions(+), 138 deletions(-)
 create mode 100644 web/js/motion.js

diff --git a/gemini.md b/gemini.md
index fdeef6c..3f820c3 100644
--- a/gemini.md
+++ b/gemini.md
@@ -176,6 +176,14 @@ The sidebar is in `index.html` (not in any section). It is a collapsible panel o
 hamburger (`☰`) button. It contains `<button data-route="...">` nav items that call
 `Router.navigate(route)`. The active item gets `class="active"` via the router.
 
+### Kinematic Motion Controller (`web/js/motion.js`)
+
+Centralised trajectory generation and streaming module.
+- **Why**: Moving side-to-side linearly in joint space causes the arms to arc dangerously out-of-bounds at the top.
+- **How it works**: Intercepts `Motion.goto(x, y)`, slices the Cartesian straight line into small segments (max `25 mm` by default, configurable in `Motion.CONFIG.SEGMENT_SIZE_MM`), and streams them to the ESP32.
+- **Dynamic Blending**: It monitors `P:` positional updates from the ESP32 and fires the next waypoint *before* `AccelStepper` decelerates (when within `BLEND_THRESHOLD_STEPS`). This produces a seamless Cartesian trajectory with no MCU code changes and allows for future "visual flavor" movement effects (e.g. snaky, erratic).
+- **Simulation Sync**: In simulation mode, `Motion.goto()` instantly updates its internal steps and manually dispatches a fake `ble:status` event containing `P:s1,s2`. This allows all UI components to effortlessly sync their visual state using their existing BLE listeners.
+
 ---
 
 ## 6. Kinematics (`web/js/kinematics.js`)
diff --git a/web/js/motion.js b/web/js/motion.js
new file mode 100644
index 0000000..925b9e6
--- /dev/null
+++ b/web/js/motion.js
@@ -0,0 +1,186 @@
+import BLE from './ble.js';
+import IK from './kinematics.js';
+import UI from './ui.js';
+
+const CONFIG = {
+  SEGMENT_SIZE_MM: 25, // Maximum distance between waypoints to prevent out-of-bounds joint-space arcs
+  BLEND_THRESHOLD_STEPS: 300 // Send next waypoint when within this many steps (prevents deceleration)
+};
+
+// Global state tracking
+let steps1 = 0;
+let steps2 = 0;
+
+// Listen to BLE position updates
+document.addEventListener('ble:status', (e) => {
+  const msg = e.detail;
+  if (msg.startsWith('P:')) {
+    const [s1, s2] = msg.slice(2).split(',').map(Number);
+    steps1 = s1;
+    steps2 = s2;
+  }
+});
+
+// To allow UI sections to override/sync steps initially
+function setSteps(s1, s2) {
+  steps1 = s1;
+  steps2 = s2;
+}
+
+function getSteps() {
+  return { steps1, steps2 };
+}
+
+let motionQueue = [];
+let isMoving = false;
+let currentResolve = null;
+let queueCheckInterval = null;
+
+async function executeQueue() {
+  if (motionQueue.length === 0) {
+    isMoving = false;
+    if (queueCheckInterval) clearInterval(queueCheckInterval);
+    if (currentResolve) currentResolve(true);
+    currentResolve = null;
+    return;
+  }
+
+  isMoving = true;
+  let target = motionQueue.shift();
+  
+  const cmd = `X:${target.s1},${target.s2}`;
+  if (BLE.isConnected()) {
+    try {
+      await BLE.write(cmd);
+    } catch (e) {
+      UI.log(`BLE error: ${e.message}`, 'error');
+      motionQueue = []; // clear queue on error
+      isMoving = false;
+      if (queueCheckInterval) clearInterval(queueCheckInterval);
+      if (currentResolve) currentResolve(false);
+      currentResolve = null;
+      return;
+    }
+    
+    // Setup interval to monitor progress
+    if (queueCheckInterval) clearInterval(queueCheckInterval);
+    queueCheckInterval = setInterval(async () => {
+      // Calculate distance to target in steps
+      const dist1 = Math.abs(target.s1 - steps1);
+      const dist2 = Math.abs(target.s2 - steps2);
+      const maxDist = Math.max(dist1, dist2);
+
+      if (motionQueue.length > 0) {
+        // We have more waypoints. Blend!
+        if (target.mode === 'direct') {
+          if (maxDist <= CONFIG.BLEND_THRESHOLD_STEPS) {
+             clearInterval(queueCheckInterval);
+             executeQueue(); // start next waypoint early
+          }
+        } else if (target.mode === 'erratic') {
+          if (maxDist <= 5) { // wait for full stop
+             clearInterval(queueCheckInterval);
+             setTimeout(executeQueue, 150); // slight pause for erratic effect
+          }
+        } else {
+           // Default to direct blending
+           if (maxDist <= CONFIG.BLEND_THRESHOLD_STEPS) {
+             clearInterval(queueCheckInterval);
+             executeQueue();
+          }
+        }
+      } else {
+        // Last waypoint. Wait until closely reached.
+        if (maxDist <= 10) {
+          clearInterval(queueCheckInterval);
+          motionQueue = [];
+          executeQueue(); // Will trigger resolve
+        }
+      }
+    }, 40); // check ~25 times a second
+    
+  } else {
+    // Simulation: instantly jump to final target
+    const finalTarget = motionQueue.length > 0 ? motionQueue[motionQueue.length - 1] : target;
+    steps1 = finalTarget.s1;
+    steps2 = finalTarget.s2;
+    console.log(`[sim] Jumped to X:${steps1},${steps2} via trajectory`);
+    
+    // Trigger pseudo-status event for the UI to update markers
+    document.dispatchEvent(new CustomEvent('ble:status', { detail: `P:${steps1},${steps2}` }));
+    
+    motionQueue = [];
+    isMoving = false;
+    if (currentResolve) {
+      currentResolve(true);
+      currentResolve = null;
+    }
+  }
+}
+
+/**
+ * Moves the arm to Cartesian targetX, targetY.
+ * Dynamically slices the path into segments to prevent out-of-bounds arcs.
+ * @returns {Promise<boolean>} Resolves to true if successful, false if aborted/error.
+ */
+async function goto(targetX, targetY, mode = 'direct') {
+  if (isMoving) {
+    UI.log('Arm is currently moving! Wait for it to finish.', 'warn');
+    return false;
+  }
+
+  // Calculate current Cartesian position
+  const t1 = IK.stepsToRad(steps1);
+  const t2 = IK.stepsToRad(steps2);
+  const currentPos = IK.forward(t1, t2);
+
+  if (!currentPos.valid || !isFinite(currentPos.endX) || !isFinite(currentPos.endY)) {
+    UI.log('Cannot calculate current IK position (arms disconnected?). Force Home needed.', 'error');
+    return false;
+  }
+
+  const dx = targetX - currentPos.endX;
+  const dy = targetY - currentPos.endY;
+  const distance = Math.sqrt(dx * dx + dy * dy);
+
+  let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
+  if (numSegments === 0) numSegments = 1;
+
+  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
+    }
+
+    const res = IK.solve(px, py);
+    if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
+      UI.log(`Waypoint geometrically unreachable.`, 'error');
+      return false;
+    }
+
+    const s1 = IK.radToStepsAbsolute(res.theta1, 1);
+    const s2 = IK.radToStepsAbsolute(res.theta2, 2);
+    
+    motionQueue.push({ s1, s2, mode });
+  }
+
+  return new Promise((resolve) => {
+    currentResolve = resolve;
+    executeQueue();
+  });
+}
+
+export default {
+  CONFIG,
+  goto,
+  setSteps,
+  getSteps,
+  get isMoving() { return isMoving; }
+};
diff --git a/web/js/sections/board-control.js b/web/js/sections/board-control.js
index a6ca918..feaf3d4 100644
--- a/web/js/sections/board-control.js
+++ b/web/js/sections/board-control.js
@@ -2,6 +2,7 @@ import BLE from '../ble.js';
 import UI from '../ui.js';
 import IK from '../kinematics.js';
 import Settings from '../settings.js';
+import Motion from '../motion.js';
 import { buildQuickSequencesHTML, initQuickSequences } from '../quick-sequences.js';
 
 let eventCleanup = [];
@@ -16,6 +17,7 @@ function syncStateFromStorage() {
   isHomed = localStorage.getItem('wiji_homed') === 'true';
   steps1 = isHomed ? parseInt(localStorage.getItem('wiji_steps1') || IK.ARM.HOME_STEPS.m1) : IK.ARM.HOME_STEPS.m1;
   steps2 = isHomed ? parseInt(localStorage.getItem('wiji_steps2') || IK.ARM.HOME_STEPS.m2) : IK.ARM.HOME_STEPS.m2;
+  Motion.setSteps(steps1, steps2);
 }
 
 function saveSteps() {
@@ -255,54 +257,8 @@ async function executeMove(spot) {
   }
 
   const mode = document.getElementById('mode-select').value;
-  if (mode !== 'direct') {
-    UI.log(`Mode '${mode}' not fully implemented yet! Using direct move.`, 'warn');
-  }
-
-  // Calculate IK
-  const wsCheck = IK.checkWorkspace(spot.x, spot.y);
-  if (!wsCheck.ok) {
-    UI.log(`⛔ ${wsCheck.reason}`, 'error');
-    return;
-  }
-
-  const res = IK.solve(spot.x, spot.y);
-  if (!res.reachable) {
-    UI.log(`Target ${spot.label} is unreachable geometrically.`, 'error');
-    return;
-  }
-  
-  if (IK.armsCrossed(res.theta1, res.theta2)) {
-    UI.log(`Target ${spot.label} rejected: elbows crossed!`, 'error');
-    return;
-  }
-
-  const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1);
-  const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2);
-  const delta1 = newSteps1 - steps1;
-  const delta2 = newSteps2 - steps2;
-
-  const cmd1 = `S1${delta1 >= 0 ? '+' : ''}${delta1}`;
-  const cmd2 = `S2${delta2 >= 0 ? '+' : ''}${delta2}`;
-
-  UI.log(`Moving to ${spot.label} (${spot.x}, ${spot.y})`, 'success');
-
-  if (BLE.isConnected()) {
-    try {
-      await BLE.write(cmd1);
-      await BLE.write(cmd2);
-    } catch (e) {
-      UI.log(`BLE error: ${e.message}`, 'error');
-    }
-  } else {
-    steps1 = newSteps1;
-    steps2 = newSteps2;
-    saveSteps();
-    updatePositionReadout();
-    const simMsg = `[sim] ${cmd1}  ${cmd2}`;
-    console.log(simMsg);
-    UI.log(simMsg, 'info');
-  }
+  UI.log(`Moving to ${spot.label} (${spot.x}, ${spot.y}) [${mode}]`, 'success');
+  await Motion.goto(spot.x, spot.y, mode);
 }
 
 export default {
@@ -369,6 +325,7 @@ export default {
       } else {
         steps1 = IK.ARM.HOME_STEPS.m1;
         steps2 = IK.ARM.HOME_STEPS.m2;
+        Motion.setSteps(steps1, steps2);
         saveSteps();
         updatePositionReadout();
         UI.log('[sim] HOMEALL', 'info');
diff --git a/web/js/sections/input-text.js b/web/js/sections/input-text.js
index be9ce54..42520b8 100644
--- a/web/js/sections/input-text.js
+++ b/web/js/sections/input-text.js
@@ -2,6 +2,7 @@ import BLE from '../ble.js';
 import UI from '../ui.js';
 import IK from '../kinematics.js';
 import Settings from '../settings.js';
+import Motion from '../motion.js';
 import { buildQuickSequencesHTML, initQuickSequences } from '../quick-sequences.js';
 
 let eventCleanup = [];
@@ -17,6 +18,7 @@ function syncStateFromStorage() {
   isHomed = localStorage.getItem('wiji_homed') === 'true';
   steps1 = isHomed ? parseInt(localStorage.getItem('wiji_steps1') || IK.ARM.HOME_STEPS.m1) : IK.ARM.HOME_STEPS.m1;
   steps2 = isHomed ? parseInt(localStorage.getItem('wiji_steps2') || IK.ARM.HOME_STEPS.m2) : IK.ARM.HOME_STEPS.m2;
+  Motion.setSteps(steps1, steps2);
 }
 
 function saveSteps() {
@@ -263,44 +265,8 @@ function delay(ms) {
 }
 
 async function executeMove(spot) {
-  const wsCheck = IK.checkWorkspace(spot.x, spot.y);
-  if (!wsCheck.ok) {
-    UI.log(`⛔ ${wsCheck.reason}`, 'error');
-    return false;
-  }
-
-  const res = IK.solve(spot.x, spot.y);
-  if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
-    UI.log(`Target ${spot.label} is unreachable or crosses arms.`, 'error');
-    return false;
-  }
-
-  const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1);
-  const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2);
-  const delta1 = newSteps1 - steps1;
-  const delta2 = newSteps2 - steps2;
-
-  const cmd1 = `S1${delta1 >= 0 ? '+' : ''}${delta1}`;
-  const cmd2 = `S2${delta2 >= 0 ? '+' : ''}${delta2}`;
-
   UI.log(`Spelling: ${spot.label}`, 'info');
-
-  if (BLE.isConnected()) {
-    try {
-      await BLE.write(cmd1);
-      await BLE.write(cmd2);
-    } catch (e) {
-      UI.log(`BLE error: ${e.message}`, 'error');
-      return false;
-    }
-  } else {
-    steps1 = newSteps1;
-    steps2 = newSteps2;
-    saveSteps();
-    updatePosMarker();
-    console.log(`[sim] ${cmd1}  ${cmd2}`);
-  }
-  return true;
+  return await Motion.goto(spot.x, spot.y);
 }
 
 async function spellText(text) {
@@ -414,6 +380,7 @@ export default {
       } else {
         steps1 = IK.ARM.HOME_STEPS.m1;
         steps2 = IK.ARM.HOME_STEPS.m2;
+        Motion.setSteps(steps1, steps2);
         saveSteps();
         updatePosMarker();
         UI.log('[sim] HOMEALL', 'info');
diff --git a/web/js/sections/stepper-test.js b/web/js/sections/stepper-test.js
index 2d52c6b..c5df155 100644
--- a/web/js/sections/stepper-test.js
+++ b/web/js/sections/stepper-test.js
@@ -15,6 +15,7 @@
 import BLE from '../ble.js';
 import IK from '../kinematics.js';
 import UI from '../ui.js';
+import Motion from '../motion.js';
 
 // ── SVG viewport ──────────────────────────────────────────────────
 // We map the real workspace onto the SVG canvas.
@@ -59,6 +60,7 @@ function syncStateFromStorage() {
     steps1 = 1024;
     localStorage.setItem('wiji_steps1', 1024);
   }
+  Motion.setSteps(steps1, steps2);
 }
 
 function saveSteps() {
@@ -186,36 +188,14 @@ function showTargetMarker(sx, sy) {
 // ─────────────────────────────────────────────────────────────────
 
 async function moveToXY(targetX, targetY) {
-  // Check workspace limits first
+  // Check workspace limits first (quick check before queue)
   const wsCheck = IK.checkWorkspace(targetX, targetY);
   if (!wsCheck.ok) {
     UI.log(`⛔ ${wsCheck.reason}`, 'warn');
     return false;
   }
 
-  const { theta1, theta2 } = IK.solve(targetX, targetY);
-
-  const newSteps1 = IK.radToStepsAbsolute(theta1, 1);
-  const newSteps2 = IK.radToStepsAbsolute(theta2, 2);
-
-  const cmd = `X:${newSteps1},${newSteps2}`;
-
-  if (BLE.isConnected()) {
-    try {
-      await BLE.write(cmd);
-      // Do not update steps1/steps2 here; let the P: notify handler update the UI smoothly
-    } catch (e) {
-      UI.log(`BLE error: ${e.message}`, 'error');
-    }
-  } else {
-    steps1 = newSteps1;
-    steps2 = newSteps2;
-    renderArm(theta1, theta2);
-    const simMsg = `[sim] IK→ θ1=${IK.radToDeg(theta1).toFixed(1)}° θ2=${IK.radToDeg(theta2).toFixed(1)}° | ${cmd}`;
-    console.log(simMsg);
-    UI.log(simMsg, 'info');
-  }
-  return true;
+  return await Motion.goto(targetX, targetY);
 }
 
 // ─────────────────────────────────────────────────────────────────
@@ -229,6 +209,7 @@ async function jogMotor(motor, delta) {
   } else {
     if (motor === 1) steps1 += delta;
     else steps2 += delta;
+    Motion.setSteps(steps1, steps2);
     renderArmFromSteps();
     console.log(`[sim] ${cmd}`);
     UI.log(`[sim] ${cmd}`, 'info');
@@ -249,6 +230,7 @@ async function zeroMotor(motor) {
     } else {
       steps2 = IK.ARM.HOME_STEPS.m2;
     }
+    Motion.setSteps(steps1, steps2);
     renderArmFromSteps();
     console.log(`[sim] ${cmd}`);
     UI.log(`[sim] ${cmd}`, 'info');
@@ -752,6 +734,7 @@ const StepperTestSection = {
       } else {
         steps1 = IK.ARM.HOME_STEPS.m1;
         steps2 = IK.ARM.HOME_STEPS.m2;
+        Motion.setSteps(steps1, steps2);
         renderArmFromSteps();
         console.log(`[sim] ZEROALL`);
         UI.log(`[sim] ZEROALL (Reset Position)`, 'info');
diff --git a/web/js/sequence-runner.js b/web/js/sequence-runner.js
index 400a8e8..d90c60f 100644
--- a/web/js/sequence-runner.js
+++ b/web/js/sequence-runner.js
@@ -2,6 +2,7 @@ import BLE from './ble.js';
 import UI from './ui.js';
 import IK from './kinematics.js';
 import Settings from './settings.js';
+import Motion from './motion.js';
 
 let isRunning = false;
 
@@ -52,41 +53,21 @@ export default {
         continue;
       }
 
-      const wsCheck = IK.checkWorkspace(targetX, targetY);
-      if (!wsCheck.ok) {
-        UI.log(`⛔ ${wsCheck.reason} [${label}]`, 'error');
-        continue; // or break? let's continue
-      }
-
-      const res = IK.solve(targetX, targetY);
-      if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
-        UI.log(`Target ${label} is unreachable or crosses arms.`, 'error');
-        continue;
-      }
-
-      const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1);
-      const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2);
-
-      const cmd = `X:${newSteps1},${newSteps2}`;
-
       UI.log(`Seq step: ${label}`, 'info');
 
-      if (BLE.isConnected()) {
-        try {
-          await BLE.write(cmd);
-        } catch (e) {
-          UI.log(`BLE error: ${e.message}`, 'error');
-          isRunning = false;
-          return false;
-        }
-      } else {
-        steps1 = newSteps1;
-        steps2 = newSteps2;
+      const success = await Motion.goto(targetX, targetY, step.mode || 'direct');
+      if (!success) {
+         isRunning = false;
+         return false;
+      }
 
+      if (!BLE.isConnected()) {
+        const st = Motion.getSteps();
+        steps1 = st.steps1;
+        steps2 = st.steps2;
         if (onStepComplete) {
           onStepComplete(steps1, steps2);
         }
-        console.log(`[sim] ${cmd}`);
       }
 
       // Wait for delay