39 lines
1.8 KiB
JavaScript
39 lines
1.8 KiB
JavaScript
/**
|
||
* Port→Motor-Rückrechnung (ToDo_9 Paket 4, Baustein).
|
||
*
|
||
* Rekonstruiert aus den von den drei GRBL/FluidNC-Controllern gemeldeten
|
||
* Maschinen-Achswerten (`MPos`) die sieben Motorwerte des Roboters.
|
||
*
|
||
* Herleitung + Verifikation: doc/ToDo_9a_PortRueckrechnung.md
|
||
* Tests: test/Robot.PortInverse.test.js (15 Tests)
|
||
*
|
||
* Gilt für die PRODUKTIV-Verkabelung (startRobot.js):
|
||
* base: GRBL x←xMotor, y←alpha·D, z←(beta−alpha)·D
|
||
* elbow: GRBL x←a·D
|
||
* hand: GRBL x←(c−b)·D, y←eMotor·D, z←b·D
|
||
*
|
||
* Die Abbildung ist linear und EINDEUTIG umkehrbar — keine Zweig-Wahl nötig.
|
||
* Ändert sich die Verkabelung in startRobot.js, muss diese Umkehrung mitgezogen
|
||
* werden; der Round-Trip-Test `portValue(motorStateFromPorts(p)) ≈ p` schützt davor.
|
||
*/
|
||
|
||
const D = 180 / Math.PI;
|
||
|
||
/**
|
||
* @param {{base:{x:number,y:number,z:number}, elbow:{x:number}, hand:{x:number,y:number,z:number}}} r
|
||
* GRBL-Readings (Grad bzw. mm) der drei Controller.
|
||
* @returns {{xMotor:number, alpha:number, beta:number, a:number, b:number, c:number, eMotor:number}}
|
||
*/
|
||
function motorStateFromPorts(r) {
|
||
const xMotor = r.base.x; // x-Port = xMotor (mm, direkt)
|
||
const alpha = r.base.y / D; // y-Port = alpha·D
|
||
const beta = (r.base.z + r.base.y) / D; // z-Port = (beta−alpha)·D ⇒ beta = z/D + alpha
|
||
const a = r.elbow.x / D; // Elbow x-Port = a·D
|
||
const b = r.hand.z / D; // Hand z-Port = b·D
|
||
const c = (r.hand.x + r.hand.z) / D; // Hand x-Port = (c−b)·D ⇒ c = x/D + b
|
||
const eMotor = r.hand.y / D; // Hand y-Port = eMotor·D
|
||
return { xMotor, alpha, beta, a, b, c, eMotor };
|
||
}
|
||
|
||
module.exports = { motorStateFromPorts, D };
|