PROFERIS - Mi³osz Stocki
299 lines
9.0 KiB
JavaScript
299 lines
9.0 KiB
JavaScript
import BLE from './ble.js';
|
|
import IK from './kinematics.js';
|
|
import UI from './ui.js';
|
|
import Settings from './settings.js';
|
|
import { MotionEffects } from './motion-effects.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)
|
|
WAYPOINT_TIMEOUT_MS: 8000 // Max time to wait for a single waypoint to complete before declaring a stall
|
|
};
|
|
|
|
// Global state tracking
|
|
let steps1 = 0;
|
|
let steps2 = 0;
|
|
|
|
function forceUnhome(reason) {
|
|
localStorage.setItem('wiji_homed', 'false');
|
|
UI.log(reason, 'warn');
|
|
document.dispatchEvent(new CustomEvent('wiji:unhomed'));
|
|
const warningEl = document.getElementById('home-warning');
|
|
if (warningEl) warningEl.style.display = 'block';
|
|
}
|
|
|
|
// 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') {
|
|
forceUnhome('Steppers are not holding. Rehoming required.');
|
|
}
|
|
}
|
|
}
|
|
});
|
|
|
|
// 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
|
|
await MotionEffects.onSequenceEnd(currentMotionMode);
|
|
|
|
if (currentResolve) currentResolve(true);
|
|
currentResolve = null;
|
|
return;
|
|
}
|
|
|
|
isMoving = true;
|
|
let target = motionQueue.shift();
|
|
|
|
const { delay: delayBeforeSend, hasHesitated: newHasHesitated } = await MotionEffects.onWaypointSend(target, hasHesitated);
|
|
hasHesitated = newHasHesitated;
|
|
|
|
const doSend = async () => {
|
|
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);
|
|
const waypointStartTime = Date.now();
|
|
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);
|
|
|
|
// Check for timeout stall
|
|
if (Date.now() - waypointStartTime > CONFIG.WAYPOINT_TIMEOUT_MS) {
|
|
clearInterval(queueCheckInterval);
|
|
motionQueue = [];
|
|
isMoving = false;
|
|
forceUnhome('Motion timeout! Arms may be stuck. Rehoming required.');
|
|
if (currentResolve) currentResolve(false);
|
|
currentResolve = null;
|
|
return;
|
|
}
|
|
|
|
if (motionQueue.length > 0) {
|
|
const nextTarget = motionQueue[0];
|
|
const queueAction = MotionEffects.getQueueAction(target.mode, nextTarget.mode, maxDist, CONFIG.BLEND_THRESHOLD_STEPS);
|
|
if (queueAction.action === 'wait') {
|
|
clearInterval(queueCheckInterval);
|
|
setTimeout(executeQueue, queueAction.delay);
|
|
} else if (queueAction.action === 'blend') {
|
|
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) {
|
|
const modDest = MotionEffects.modifyDestination(startX, startY, endX, endY, mode);
|
|
const actualEndX = modDest.endX;
|
|
const actualEndY = modDest.endY;
|
|
|
|
const dx = actualEndX - startX;
|
|
const dy = actualEndY - startY;
|
|
const distance = Math.sqrt(dx * dx + dy * dy);
|
|
|
|
let numSegments = MotionEffects.getSegmentCount(distance, CONFIG.SEGMENT_SIZE_MM, mode);
|
|
|
|
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;
|
|
|
|
const perturbed = MotionEffects.applyPerturbation(px, py, nx, ny, fraction, distance, mode);
|
|
px = perturbed.px;
|
|
py = perturbed.py;
|
|
|
|
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 });
|
|
}
|
|
|
|
const settlingSegs = MotionEffects.getSettlingWaypoints(endX, endY, mode);
|
|
segs = segs.concat(settlingSegs);
|
|
|
|
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;
|
|
}
|
|
|
|
mode = MotionEffects.resolveMode(mode);
|
|
|
|
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;
|
|
|
|
await MotionEffects.onSequenceStart(mode);
|
|
|
|
return new Promise((resolve) => {
|
|
currentResolve = resolve;
|
|
executeQueue();
|
|
});
|
|
}
|
|
|
|
export default {
|
|
CONFIG,
|
|
goto,
|
|
setSteps,
|
|
getSteps,
|
|
get isMoving() { return isMoving; }
|
|
};
|