kinematic motion controller

2026-07-08 18:14:17 +02:00
parent e1416f6979
commit 57ea3f7b4a
6 changed files with 220 additions and 138 deletions
@@ -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; }
+};
@@ -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');
@@ -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');
@@ -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');
@@ -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