PROFERIS - Mi³osz Stocki
40 lines
1.3 KiB
JavaScript
40 lines
1.3 KiB
JavaScript
const ARM = { d2: 12.9, l1: 85.0, l2: 110.0, STEPS_PER_REV: 2048 };
|
|
const HOME_STEPS = { m1: 1024, m2: 0 };
|
|
|
|
function solve(x, y) {
|
|
const { d2, l1, l2 } = ARM;
|
|
const xpd = x + d2;
|
|
const t = Math.sqrt(xpd * xpd + y * y);
|
|
const cosW2 = (l2 * l2 - t * t - l1 * l1) / (-2 * l1 * t);
|
|
const r = Math.atan2(y, xpd);
|
|
const w2 = Math.acos(cosW2);
|
|
const theta1 = r + w2;
|
|
|
|
const xmd = x - d2;
|
|
const s = Math.sqrt(xmd * xmd + y * y);
|
|
const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
|
|
const q = Math.atan2(y, xmd);
|
|
const w1 = Math.acos(cosW1);
|
|
const theta2 = q - w1;
|
|
return { theta1, theta2 };
|
|
}
|
|
|
|
function radToSteps(rad) { return Math.round((rad / (2 * Math.PI)) * ARM.STEPS_PER_REV); }
|
|
|
|
const targetX = 115, targetY = 113;
|
|
const { theta1, theta2 } = solve(targetX, targetY);
|
|
console.log(`Target: (${targetX}, ${targetY})`);
|
|
console.log(`theta1: ${theta1 * 180 / Math.PI} deg`);
|
|
console.log(`theta2: ${theta2 * 180 / Math.PI} deg`);
|
|
|
|
const newSteps1 = radToSteps(theta1);
|
|
const newSteps2 = radToSteps(theta2);
|
|
console.log(`newSteps1: ${newSteps1}`);
|
|
console.log(`newSteps2: ${newSteps2}`);
|
|
|
|
console.log(`delta1 from HOME_STEPS.m1 (1024): ${newSteps1 - HOME_STEPS.m1}`);
|
|
console.log(`delta2 from HOME_STEPS.m2 (0): ${newSteps2 - HOME_STEPS.m2}`);
|
|
|
|
const delta1_old = newSteps1 - (-1024);
|
|
console.log(`delta1 from OLD_HOME_STEPS.m1 (-1024): ${delta1_old}`);
|