osiu97
340 lines
10 KiB
JavaScript
340 lines
10 KiB
JavaScript
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
|
|
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 parts = msg.slice(2).split(',');
|
|
steps1 = Number(parts[0]);
|
|
steps2 = Number(parts[1]);
|
|
|
|
// parts[2] is the holding status (1 = holding, 0 = disabled)
|
|
if (parts.length > 2) {
|
|
const isHolding = parts[2] === '1';
|
|
if (!isHolding && localStorage.getItem('wiji_homed') === 'true') {
|
|
localStorage.setItem('wiji_homed', 'false');
|
|
UI.log('Steppers are not holding. Rehoming required.', 'warn');
|
|
document.dispatchEvent(new CustomEvent('wiji:unhomed'));
|
|
const warningEl = document.getElementById('home-warning');
|
|
if (warningEl) warningEl.style.display = 'block';
|
|
}
|
|
}
|
|
}
|
|
});
|
|
|
|
// 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;
|
|
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;
|
|
}
|
|
|
|
isMoving = true;
|
|
let target = motionQueue.shift();
|
|
|
|
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
|
|
}
|
|
|
|
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 {
|
|
// 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;
|
|
}
|
|
}
|
|
};
|
|
|
|
if (delayBeforeSend > 0) {
|
|
setTimeout(doSend, delayBeforeSend);
|
|
} else {
|
|
doSend();
|
|
}
|
|
}
|
|
|
|
function buildSegments(startX, startY, endX, endY, mode) {
|
|
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;
|
|
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) {
|
|
px = startX + dx * fraction;
|
|
py = startY + dy * fraction;
|
|
}
|
|
|
|
const res = IK.solve(px, py);
|
|
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, 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;
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
|
|
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) {
|
|
UI.log(`[ERROR] Destination blocked: ${destCheck.reason}`, 'error');
|
|
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;
|
|
}
|
|
|
|
let segments = buildSegments(currentPos.endX, currentPos.endY, targetX, targetY, mode);
|
|
|
|
if (!segments) {
|
|
// Attempt rerouting through safe overhead point
|
|
const SAFE_X = 0;
|
|
const SAFE_Y = 75;
|
|
|
|
// Prevent infinite loop if already at/near safe point
|
|
const distToSafe = Math.sqrt(Math.pow(currentPos.endX - SAFE_X, 2) + Math.pow(currentPos.endY - SAFE_Y, 2));
|
|
if (distToSafe < 5) {
|
|
UI.log(`[ERROR] Path completely blocked even via safe point`, 'error');
|
|
return false;
|
|
}
|
|
|
|
const leg1 = buildSegments(currentPos.endX, currentPos.endY, SAFE_X, SAFE_Y, mode);
|
|
if (leg1) {
|
|
const leg2 = buildSegments(SAFE_X, SAFE_Y, targetX, targetY, mode);
|
|
if (leg2) {
|
|
segments = leg1.concat(leg2);
|
|
UI.log(`Rerouting around exclusion zone via (0, 75)`, 'info');
|
|
}
|
|
}
|
|
|
|
if (!segments) {
|
|
UI.log(`[ERROR] Target unreachable (path blocked by mechanism)`, 'error');
|
|
return false;
|
|
}
|
|
}
|
|
|
|
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();
|
|
});
|
|
}
|
|
|
|
export default {
|
|
CONFIG,
|
|
goto,
|
|
setSteps,
|
|
getSteps,
|
|
get isMoving() { return isMoving; }
|
|
};
|