osiu97/ESP32-WijiBoard
1
0
Fork 0
Code Issues 10 Pull Requests Packages Projects Releases Wiki Activity

upgrade motion pathing

Browse Source
This commit is contained in:
osiu97
2026-07-08 18:23:49 +02:00
parent 57ea3f7b4a
commit 4048ad0fe6
1 changed files with 59 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
web/js/motion.js
+59 -24
View File
@@ -118,6 +118,35 @@ async function executeQueue() {
}
}
function buildSegments(startX, startY, endX, endY, mode) {
const dx = endX - startX;
const dy = endY - startY;
const distance = Math.sqrt(dx * dx + dy * dy);
let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
if (numSegments === 0) numSegments = 1;
let segs = [];
for (let i = 1; i <= numSegments; i++) {
const fraction = i / numSegments;
const px = startX + dx * fraction;
const py = startY + dy * fraction;
const wsCheck = IK.checkWorkspace(px, py);
if (!wsCheck.ok) return null;
const res = IK.solve(px, py);
if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) return null;
const s1 = IK.radToStepsAbsolute(res.theta1, 1);
const s2 = IK.radToStepsAbsolute(res.theta2, 2);
segs.push({ s1, s2, mode });
}
return segs;
}
/**
* Moves the arm to Cartesian targetX, targetY.
* Dynamically slices the path into segments to prevent out-of-bounds arcs.
@@ -129,6 +158,13 @@ async function goto(targetX, targetY, mode = 'direct') {
return false;
}
// Quick check on final destination
const destCheck = IK.checkWorkspace(targetX, targetY);
if (!destCheck.ok) {
UI.log(`⛔ Destination blocked: ${destCheck.reason}`, 'error');
return false;
}
// Calculate current Cartesian position
const t1 = IK.stepsToRad(steps1);
const t2 = IK.stepsToRad(steps2);
@@ -139,38 +175,37 @@ async function goto(targetX, targetY, mode = 'direct') {
return false;
}
const dx = targetX - currentPos.endX;
const dy = targetY - currentPos.endY;
const distance = Math.sqrt(dx * dx + dy * dy);
let segments = buildSegments(currentPos.endX, currentPos.endY, targetX, targetY, mode);
let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
if (numSegments === 0) numSegments = 1;
if (!segments) {
// Attempt rerouting through safe overhead point
const SAFE_X = 0;
const SAFE_Y = 75;
motionQueue = [];
for (let i = 1; i <= numSegments; i++) {
const fraction = i / numSegments;
const px = currentPos.endX + dx * fraction;
const py = currentPos.endY + dy * fraction;
const wsCheck = IK.checkWorkspace(px, py);
if (!wsCheck.ok) {
UI.log(`⛔ Waypoint blocked: ${wsCheck.reason}`, 'error');
return false; // abort full move
// 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(`⛔ Path completely blocked even via safe point`, 'error');
return false;
}
const res = IK.solve(px, py);
if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
UI.log(`Waypoint geometrically unreachable.`, 'error');
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(`⛔ Target unreachable (path blocked by mechanism)`, 'error');
return false;
}
const s1 = IK.radToStepsAbsolute(res.theta1, 1);
const s2 = IK.radToStepsAbsolute(res.theta2, 2);
motionQueue.push({ s1, s2, mode });
}
motionQueue = segments;
return new Promise((resolve) => {
currentResolve = resolve;
executeQueue();