kinematic motion controller

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