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

even more kinematics

Browse Source
This commit is contained in:
PROFERIS - Mi³osz Stocki
2026-07-08 14:39:32 +02:00
parent 4fad71ec49
commit ed6c6a6658
3 changed files with 118 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
web/js/kinematics.js
+50 -8
View File
@@ -245,21 +245,63 @@ function forward(theta1, theta2) {
*/
function armsCrossed(theta1, theta2) {
const { d2, l1 } = ARM;
const { ELBOW_BOX_X_INNER: XI, ELBOW_BOX_Y_MAX: YM } = LIMITS;
const { BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX } = LIMITS;
// Elbow positions (same formula as forward())
const m1x = -d2, m1y = 0;
const m2x = d2, m2y = 0;
const e1x = -d2 + l1 * Math.cos(theta1);
const e1y = l1 * Math.sin(theta1);
const e2x = +d2 + l1 * Math.cos(theta2);
const e2y = l1 * Math.sin(theta2);
// Elbow1 (from M1 on the LEFT) must not appear far to the RIGHT at low height
// Elbow2 (from M2 on the RIGHT) must not appear far to the LEFT at low height
// Both conditions together catch the "arms have swapped sides" scenario.
const e1_crossed = e1x > XI && e1y < YM; // M1's elbow went too far right
const e2_crossed = e2x < -XI && e2y < YM; // M2's elbow went too far left
// Helper: line segment (x1,y1)->(x2,y2) intersects AABB (minX,maxX,minY,maxY)
function lineIntersectsBox(x1, y1, x2, y2, minX, maxX, minY, maxY) {
if (Math.max(x1, x2) < minX || Math.min(x1, x2) > maxX) return false;
if (Math.max(y1, y2) < minY || Math.min(y1, y2) > maxY) return false;
return e1_crossed || e2_crossed;
// Check if line crosses the box
const outcodes = (x, y) => {
let code = 0;
if (x < minX) code |= 1; else if (x > maxX) code |= 2;
if (y < minY) code |= 4; else if (y > maxY) code |= 8;
return code;
};
let c1 = outcodes(x1, y1);
let c2 = outcodes(x2, y2);
if ((c1 & c2) !== 0) return false; // Both endpoints share an outside zone
if (c1 === 0 || c2 === 0) return true; // One endpoint is inside
// Detailed check: find intersection points with box boundaries
const m = (y2 - y1) / (x2 - x1);
const b = y1 - m * x1;
// Check Left/Right edges
if (x1 !== x2) {
let yLeft = m * minX + b;
if (yLeft >= minY && yLeft <= maxY) return true;
let yRight = m * maxX + b;
if (yRight >= minY && yRight <= maxY) return true;
}
// Check Top/Bottom edges
if (y1 !== y2) {
let xBottom = (minY - b) / m;
if (xBottom >= minX && xBottom <= maxX) return true;
let xTop = (maxY - b) / m;
if (xTop >= minX && xTop <= maxX) return true;
}
return false;
}
const cross1 = lineIntersectsBox(m1x, m1y, e1x, e1y, -BOX_HALF_W, BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX);
const cross2 = lineIntersectsBox(m2x, m2y, e2x, e2y, -BOX_HALF_W, BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX);
// Also check if elbows went rogue (e.g. wrapped entirely around the wrong side)
const e1_rogue = e1x > BOX_HALF_W && e1y < BOX_Y_MAX;
const e2_rogue = e2x < -BOX_HALF_W && e2y < BOX_Y_MAX;
return cross1 || cross2 || e1_rogue || e2_rogue;
}
// ── Workspace check ───────────────────────────────────────────────