PROFERIS - Mi³osz Stocki
104 lines
2.7 KiB
JavaScript
104 lines
2.7 KiB
JavaScript
import BLE from './ble.js';
|
|
import UI from './ui.js';
|
|
import IK from './kinematics.js';
|
|
import Settings from './settings.js';
|
|
|
|
let isRunning = false;
|
|
|
|
function delayMs(ms) {
|
|
return new Promise(resolve => setTimeout(resolve, ms));
|
|
}
|
|
|
|
export default {
|
|
get isRunning() { return isRunning; },
|
|
stop() { isRunning = false; },
|
|
|
|
async run(sequence, spotsData, currentSteps1, currentSteps2, onStepComplete) {
|
|
if (isRunning) {
|
|
UI.log('A sequence is already running.', 'warn');
|
|
return false;
|
|
}
|
|
|
|
isRunning = true;
|
|
UI.log(`Running sequence: ${sequence.name}`, 'info');
|
|
|
|
let steps1 = currentSteps1;
|
|
let steps2 = currentSteps2;
|
|
|
|
for (let i = 0; i < sequence.steps.length; i++) {
|
|
if (!isRunning) {
|
|
UI.log('Sequence stopped.', 'info');
|
|
break;
|
|
}
|
|
|
|
const step = sequence.steps[i];
|
|
let targetX, targetY, label;
|
|
|
|
if (step.type === 'spot') {
|
|
const spot = spotsData.find(s => s.id === step.id);
|
|
if (!spot) {
|
|
UI.log(`Sequence error: Spot '${step.id}' not found. Skipping.`, 'error');
|
|
continue;
|
|
}
|
|
targetX = spot.x;
|
|
targetY = spot.y;
|
|
label = spot.label || spot.id;
|
|
} else if (step.type === 'xy') {
|
|
targetX = step.x;
|
|
targetY = step.y;
|
|
label = `(${targetX}, ${targetY})`;
|
|
} else {
|
|
UI.log(`Unknown step type: ${step.type}`, 'error');
|
|
continue;
|
|
}
|
|
|
|
const wsCheck = IK.checkWorkspace(targetX, targetY);
|
|
if (!wsCheck.ok) {
|
|
UI.log(`⛔ ${wsCheck.reason} [${label}]`, 'error');
|
|
continue; // or break? let's continue
|
|
}
|
|
|
|
const res = IK.solve(targetX, targetY);
|
|
if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
|
|
UI.log(`Target ${label} is unreachable or crosses arms.`, 'error');
|
|
continue;
|
|
}
|
|
|
|
const newSteps1 = IK.radToSteps(res.theta1);
|
|
const newSteps2 = IK.radToSteps(res.theta2);
|
|
|
|
const cmd1 = `X1:${newSteps1}`;
|
|
const cmd2 = `X2:${newSteps2}`;
|
|
|
|
UI.log(`Seq step: ${label}`, 'info');
|
|
|
|
if (BLE.isConnected()) {
|
|
try {
|
|
await BLE.write(cmd1);
|
|
await BLE.write(cmd2);
|
|
} catch (e) {
|
|
UI.log(`BLE error: ${e.message}`, 'error');
|
|
isRunning = false;
|
|
return false;
|
|
}
|
|
} else {
|
|
steps1 = newSteps1;
|
|
steps2 = newSteps2;
|
|
|
|
if (onStepComplete) {
|
|
onStepComplete(steps1, steps2);
|
|
}
|
|
console.log(`[sim] ${cmd1} ${cmd2}`);
|
|
}
|
|
|
|
// Wait for delay
|
|
const waitTime = typeof step.delay === 'number' ? step.delay : Settings.delay;
|
|
await delayMs(waitTime);
|
|
}
|
|
|
|
isRunning = false;
|
|
UI.log(`Sequence completed.`, 'success');
|
|
return true;
|
|
}
|
|
};
|