/** * WijiBoard – Stepper Test Section * web/js/sections/stepper-test.js * * 5-bar parallel linkage visualiser + IK-based click-to-move. * * Mechanism geometry (corrected from PositionControl.cpp bug): * Motor 1 at (-12.9, 0) mm Motor 2 at (+12.9, 0) mm ← NOTE: M1 is on the LEFT * Proximal links: l1 = 85 mm Distal links: l2 = 110 mm * * IK flow: click XY → IK.solve(x,y) → {θ1, θ2} → steps → BLE * FK flow: steps → radians → IK.forward(θ1, θ2) → draw SVG */ import BLE from '../ble.js'; import IK from '../kinematics.js'; import UI from '../ui.js'; // ── SVG viewport ────────────────────────────────────────────────── // We map the real workspace onto the SVG canvas. // Real workspace: X ∈ [-155, 155], Y ∈ [-35, 150] // We add some margin and invert Y (SVG y grows downward). const SV = { W: 480, H: 380, // World→SVG transform: scale + offset so the board fits nicely SCALE: 1.35, // px per mm // Origin (world 0,0) maps to this SVG point: OX: 240, // ≈ centre horizontally OY: 295, // base line — motors sit here // Convenience: world mm → SVG px wx(worldX) { return this.OX + worldX * this.SCALE; }, wy(worldY) { return this.OY - worldY * this.SCALE; }, // SVG px → world mm svgToWorldX(sx) { return (sx - this.OX) / this.SCALE; }, svgToWorldY(sy) { return -(sy - this.OY) / this.SCALE; }, }; // ── Arm length in px (for readable references) ──────────────────── const PX_L1 = IK.ARM.l1 * SV.SCALE; const PX_L2 = IK.ARM.l2 * SV.SCALE; const PX_D2 = IK.ARM.d2 * SV.SCALE; // ── Section state ───────────────────────────────────────────────── let steps1 = 0; let steps2 = 0; let eventCleanup = []; function syncStateFromStorage() { const 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; // Auto-migrate the old buggy negative home position to the new positive one if (steps1 === -1024) { steps1 = 1024; localStorage.setItem('wiji_steps1', 1024); } } function saveSteps() { if (localStorage.getItem('wiji_homed') === 'true') { localStorage.setItem('wiji_steps1', steps1); localStorage.setItem('wiji_steps2', steps2); } } // ── SVG element refs ────────────────────────────────────────────── let svgEl; // Real arm elements let arm1Prox, arm1Dist, arm2Prox, arm2Dist; let elbow1, elbow2, endEff, coordLabel, xhH, xhV; // Ghost arm let ghost1Prox, ghost1Dist, ghost2Prox, ghost2Dist; let ghostElbow1, ghostElbow2, ghostEnd, ghostLabel; // Target marker let targetMarker; // ── Readout refs ────────────────────────────────────────────────── let elCurrX, elCurrY, elTheta1, elTheta2, elSteps1, elSteps2; let simBadge; // ───────────────────────────────────────────────────────────────── // SVG rendering helpers // ───────────────────────────────────────────────────────────────── function setLine(el, x1, y1, x2, y2) { el.setAttribute('x1', x1); el.setAttribute('y1', y1); el.setAttribute('x2', x2); el.setAttribute('y2', y2); } function setCircle(el, cx, cy) { el.setAttribute('cx', cx); el.setAttribute('cy', cy); } /** Render the 5-bar arm from two motor angles. */ function renderArm(theta1, theta2, isGhost = false) { const { elbow1: e1, elbow2: e2, endX, endY } = IK.forward(theta1, theta2); // Motor base SVG positions — M1 at -d2, M2 at +d2 (hardware convention) const motor1sx = SV.wx(-IK.ARM.d2); const motor1sy = SV.wy(0); const motor2sx = SV.wx(+IK.ARM.d2); const motor2sy = SV.wy(0); const elbow1sx = SV.wx(e1.x); const elbow1sy = SV.wy(e1.y); const elbow2sx = SV.wx(e2.x); const elbow2sy = SV.wy(e2.y); const endsx = SV.wx(endX); const endsy = SV.wy(endY); // Guard: forward() can return NaN for degenerate angles – skip render if (!isFinite(endX) || !isFinite(endY)) return { endsx: SV.OX, endsy: SV.OY }; if (isGhost) { setLine(ghost1Prox, motor1sx, motor1sy, elbow1sx, elbow1sy); setLine(ghost1Dist, elbow1sx, elbow1sy, endsx, endsy); setLine(ghost2Prox, motor2sx, motor2sy, elbow2sx, elbow2sy); setLine(ghost2Dist, elbow2sx, elbow2sy, endsx, endsy); setCircle(ghostElbow1, elbow1sx, elbow1sy); setCircle(ghostElbow2, elbow2sx, elbow2sy); setCircle(ghostEnd, endsx, endsy); ghostLabel.setAttribute('x', endsx + 10); ghostLabel.setAttribute('y', endsy); } else { setLine(arm1Prox, motor1sx, motor1sy, elbow1sx, elbow1sy); setLine(arm1Dist, elbow1sx, elbow1sy, endsx, endsy); setLine(arm2Prox, motor2sx, motor2sy, elbow2sx, elbow2sy); setLine(arm2Dist, elbow2sx, elbow2sy, endsx, endsy); setCircle(elbow1, elbow1sx, elbow1sy); setCircle(elbow2, elbow2sx, elbow2sy); setCircle(endEff, endsx, endsy); // Crosshairs xhH.setAttribute('x1', endsx - 13); xhH.setAttribute('y1', endsy); xhH.setAttribute('x2', endsx + 13); xhH.setAttribute('y2', endsy); xhV.setAttribute('x1', endsx); xhV.setAttribute('y1', endsy - 13); xhV.setAttribute('x2', endsx); xhV.setAttribute('y2', endsy + 13); coordLabel.textContent = `(${endX.toFixed(1)}, ${endY.toFixed(1)}) mm`; updateReadouts(theta1, theta2, endX, endY); } return { endsx, endsy }; } function renderArmFromSteps() { const t1 = IK.stepsToRad(steps1); const t2 = IK.stepsToRad(steps2); renderArm(t1, t2); } function updateReadouts(t1, t2, ex, ey) { if (elCurrX) elCurrX.textContent = isFinite(ex) ? ex.toFixed(1) : '–'; if (elCurrY) elCurrY.textContent = isFinite(ey) ? ey.toFixed(1) : '–'; if (elTheta1) elTheta1.textContent = IK.radToDeg(t1).toFixed(1) + '°'; if (elTheta2) elTheta2.textContent = IK.radToDeg(t2).toFixed(1) + '°'; if (elSteps1) elSteps1.textContent = steps1; if (elSteps2) elSteps2.textContent = steps2; saveSteps(); } // ── Ghost arm show/hide ─────────────────────────────────────────── function showGhost(theta1, theta2, labelText) { const ghosts = [ghost1Prox, ghost1Dist, ghost2Prox, ghost2Dist, ghostElbow1, ghostElbow2, ghostEnd, ghostLabel]; ghosts.forEach(el => el.style.display = ''); renderArm(theta1, theta2, true); ghostLabel.textContent = labelText; } function hideGhost() { [ghost1Prox, ghost1Dist, ghost2Prox, ghost2Dist, ghostElbow1, ghostElbow2, ghostEnd, ghostLabel] .forEach(el => el.style.display = 'none'); } // ── Target marker ───────────────────────────────────────────────── function showTargetMarker(sx, sy) { targetMarker.setAttribute('cx', sx); targetMarker.setAttribute('cy', sy); targetMarker.style.display = ''; targetMarker.classList.remove('target-pulse'); void targetMarker.offsetWidth; // force reflow targetMarker.classList.add('target-pulse'); } // ───────────────────────────────────────────────────────────────── // IK movement // ───────────────────────────────────────────────────────────────── async function moveToXY(targetX, targetY) { // Check workspace limits first 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.radToSteps(theta1); const newSteps2 = IK.radToSteps(theta2); const delta1 = newSteps1 - steps1; const delta2 = newSteps2 - steps2; const cmd1 = `S1${delta1 >= 0 ? '+' : ''}${delta1}`; const cmd2 = `S2${delta2 >= 0 ? '+' : ''}${delta2}`; if (BLE.isConnected()) { try { await BLE.write(cmd1); await BLE.write(cmd2); // 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)}° | ${cmd1} ${cmd2}`; console.log(simMsg); UI.log(simMsg, 'info'); } return true; } // ───────────────────────────────────────────────────────────────── // Manual jog // ───────────────────────────────────────────────────────────────── async function jogMotor(motor, delta) { const cmd = `S${motor}${delta >= 0 ? '+' : ''}${delta}`; if (BLE.isConnected()) { await BLE.write(cmd).catch(e => UI.log(e.message, 'error')); } else { if (motor === 1) steps1 += delta; else steps2 += delta; renderArmFromSteps(); console.log(`[sim] ${cmd}`); UI.log(`[sim] ${cmd}`, 'info'); } } async function zeroMotor(motor) { const cmd = motor === 'ALL' ? 'HOMEALL' : `HOME${motor}`; if (BLE.isConnected()) { await BLE.write(cmd).catch(e => UI.log(e.message, 'error')); } else { if (motor === 'ALL') { steps1 = IK.ARM.HOME_STEPS.m1; steps2 = IK.ARM.HOME_STEPS.m2; localStorage.setItem('wiji_homed', 'true'); } else if (motor === 1) { steps1 = IK.ARM.HOME_STEPS.m1; } else { steps2 = IK.ARM.HOME_STEPS.m2; } renderArmFromSteps(); console.log(`[sim] ${cmd}`); UI.log(`[sim] ${cmd}`, 'info'); } } // ───────────────────────────────────────────────────────────────── // Manual jog // ───────────────────────────────────────────────────────────────── // ───────────────────────────────────────────────────────────────── // SVG workspace zones (pre-computed) // ───────────────────────────────────────────────────────────────── function buildZones() { const L = IK.LIMITS; // Outer bounding box rect const bx = SV.wx(L.X_MIN); const by = SV.wy(L.Y_MAX); const bw = (L.X_MAX - L.X_MIN) * SV.SCALE; const bh = (L.Y_MAX - L.Y_MIN) * SV.SCALE; // Mechanism exclusion box (uses updated LIMITS fields) const ex = SV.wx(-L.BOX_HALF_W); const ey = SV.wy(L.BOX_Y_MAX); const ew = L.BOX_HALF_W * 2 * SV.SCALE; const eh = (L.BOX_Y_MAX - L.BOX_Y_MIN) * SV.SCALE; // Motor positions — M1 at -d2, M2 at +d2 const m1x = SV.wx(-IK.ARM.d2); const m2x = SV.wx(+IK.ARM.d2); const my = SV.wy(0); // Scale ruler const r0x = SV.wx(0); const r0y = SV.wy(-20); const r1x = SV.wx(50); return ` NO-GO ZONE +X +Y 50 mm M1 M2 `; } // ───────────────────────────────────────────────────────────────── // HTML template // ───────────────────────────────────────────────────────────────── function buildHTML() { return `

Stepper Test

Click anywhere on the SCARA map to move via IK. Red zone = mechanism exclusion. Dashed box = board limits.

SCARA Map — click to move SIMULATION
${buildZones()} (0.0, 0.0) mm
━ M1 (left) ━ M2 (right) ▪ NO-GO zone ╌ board limits l1=${IK.ARM.l1}mm l2=${IK.ARM.l2}mm d=${IK.ARM.d}mm
Current Position
0.0
X (mm)
0.0
Y (mm)
0.0°
θ1 (M1)
0.0°
θ2 (M2)
0
M1 steps
0
M2 steps
Move to XY
mm
mm
Motion
600
100
Manual Jog Incremental step control — bypasses IK
Motor 1 — Left
${[10, 50, 100, 512].map(n => ``).join('')} ${[10, 50, 100, 512].map(n => ``).join('')}
Motor 2 — Right
${[10, 50, 100, 512].map(n => ``).join('')} ${[10, 50, 100, 512].map(n => ``).join('')}
Event Log
`; } // ───────────────────────────────────────────────────────────────── // Section lifecycle // ───────────────────────────────────────────────────────────────── const StepperTestSection = { mount(container) { syncStateFromStorage(); container.innerHTML = buildHTML(); // ── Cache refs ────────────────────────────────────────────── svgEl = document.getElementById('scara-svg'); arm1Prox = document.getElementById('arm1-prox'); arm1Dist = document.getElementById('arm1-dist'); arm2Prox = document.getElementById('arm2-prox'); arm2Dist = document.getElementById('arm2-dist'); elbow1 = document.getElementById('elbow1'); elbow2 = document.getElementById('elbow2'); endEff = document.getElementById('end-eff'); coordLabel = document.getElementById('coord-label'); xhH = document.getElementById('xh-h'); xhV = document.getElementById('xh-v'); ghost1Prox = document.getElementById('ghost-1p'); ghost1Dist = document.getElementById('ghost-1d'); ghost2Prox = document.getElementById('ghost-2p'); ghost2Dist = document.getElementById('ghost-2d'); ghostElbow1 = document.getElementById('ghost-e1'); ghostElbow2 = document.getElementById('ghost-e2'); ghostEnd = document.getElementById('ghost-e1'); ghostLabel = document.getElementById('ghost-label'); targetMarker = document.getElementById('target-marker'); elCurrX = document.getElementById('curr-x'); elCurrY = document.getElementById('curr-y'); elTheta1 = document.getElementById('curr-t1'); elTheta2 = document.getElementById('curr-t2'); elSteps1 = document.getElementById('curr-s1'); elSteps2 = document.getElementById('curr-s2'); simBadge = document.getElementById('sim-badge'); // ── SVG coordinate helpers ─────────────────────────────────── function pointerToWorld(e) { const rect = svgEl.getBoundingClientRect(); const sx = (e.clientX - rect.left) * (SV.W / rect.width); const sy = (e.clientY - rect.top) * (SV.H / rect.height); return { wx: SV.svgToWorldX(sx), wy: SV.svgToWorldY(sy), sx, sy }; } // ── SVG hover ─────────────────────────────────────────────── svgEl.addEventListener('mousemove', (e) => { const { wx, wy } = pointerToWorld(e); const ws = IK.checkWorkspace(wx, wy); if (ws.ok) { const { theta1, theta2 } = IK.solve(wx, wy); showGhost(theta1, theta2, `(${wx.toFixed(0)},${wy.toFixed(0)})`); svgEl.style.cursor = 'crosshair'; } else { hideGhost(); svgEl.style.cursor = 'not-allowed'; } }); svgEl.addEventListener('mouseleave', () => { hideGhost(); svgEl.style.cursor = 'crosshair'; }); // ── SVG click ─────────────────────────────────────────────── svgEl.addEventListener('click', async (e) => { const { wx, wy, sx, sy } = pointerToWorld(e); const ws = IK.checkWorkspace(wx, wy); if (!ws.ok) { UI.log(`⛔ ${ws.reason}`, 'warn'); return; } showTargetMarker(sx, sy); const xi = document.getElementById('input-x'); const yi = document.getElementById('input-y'); if (xi) xi.value = wx.toFixed(1); if (yi) yi.value = wy.toFixed(1); await moveToXY(wx, wy); }); // ── Move to XY button ──────────────────────────────────────── document.getElementById('btn-goto').addEventListener('click', async () => { const x = parseFloat(document.getElementById('input-x').value); const y = parseFloat(document.getElementById('input-y').value); if (isNaN(x) || isNaN(y)) { UI.log('Invalid XY.', 'warn'); return; } await moveToXY(x, y); }); // ── Jog CW / CCW ──────────────────────────────────────────── const gs = (n) => parseInt(document.getElementById(`jog-steps-${n}`)?.value ?? '50', 10) || 50; document.getElementById('s1-cw').addEventListener('click', () => jogMotor(1, +gs(1))); document.getElementById('s1-ccw').addEventListener('click', () => jogMotor(1, -gs(1))); document.getElementById('s2-cw').addEventListener('click', () => jogMotor(2, +gs(2))); document.getElementById('s2-ccw').addEventListener('click', () => jogMotor(2, -gs(2))); document.getElementById('s1-zero').addEventListener('click', () => zeroMotor(1)); document.getElementById('s2-zero').addEventListener('click', () => zeroMotor(2)); document.getElementById('sall-zero').addEventListener('click', () => zeroMotor('ALL')); // ── Quick-step buttons ──────────────────────────────────────── container.querySelectorAll('[data-motor][data-steps]').forEach(btn => { btn.addEventListener('click', () => jogMotor(parseInt(btn.dataset.motor, 10), parseInt(btn.dataset.steps, 10))); }); // ── Speed / Accel sliders ───────────────────────────────────── let spdT, accT; document.getElementById('speed-slider').addEventListener('input', (e) => { document.getElementById('speed-val').textContent = e.target.value; clearTimeout(spdT); spdT = setTimeout(() => { const cmd = `SPD:${e.target.value}`; BLE.isConnected() ? BLE.write(cmd).catch(() => { }) : UI.log(`[sim] ${cmd}`, 'info'); }, 400); }); document.getElementById('accel-slider').addEventListener('input', (e) => { document.getElementById('accel-val').textContent = e.target.value; clearTimeout(accT); accT = setTimeout(() => { const cmd = `ACC:${e.target.value}`; BLE.isConnected() ? BLE.write(cmd).catch(() => { }) : UI.log(`[sim] ${cmd}`, 'info'); }, 400); }); // ── Log clear ───────────────────────────────────────────────── document.getElementById('clear-log-btn').addEventListener('click', UI.clearLog); // ── BLE badge ───────────────────────────────────────────────── function updateBadge() { if (!simBadge) return; const live = BLE.isConnected(); simBadge.textContent = live ? 'LIVE' : 'SIMULATION'; simBadge.style.background = live ? 'rgba(0,230,118,0.12)' : 'rgba(255,202,40,0.12)'; simBadge.style.color = live ? 'var(--accent-green)' : 'var(--accent-amber)'; simBadge.style.border = live ? '1px solid rgba(0,230,118,0.3)' : '1px solid rgba(255,202,40,0.25)'; } BLE.on('connected', updateBadge); BLE.on('disconnected', updateBadge); updateBadge(); // ── BLE NOTIFY position feedback ────────────────────────────── function onBLEStatus(e) { const msg = e.detail; if (msg.startsWith('P:')) { const [s1, s2] = msg.slice(2).split(',').map(Number); steps1 = s1; steps2 = s2; renderArmFromSteps(); } } document.addEventListener('ble:status', onBLEStatus); eventCleanup = [ () => BLE.off('connected', updateBadge), () => BLE.off('disconnected', updateBadge), () => document.removeEventListener('ble:status', onBLEStatus), ]; // ── Initial render ───────────────────────────────────────────── renderArmFromSteps(); UI.log('Stepper Test ready — 5-bar parallel IK active.', 'success'); UI.log(`Arm: d=${IK.ARM.d}mm l1=${IK.ARM.l1}mm l2=${IK.ARM.l2}mm`, 'info'); if (!BLE.isConnected()) { UI.log('Simulation mode — BLE commands logged but not sent.', 'warn'); } }, unmount() { eventCleanup.forEach(fn => fn()); eventCleanup = []; }, }; export default StepperTestSection;