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; }
+};