/*** * RobotController — Steuerlogik (ToDo_6). * * Verarbeitet bereits geparste G-Code-/Befehlsobjekte (aus GCodeParser) und wendet * sie auf das Robotermodell an: Bewegungsmodus, Kinematik, sendCommand(). * * Bewusst getrennt vom Parsing (GCodeParser) und vom Datei-Handling (GCode): * der Controller kennt nur strukturierte Befehle, keine rohen Textstrings. */ const GCodeParser = require('./GCodeParser'); const { motorStateFromPorts, D } = require('./portInverse'); class RobotController { /** * Parst eine rohe Nachricht und wendet alle enthaltenen Befehle der Reihe nach an. * * Rückgabe: `undefined` (synchron, wie bisher) für alle gewöhnlichen Befehle. * Nur wenn ein asynchroner Befehl enthalten war (Hardware-Sync, ToDo_9 Paket 4) * wird ein Promise zurückgegeben, das auf dessen Abschluss wartet. * @param {object} robot Robotermodell * @param {string|Buffer} message rohe G-Code-Nachricht */ static receive(robot, message) { const commands = GCodeParser.parse(message); if (!commands.length) return; const results = commands.map(parsed => this.applyCommand(robot, parsed)); const pending = results.filter(r => r && typeof r.then === 'function'); if (pending.length === 0) return; // synchroner Pfad unverändert return Promise.all(pending).then(arr => arr[arr.length - 1]); } /** * Wendet einen einzelnen, bereits geparsten Befehl auf das Robotermodell an. * @param {object} robot * @param {{command: string, params: object}} parsed */ static applyCommand(robot, parsed) { if (!parsed || !parsed.command) return; const cmd = parsed.command; const params = parsed.params || {}; if (cmd === 'G90') { robot.moveRelative = false; return; } if (cmd === 'G91') { robot.moveRelative = true; return; } if (cmd === 'G28') { // Home = Grundstellung: Arm + Hand gestreckt entlang -y (siehe // doc/Info_Koordinaten.md). Ziel-Fingerspitze: (0, -(l1+l2+l3), 0). const reach = robot.l1 + robot.l2 + robot.l3; const homeY = -reach; if (Math.abs(Math.abs(homeY) - reach) < 1e-6) { // Sonderfall voll ausgestreckt: |y| = l1+l2+l3 ist eine Handgelenk- // Singularität — die IK kann den Unterarm-Dreher a (und damit c) dort // nicht bestimmen und liefert Müll (z.B. a=135°, c=45°), wodurch der // Finger schräg/nach unten zeigt. Daher die Motorwerte DIREKT in die // Grundstellung setzen und die Workspace-Pose per FK füllen. robot.xMotor = 0; robot.alpha = 0; robot.beta = 0; robot.a = 0; robot.b = Math.PI; // gerade Hand (Phase-1-Konvention; Phase 2: 0) robot.c = 0; // Greifer (e/eMotor) bewusst UNANGETASTET lassen: G28 fährt nur den Arm. // Würde man e=0 setzen, ergäbe die Kopplung eMotor = e − b − c bei b=π den // Wert −π → −180° am Finger-Motor → Anschlag-Slam (siehe // doc/Info_Koordinaten.md, Phase 2). Phase 2 (b=0 = gerade) löst das sauber. robot.calculatePositionFromMotorAngles(); // FK -> x=0, y=-(l1+l2+l3), z=0 } else { // Allgemeiner (nicht-singulärer) Home-Punkt: regulär über die IK. robot.x = 0; robot.y = homeY; robot.z = 0; robot.phi = Math.PI / 2; robot.theta = Math.PI / 2; robot.psi = 0; robot.e = 0; robot.calculateAngles3D(); } robot.sendCommand(); return; } if (cmd === 'G1') { if (robot.moveRelative) { if (Number.isFinite(params.X)) { robot.x += params.X; robot.xMotorChanged = true; } if (Number.isFinite(params.Y)) { robot.y += params.Y; robot.yMotorChanged = true; } if (Number.isFinite(params.Z)) { robot.z += params.Z; robot.zMotorChanged = true; } if (Number.isFinite(params.A)) { robot.phi += params.A; robot.aMotorChanged = true; } if (Number.isFinite(params.B)) { robot.theta += params.B; robot.bMotorChanged = true; } if (Number.isFinite(params.C)) { robot.psi += params.C; robot.cMotorChanged = true; } if (Number.isFinite(params.E)) { robot.e += params.E; robot.eMotorChanged = true; } if (Number.isFinite(params.F)) { robot.feedrate = params.F; } } else { if (Number.isFinite(params.X)) { robot.x = params.X; robot.xMotorChanged = true; } if (Number.isFinite(params.Y)) { robot.y = params.Y; robot.yMotorChanged = true; } if (Number.isFinite(params.Z)) { robot.z = params.Z; robot.zMotorChanged = true; } if (Number.isFinite(params.A)) { robot.phi = params.A; robot.aMotorChanged = true; } if (Number.isFinite(params.B)) { robot.theta = params.B; robot.bMotorChanged = true; } if (Number.isFinite(params.C)) { robot.psi = params.C; robot.cMotorChanged = true; } if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; } if (Number.isFinite(params.F)) { robot.feedrate = params.F; } } robot.calculateAngles3D(); robot.sendCommand(); return; } if (cmd === 'M1') { if (robot.moveRelative) { if (Number.isFinite(params.X)) { robot.xMotor += params.X; robot.xMotorChanged = true; } if (Number.isFinite(params.Y)) { robot.alpha += params.Y; robot.yMotorChanged = true; } if (Number.isFinite(params.Z)) { robot.beta += params.Z; robot.zMotorChanged = true; } if (Number.isFinite(params.A)) { robot.a += params.A; robot.aMotorChanged = true; } if (Number.isFinite(params.B)) { robot.b += params.B; robot.bMotorChanged = true; } if (Number.isFinite(params.C)) { robot.c += params.C; robot.cMotorChanged = true; } if (Number.isFinite(params.E)) { robot.e += params.E; robot.eMotorChanged = true; } } else { if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; } if (Number.isFinite(params.Y)) { robot.alpha = params.Y; robot.yMotorChanged = true; } if (Number.isFinite(params.Z)) { robot.beta = params.Z; robot.zMotorChanged = true; } if (Number.isFinite(params.A)) { robot.a = params.A; robot.aMotorChanged = true; } if (Number.isFinite(params.B)) { robot.b = params.B; robot.bMotorChanged = true; } if (Number.isFinite(params.C)) { robot.c = params.C; robot.cMotorChanged = true; } if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; } } robot.calculatePositionFromMotorAngles(); robot.sendCommand(); return; } if (cmd === 'M114' && params.R === true) { // Hardware-Sync (ToDo_9 Paket 4): liest die echten MPos aller Controller // und übernimmt sie als Soll-Zustand. Asynchron → gibt ein Promise zurück. return this.syncFromHardware(robot); } if (cmd === 'M92' || cmd === 'G92') { // Beide setzen die Motorposition ohne Bewegung, unterscheiden sich aber in den // Winkel-EINHEITEN: // G92 → GRAD (G-Code-Konvention für Rotationsachsen, wie FluidNC und die // "Position Motoren"-Anzeige in public/app.js). Intern sind die // Winkel-Slots in Radiant → Grad/D umrechnen (D = 180/π). // M92 → RADIANT, roh in die internen Slots (interne/Test-Variante). // X ist die lineare mm-Schiene, E die Greifer-Öffnung in mm (ab Null-Position // eines Fingers) — beide ohne Winkel-Umrechnung. const angScale = (cmd === 'G92') ? 1 / D : 1; robot.createMotorPosition(); if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; } if (Number.isFinite(params.Y)) { robot.alpha = params.Y * angScale; robot.yMotorChanged = true; } if (Number.isFinite(params.Z)) { robot.beta = params.Z * angScale; robot.zMotorChanged = true; } if (Number.isFinite(params.A)) { robot.a = params.A * angScale; robot.aMotorChanged = true; } if (Number.isFinite(params.B)) { robot.b = params.B * angScale; robot.bMotorChanged = true; } if (Number.isFinite(params.C)) { robot.c = params.C * angScale; robot.cMotorChanged = true; } // E nach B/C setzen: der Greifer-Motorwert hängt über die Kinematik-Kopplung // von b und c ab. robot.e = Finger-Öffnung (mm), eMotor = abgeleiteter Motorwert. // Ohne diese eMotor-Ableitung bliebe der Greiferwert stale (alte E-Inkonsistenz): // sendCommand() verschickt eMotor, nicht e. if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotor = robot.gripperMotorFromOpening(robot.e); robot.eMotorChanged = true; } robot.calculatePositionFromMotorAngles(); robot.sendCommand('G92'); return; } } /** * Hardware-Sync (ToDo_9 Paket 4): liest die echten Achs-Positionen (`MPos`) der * drei Controller, rekonstruiert daraus die sieben Motorwerte und übernimmt sie * als neuen Soll-Zustand. Danach Vorwärtskinematik → Pose. * * Bewegt den Roboter NICHT — es wird kein `sendCommand()`/Move ausgelöst, nur der * interne Zustand an die Realität angeglichen (nach Homing/Jog/Stall/Reconnect). * * @param {object} robot * @param {{timeoutMs?: number}} [options] * @returns {Promise<{x,y,z,phi,theta,psi}>} die übernommene Pose */ static async syncFromHardware(robot, options = {}) { const timeoutMs = Number.isFinite(options.timeoutMs) ? options.timeoutMs : 1000; const receivers = (robot && robot.cmdReceivers) || []; // Sender nach Rolle zuordnen (controllerRole wird in startRobot.js gesetzt). const byRole = {}; for (const s of receivers) { if (s && s.controllerRole) byRole[s.controllerRole] = s; } for (const role of ['base', 'elbow', 'hand']) { if (!byRole[role]) { throw new Error(`Sync: Controller '${role}' fehlt (kein Sender mit controllerRole='${role}')`); } } // Frische Reports von allen drei Controllern anfordern (aktiv '?', mit await). const [baseSnap, elbowSnap, handSnap] = await Promise.all([ byRole.base.requestStatusReport(timeoutMs), byRole.elbow.requestStatusReport(timeoutMs), byRole.hand.requestStatusReport(timeoutMs), ]); // MPos-Arrays validieren (FluidNC meldet ggf. mehr Achsen; nur die nötigen lesen). const need = (snap, role, n) => { const mp = snap && snap.machinePosition; if (!Array.isArray(mp) || mp.length < n || !mp.slice(0, n).every(Number.isFinite)) { throw new Error(`Sync: ${role} lieferte keine gültige MPos (${JSON.stringify(mp)})`); } return mp; }; const b = need(baseSnap, 'base', 3); const e = need(elbowSnap, 'elbow', 1); const h = need(handSnap, 'hand', 3); // Port → Motorwerte (linear/eindeutig, ToDo_9a) → auf den Roboter schreiben. const m = motorStateFromPorts({ base: { x: b[0], y: b[1], z: b[2] }, elbow: { x: e[0] }, hand: { x: h[0], y: h[1], z: h[2] }, }); robot.xMotor = m.xMotor; robot.alpha = m.alpha; robot.beta = m.beta; robot.a = m.a; robot.b = m.b; robot.c = m.c; robot.eMotor = m.eMotor; // Vorwärtskinematik: füllt x/y/z + phi/theta/psi aus den Hardwarewerten. robot.calculatePositionFromMotorAngles(); // motorPosition zurücksetzen, damit der nächste Move den Speed-Delta von der // echten Position aus rechnet (sonst falscher Feedrate im Korrekt-Modus). robot.motorPosition = null; robot.motorPositionOld = null; return { x: robot.x, y: robot.y, z: robot.z, phi: robot.phi, theta: robot.theta, psi: robot.psi, }; } } module.exports = RobotController;