Initial effects

2026-07-08 21:02:48 +02:00
parent 1a94ca3947
commit 0ef1e99daf
8 changed files with 243 additions and 78 deletions
@@ -1,6 +1,7 @@
 import BLE from './ble.js';
 import IK from './kinematics.js';
 import UI from './ui.js';
+import Settings from './settings.js';

 const CONFIG = {
  SEGMENT_SIZE_MM: 25, // Maximum distance between waypoints to prevent out-of-bounds joint-space arcs
@@ -47,11 +48,19 @@ let motionQueue = [];
 let isMoving = false;
 let currentResolve = null;
 let queueCheckInterval = null;
+let currentMotionMode = 'direct';
+let hasHesitated = false;

 async function executeQueue() {
  if (motionQueue.length === 0) {
    isMoving = false;
    if (queueCheckInterval) clearInterval(queueCheckInterval);
+    
+    // Restore speed if it was altered
+    if (BLE.isConnected() && ['jumpy', 'hesitant', 'overshoot'].includes(currentMotionMode)) {
+      try { await BLE.write(`SPD:${Settings.speed}`); } catch(e){}
+    }
+    
    if (currentResolve) currentResolve(true);
    currentResolve = null;
    return;
@@ -60,102 +69,184 @@ async function executeQueue() {
  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);
+  let delayBeforeSend = 0;
+  if (target.mode === 'hesitant' && target.fraction >= 0.45 && !hasHesitated) {
+     hasHesitated = true;
+     delayBeforeSend = 400;
+  } else if (target.mode === 'overshoot_settle') {
+     delayBeforeSend = 200; // brief pause before returning
+  }

-      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
+  const doSend = async () => {
+    const cmd = `X:${target.s1},${target.s2}`;
+    if (BLE.isConnected()) {
+      if (target.mode === 'overshoot_settle') {
+        try { await BLE.write(`SPD:${Math.floor(Math.max(100, Settings.speed * 0.4))}`); } catch(e){}
+      }
+      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 or wait depending on mode.
+          if (target.mode === 'erratic') {
+            if (maxDist <= 5) { // wait for full stop
+               clearInterval(queueCheckInterval);
+               setTimeout(executeQueue, 50 + Math.random() * 100); 
+            }
+          } else if (target.mode === 'jumpy') {
+            if (maxDist <= 5) { // stop and go rhythm
+               clearInterval(queueCheckInterval);
+               setTimeout(executeQueue, 100);
+            }
+          } else {
+             // direct, snaky, hesitant, etc default to direct blending
+             if (maxDist <= CONFIG.BLEND_THRESHOLD_STEPS) {
+               clearInterval(queueCheckInterval);
+               executeQueue();
+            }
          }
        } else {
-           // Default to direct blending
-           if (maxDist <= CONFIG.BLEND_THRESHOLD_STEPS) {
-             clearInterval(queueCheckInterval);
-             executeQueue();
+          // Last waypoint. Wait until closely reached.
+          if (maxDist <= 10) {
+            clearInterval(queueCheckInterval);
+            motionQueue = [];
+            executeQueue(); // Will trigger resolve
          }
        }
-      } 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;
      }
-    }, 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;
    }
+  };
+
+  if (delayBeforeSend > 0) {
+    setTimeout(doSend, delayBeforeSend);
+  } else {
+    doSend();
  }
 }

 function buildSegments(startX, startY, endX, endY, mode) {
-  const dx = endX - startX;
-  const dy = endY - startY;
+  let actualEndX = endX;
+  let actualEndY = endY;
+
+  if (mode === 'overshoot') {
+     const totalDx = endX - startX;
+     const totalDy = endY - startY;
+     const totalDist = Math.sqrt(totalDx * totalDx + totalDy * totalDy);
+     if (totalDist > 0) {
+       actualEndX = endX + (totalDx / totalDist) * 15;
+       actualEndY = endY + (totalDy / totalDist) * 15;
+       
+       const wsCheck = IK.checkWorkspace(actualEndX, actualEndY);
+       if (!wsCheck.ok) {
+           actualEndX = endX;
+           actualEndY = endY;
+       }
+     }
+  }
+
+  const dx = actualEndX - startX;
+  const dy = actualEndY - startY;
  const distance = Math.sqrt(dx * dx + dy * dy);

  let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
+  
+  if (mode === 'snaky' || mode === 'erratic') {
+    numSegments = Math.max(numSegments, Math.ceil(distance / 10)); // 10mm segments max for finer resolution
+  }
  if (numSegments === 0) numSegments = 1;

  let segs = [];

+  const nx = -dy / distance || 0;
+  const ny = dx / distance || 0;
+
  for (let i = 1; i <= numSegments; i++) {
    const fraction = i / numSegments;
-    const px = startX + dx * fraction;
-    const py = startY + dy * fraction;
+    let px = startX + dx * fraction;
+    let py = startY + dy * fraction;
+
+    if (mode === 'snaky') {
+      const cycles = Math.max(1, Math.floor(distance / 50)); 
+      const offset = Math.sin(fraction * Math.PI * 2 * cycles) * 15;
+      px += nx * offset;
+      py += ny * offset;
+    } else if (mode === 'erratic') {
+      px += (Math.random() - 0.5) * 10;
+      py += (Math.random() - 0.5) * 10;
+    }

    const wsCheck = IK.checkWorkspace(px, py);
-    if (!wsCheck.ok) return null;
+    if (!wsCheck.ok) {
+      px = startX + dx * fraction;
+      py = startY + dy * fraction;
+    }

    const res = IK.solve(px, py);
-    if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) return null;
+    if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
+       px = startX + dx * fraction;
+       py = startY + dy * fraction;
+       const fallbackRes = IK.solve(px, py);
+       if (!fallbackRes.reachable || IK.armsCrossed(fallbackRes.theta1, fallbackRes.theta2)) {
+           return null;
+       }
+       segs.push({
+           s1: IK.radToStepsAbsolute(fallbackRes.theta1, 1),
+           s2: IK.radToStepsAbsolute(fallbackRes.theta2, 2),
+           mode, 
+           fraction
+       });
+       continue;
+    }

    const s1 = IK.radToStepsAbsolute(res.theta1, 1);
    const s2 = IK.radToStepsAbsolute(res.theta2, 2);
    
-    segs.push({ s1, s2, mode });
+    segs.push({ s1, s2, mode, fraction });
  }
+
+  if (mode === 'overshoot') {
+     const res = IK.solve(endX, endY);
+     if (res.reachable && !IK.armsCrossed(res.theta1, res.theta2)) {
+        const s1 = IK.radToStepsAbsolute(res.theta1, 1);
+        const s2 = IK.radToStepsAbsolute(res.theta2, 2);
+        segs.push({ s1, s2, mode: 'overshoot_settle', fraction: 1.1 });
+     }
+  }
+
  return segs;
 }

@@ -170,6 +261,14 @@ async function goto(targetX, targetY, mode = 'direct') {
    return false;
  }

+  if (mode === 'random') {
+    const modes = ['direct', 'snaky', 'erratic', 'jumpy', 'hesitant', 'overshoot'];
+    mode = modes[Math.floor(Math.random() * modes.length)];
+  }
+  
+  currentMotionMode = mode;
+  hasHesitated = false;
+
  // Quick check on final destination
  const destCheck = IK.checkWorkspace(targetX, targetY);
  if (!destCheck.ok) {
@@ -218,6 +317,13 @@ async function goto(targetX, targetY, mode = 'direct') {

  motionQueue = segments;

+  if (BLE.isConnected()) {
+    if (mode === 'jumpy' || mode === 'hesitant') {
+      const fastSpeed = Math.min(2000, Settings.speed * 1.5);
+      try { await BLE.write(`SPD:${Math.floor(fastSpeed)}`); } catch(e){}
+    }
+  }
+
  return new Promise((resolve) => {
    currentResolve = resolve;
    executeQueue();