PROFERIS - Mi³osz Stocki
375 lines
14 KiB
JavaScript
375 lines
14 KiB
JavaScript
/**
|
||
* WijiBoard – Kinematics (exact port of PositionControl.cpp)
|
||
* web/js/kinematics.js
|
||
*
|
||
* ── Mechanism: symmetric 5-bar parallel linkage ─────────────────
|
||
*
|
||
* END EFFECTOR (x, y)
|
||
* / \
|
||
* L2 (110) L2 (110)
|
||
* / \
|
||
* ELBOW1 ELBOW2
|
||
* \ /
|
||
* L1 (85) L1 (85)
|
||
* \ /
|
||
* MOTOR1 (+d2,0) MOTOR2 (-d2,0)
|
||
* | |
|
||
* [===BASE===] (d=25.8 mm wide)
|
||
* d2=12.9 mm
|
||
*
|
||
* ── IMPORTANT: PositionControl.cpp motor convention ─────────────
|
||
* Motor 1 pivot is at (-d2, 0) = (-12.9, 0) [left side!]
|
||
* Motor 2 pivot is at (+d2, 0) = (+12.9, 0) [right side!]
|
||
*
|
||
* This is determined by a variable swap in the C++ firmware where
|
||
* angle2 is passed to stepper1 and angle1 to stepper2. Thus:
|
||
* Motor 1 (Left): xpd = x + d2 → target is measured from x = -d2
|
||
* Motor 2 (Right): xmd = x - d2 → target is measured from x = +d2
|
||
*
|
||
* All FK / visualisation code MUST use this exact convention or
|
||
* the arms will appear visually crossed and physically collide.
|
||
*
|
||
* Source: lib/Position/PositionControl.cpp (nerd-sniped/WijiBoard)
|
||
*/
|
||
|
||
// ── Exact constants from PositionControl.cpp ─────────────────────
|
||
const ARM = {
|
||
d: 25.8, // full motor separation (mm)
|
||
d2: 12.9, // half separation; M1 at (-d2, 0), M2 at (+d2, 0)
|
||
l1: 85.0, // proximal link length (mm)
|
||
l2: 110.0, // distal link length (mm)
|
||
STEPS_PER_REV: 2048,
|
||
STEP_ANGLE_DEG: 360 / 2048, // ≈ 0.17578125 °/step
|
||
HOME_STEPS: { m1: 1024, m2: 0 }, // M1 (Left) at +180°, M2 (Right) at 0° (arms folded outward)
|
||
};
|
||
|
||
// ── WORKSPACE CONSTRAINTS ─────────────────────────────────────────
|
||
// All values are in mm. Tune these to match real hardware.
|
||
// They are exported so the visualiser can draw the zones.
|
||
const LIMITS = {
|
||
// ── Outer board boundary ───────────────────────────────────────
|
||
// The EE cannot be requested outside this rectangle.
|
||
X_MIN: -150, // ← TUNE: left edge of board
|
||
X_MAX: 150, // ← TUNE: right edge of board
|
||
Y_MIN: -30, // ← TUNE: bottom (numbers reach ~44 mm; sign coords go lower)
|
||
Y_MAX: 145, // ← TUNE: top (highest letter is ~128 mm)
|
||
|
||
// ── Centre mechanism exclusion box ────────────────────────────
|
||
// Rectangular zone centred on (0, 0) where the motor housing sits.
|
||
// The EE and both elbows must stay outside this area.
|
||
BOX_HALF_W: 30, // ← TUNE: half-width in X (motors at ±12.9, housing wider)
|
||
BOX_Y_MIN: -10, // ← TUNE: bottom of housing
|
||
BOX_Y_MAX: 40, // ← TUNE: top of housing
|
||
|
||
// ── Elbow exclusion zone (prevents arm crossing near the box) ─
|
||
// Each elbow has a separate rectangular exclusion box.
|
||
// Left-side elbow (from M1 at -d2): must NOT enter this region.
|
||
// Right-side elbow (from M2 at +d2): uses mirrored X limits.
|
||
// If ELBOW_BOX_X_INNER is 5, the left elbow's X must be < -5 mm
|
||
// (can never cross to the right side of the box mid-point).
|
||
ELBOW_BOX_X_INNER: 5, // ← TUNE: inner X margin from centre for each elbow
|
||
ELBOW_BOX_Y_MAX: 50, // ← TUNE: Y below which elbow crossing is forbidden
|
||
ELBOW_MIN_Y: -70, // ← TUNE: Y below which the elbow hits the bottom housing/desk
|
||
};
|
||
|
||
// ── Letter / number position lookup table ────────────────────────
|
||
// Directly ported from PositionControl.cpp
|
||
const LOOKUP_TABLE = {
|
||
'Q': { x: -66.5, y: 91.6 },
|
||
'W': { x: 70.8, y: 92.0 },
|
||
'E': { x: -41.4, y: 125.5 },
|
||
'R': { x: -44.0, y: 96.0 },
|
||
'T': { x: 1.3, y: 97.8 },
|
||
'Y': { x: 117.8, y: 91.7 },
|
||
'U': { x: 22.5, y: 98.3 },
|
||
'I': { x: 53.5, y: 124.5 },
|
||
'O': { x: -112.0, y: 82.0 },
|
||
'P': { x: -90.0, y: 89.0 },
|
||
'A': { x: -143.0, y: 99.0 },
|
||
'S': { x: -19.8, y: 98.0 },
|
||
'D': { x: -68.5, y: 121.7 },
|
||
'F': { x: -19.5, y: 127.0 },
|
||
'G': { x: 4.0, y: 128.4 },
|
||
'H': { x: 31.3, y: 127.3 },
|
||
'J': { x: 72.5, y: 121.0 },
|
||
'K': { x: 93.0, y: 116.8 },
|
||
'L': { x: 114.3, y: 110.2 },
|
||
'Z': { x: 130.0, y: 75.0 },
|
||
'X': { x: 95.9, y: 87.3 },
|
||
'C': { x: -94.0, y: 116.0 },
|
||
'V': { x: 44.5, y: 97.0 },
|
||
'B': { x: -119.0, y: 109.0 },
|
||
'N': { x: -135.4, y: 75.1 },
|
||
'M': { x: 132.0, y: 100.5 },
|
||
|
||
'+': { x: -110.0, y: 1.0 },
|
||
'-': { x: 110.0, y: 1.0 },
|
||
'*': { x: 110.0, y: -24.0 },
|
||
',': { x: -110.0, y: -24.0 },
|
||
|
||
'0': { x: -130.4, y: 44.2 },
|
||
'1': { x: -101.1, y: 53.3 },
|
||
'2': { x: -71.5, y: 60.4 },
|
||
'3': { x: -41.7, y: 65.2 },
|
||
'4': { x: -13.0, y: 66.5 },
|
||
'5': { x: 16.3, y: 68.0 },
|
||
'6': { x: 45.8, y: 64.0 },
|
||
'7': { x: 75.7, y: 61.1 },
|
||
'8': { x: 103.6, y: 53.8 },
|
||
'9': { x: 132.1, y: 45.0 },
|
||
};
|
||
|
||
// ── IK solver — exact port of calculateInverseKinematics() ───────
|
||
/**
|
||
* Compute motor angles for a given target end-effector position.
|
||
* Exactly mirrors the C++ implementation in PositionControl.cpp.
|
||
*
|
||
* @param {number} x Target X in mm (origin = midpoint between motors)
|
||
* @param {number} y Target Y in mm
|
||
* @returns {{ theta1: number, theta2: number, reachable: boolean }}
|
||
* theta1 / theta2 in RADIANS (motor 1 = left, motor 2 = right)
|
||
*/
|
||
function solve(x, y) {
|
||
const { d2, l1, l2 } = ARM;
|
||
|
||
// ── Motor 1 (Left, pivot at -d2, 0 = -12.9 mm) ───────────────
|
||
// xpd = x + d2 is the X component of (target – M1_pivot).
|
||
const xpd = x + d2;
|
||
const t = Math.sqrt(xpd * xpd + y * y);
|
||
if (t < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
|
||
const cosW2 = (l2 * l2 - t * t - l1 * l1) / (-2 * l1 * t);
|
||
if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false };
|
||
const r = Math.atan2(y, xpd);
|
||
const w2 = Math.acos(cosW2);
|
||
const t1A = r + w2; // Outward
|
||
const t1B = r - w2; // Inward
|
||
|
||
// ── Motor 2 (Right, pivot at +d2, 0 = +12.9 mm) ──────────────
|
||
// xmd = x - d2 is the X component of (target – M2_pivot).
|
||
const xmd = x - d2;
|
||
const s = Math.sqrt(xmd * xmd + y * y);
|
||
if (s < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
|
||
const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
|
||
if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
|
||
const q = Math.atan2(y, xmd);
|
||
const w1 = Math.acos(cosW1);
|
||
const t2A = q - w1; // Outward
|
||
const t2B = q + w1; // Inward
|
||
|
||
// Evaluate all 4 possible elbow configurations
|
||
const combos = [
|
||
{ t1: t1A, t2: t2A }, // Out/Out
|
||
{ t1: t1B, t2: t2A }, // In/Out
|
||
{ t1: t1A, t2: t2B }, // Out/In
|
||
{ t1: t1B, t2: t2B }, // In/In
|
||
];
|
||
|
||
let bestCombo = null;
|
||
|
||
for (const c of combos) {
|
||
if (armsCrossed(c.t1, c.t2)) continue; // Rejected by physical constraints (housing/bottom)
|
||
|
||
// Calculate distance between elbows to avoid physical joint collision
|
||
const e1x = -d2 + l1 * Math.cos(c.t1);
|
||
const e1y = l1 * Math.sin(c.t1);
|
||
const e2x = +d2 + l1 * Math.cos(c.t2);
|
||
const e2y = l1 * Math.sin(c.t2);
|
||
const edist = Math.sqrt((e2x - e1x)**2 + (e2y - e1y)**2);
|
||
|
||
// If elbows are absurdly close (< 10mm), the physical joints will collide
|
||
// and the FK will glitch (since intersections become highly sensitive).
|
||
if (edist < 10) continue;
|
||
|
||
// We prefer the combination where elbows point UP (y > 0)
|
||
// Score based on how far above the bottom housing they are
|
||
c.score = e1y + e2y;
|
||
|
||
// Penalize configurations that point downwards heavily
|
||
if (e1y < 0) c.score -= 1000;
|
||
if (e2y < 0) c.score -= 1000;
|
||
|
||
if (!bestCombo || c.score > bestCombo.score) {
|
||
bestCombo = c;
|
||
}
|
||
}
|
||
|
||
if (!bestCombo) {
|
||
return { theta1: t1A, theta2: t2A, reachable: false };
|
||
}
|
||
|
||
return { theta1: bestCombo.t1, theta2: bestCombo.t2, reachable: true };
|
||
}
|
||
|
||
// ── Forward kinematics ───────────────────────────────────────────
|
||
/**
|
||
* Given motor angles, compute elbow and end-effector positions.
|
||
* The end-effector is found as the intersection of the two distal
|
||
* link circles — this is the FK complement to the 5-bar IK above.
|
||
*
|
||
* In practice for visualisation we just re-derive the elbow
|
||
* positions from each motor.
|
||
*
|
||
* @param {number} theta1 Motor 1 angle (radians)
|
||
* @param {number} theta2 Motor 2 angle (radians)
|
||
* @returns {{
|
||
* elbow1: {x,y}, elbow2: {x,y},
|
||
* endX: number, endY: number
|
||
* }}
|
||
*/
|
||
function forward(theta1, theta2) {
|
||
const { d2, l1, l2 } = ARM;
|
||
|
||
// ── IMPORTANT: match physical hardware convention ──────────────
|
||
// Motor 1 pivot at (-d2, 0), Motor 2 pivot at (+d2, 0).
|
||
// Using the opposite sign here is the single most common source
|
||
// of visually-crossed arms in the SVG visualiser.
|
||
|
||
// Elbow 1 — tip of Motor 1 proximal link (motor at -d2, Left)
|
||
const e1x = -d2 + l1 * Math.cos(theta1);
|
||
const e1y = l1 * Math.sin(theta1);
|
||
|
||
// Elbow 2 — tip of Motor 2 proximal link (motor at +d2, Right)
|
||
const e2x = +d2 + l1 * Math.cos(theta2);
|
||
const e2y = l1 * Math.sin(theta2);
|
||
|
||
// End-effector: intersection of the two distal-link circles
|
||
// (radius l2, centred on each elbow). Pick the "upward" solution.
|
||
const dx = e2x - e1x;
|
||
const dy = e2y - e1y;
|
||
const dist = Math.sqrt(dx * dx + dy * dy);
|
||
|
||
if (dist < 1e-6 || dist > 2 * l2) {
|
||
// Degenerate / unreachable — fall back to midpoint
|
||
return {
|
||
elbow1: { x: e1x, y: e1y },
|
||
elbow2: { x: e2x, y: e2y },
|
||
endX: (e1x + e2x) / 2,
|
||
endY: (e1y + e2y) / 2,
|
||
valid: false,
|
||
};
|
||
}
|
||
|
||
const a = dist / 2;
|
||
const h = Math.sqrt(Math.max(0, l2 * l2 - a * a));
|
||
const mx = (e1x + e2x) / 2;
|
||
const my = (e1y + e2y) / 2;
|
||
|
||
// Two intersection candidates
|
||
const px1 = mx + h * (dy / dist);
|
||
const py1 = my + h * (-dx / dist);
|
||
const px2 = mx - h * (dy / dist);
|
||
const py2 = my - h * (-dx / dist);
|
||
|
||
// Always pick the candidate with higher Y (EE above the elbow line)
|
||
const useFirst = py1 >= py2;
|
||
const endX = useFirst ? px1 : px2;
|
||
const endY = useFirst ? py1 : py2;
|
||
|
||
return {
|
||
elbow1: { x: e1x, y: e1y },
|
||
elbow2: { x: e2x, y: e2y },
|
||
endX, endY,
|
||
valid: true,
|
||
};
|
||
}
|
||
|
||
// ── Arm-crossing check ────────────────────────────────────────────
|
||
/**
|
||
* Returns true if the two proximal arm segments geometrically
|
||
* cross each other near the centre mechanism box.
|
||
*
|
||
* Physical rule: each elbow must stay on the OUTER side of the
|
||
* mechanism housing. If elbow1 (from M1 at -d2) has a small
|
||
* negative X at low Y, or elbow2 (from M2 at +d2) has a small
|
||
* positive X at low Y, the arm would collide with the housing.
|
||
*
|
||
* @param {number} theta1 Motor 1 angle (rad)
|
||
* @param {number} theta2 Motor 2 angle (rad)
|
||
* @returns {boolean} true = arms will cross / collide
|
||
*/
|
||
function armsCrossed(theta1, theta2) {
|
||
const { d2, l1 } = ARM;
|
||
const { ELBOW_BOX_X_INNER, ELBOW_BOX_Y_MAX, ELBOW_MIN_Y } = LIMITS;
|
||
|
||
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);
|
||
|
||
// Bottom housing collision
|
||
if (e1y < ELBOW_MIN_Y || e2y < ELBOW_MIN_Y) return true;
|
||
|
||
// E1 (left motor) should not cross to the right side (X > -ELBOW_BOX_X_INNER) if it's low (Y < ELBOW_BOX_Y_MAX)
|
||
if (e1y < ELBOW_BOX_Y_MAX && e1x > -ELBOW_BOX_X_INNER) return true;
|
||
|
||
// E2 (right motor) should not cross to the left side (X < ELBOW_BOX_X_INNER) if it's low (Y < ELBOW_BOX_Y_MAX)
|
||
if (e2y < ELBOW_BOX_Y_MAX && e2x < ELBOW_BOX_X_INNER) return true;
|
||
|
||
return false;
|
||
}
|
||
|
||
// ── Workspace check ───────────────────────────────────────────────
|
||
/**
|
||
* Check whether a target point is safe to move to.
|
||
*
|
||
* Order of checks (fail-fast):
|
||
* 1. Board outer boundary
|
||
* 2. Mechanism housing exclusion box
|
||
* 3. IK geometric reachability
|
||
* 4. Elbow constraints (prevents crossing and hitting the bottom)
|
||
*
|
||
* @param {number} x
|
||
* @param {number} y
|
||
* @returns {{ ok: boolean, reason: string }}
|
||
*/
|
||
function checkWorkspace(x, y) {
|
||
const L = LIMITS;
|
||
|
||
// 1. Board boundary
|
||
if (x < L.X_MIN || x > L.X_MAX)
|
||
return { ok: false, reason: `X=${x.toFixed(1)} mm outside board (${L.X_MIN}…${L.X_MAX})` };
|
||
if (y < L.Y_MIN || y > L.Y_MAX)
|
||
return { ok: false, reason: `Y=${y.toFixed(1)} mm outside board (${L.Y_MIN}…${L.Y_MAX})` };
|
||
|
||
// 2. Centre mechanism exclusion box
|
||
if (x > -L.BOX_HALF_W && x < L.BOX_HALF_W &&
|
||
y > L.BOX_Y_MIN && y < L.BOX_Y_MAX)
|
||
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is inside the mechanism housing` };
|
||
|
||
// 3. IK geometric reachability
|
||
const { theta1, theta2, reachable } = solve(x, y);
|
||
if (!reachable)
|
||
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is geometrically unreachable` };
|
||
|
||
// 4. Elbow-crossing guard
|
||
if (armsCrossed(theta1, theta2))
|
||
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) would cross elbows near the mechanism box` };
|
||
|
||
return { ok: true, reason: '' };
|
||
}
|
||
|
||
// ── Lookup ────────────────────────────────────────────────────────
|
||
function lookup(char) {
|
||
return LOOKUP_TABLE[char.toUpperCase()] ?? null;
|
||
}
|
||
|
||
// ── Unit converters ───────────────────────────────────────────────
|
||
function stepsToRad(steps) {
|
||
return (steps / ARM.STEPS_PER_REV) * 2 * Math.PI;
|
||
}
|
||
function radToSteps(rad) {
|
||
return Math.round((rad / (2 * Math.PI)) * ARM.STEPS_PER_REV);
|
||
}
|
||
function stepsToDeg(steps) {
|
||
return steps * ARM.STEP_ANGLE_DEG;
|
||
}
|
||
function radToDeg(rad) {
|
||
return rad * 180 / Math.PI;
|
||
}
|
||
|
||
export default {
|
||
ARM, LIMITS, LOOKUP_TABLE,
|
||
solve, forward, armsCrossed, checkWorkspace, lookup,
|
||
stepsToRad, radToSteps, stepsToDeg, radToDeg,
|
||
};
|