Kleine Arbeiten

robot/GCode.js

@@ -31,6 +31,7 @@ class GCode{
     static containsCommand(s){
 
         if(s.indexOf('M1 ') !== -1){return true;} // M1-Commands = G1-Command only for Motor-Coordinates
+        if(s.indexOf('M114') === 0){return true;} // M114 R - Hardware-Sync (MPos lesen, ToDo_9 Paket 4)
         if(s.indexOf('G') !== 0){return false;}
         if(s.indexOf('G90') ==  0){return true;}
         if(s.indexOf('G91') ==  0){return true;}
@@ -88,7 +89,9 @@ class GCode{
      * funktionieren.
      */
     static receiveGCode(robot, g){
-        RobotController.receive(robot, g);
+        // Rückgabe durchreichen: synchron `undefined` wie bisher, oder ein Promise,
+        // falls ein asynchroner Befehl (Hardware-Sync, ToDo_9 Paket 4) enthalten war.
+        return RobotController.receive(robot, g);
     }
 
     //////////////////////////////////////////////////////////////////////////////////////////////////////////////77

robot/GCodeParser.js

@@ -43,7 +43,14 @@ class GCodeParser {
         const params = {};
         for (let i = 1; i < tokens.length; i++) {
             const token = tokens[i].trim();
-            if (token.length < 2) {
+            if (token.length === 0) {
+                continue;
+            }
+            if (token.length === 1) {
+                // Einzelner Buchstabe = Flag ohne Zahlenwert (z. B. 'R' in 'M114 R').
+                if (/^[A-Za-z]$/.test(token)) {
+                    params[token.toUpperCase()] = true;
+                }
                 continue;
             }
 

robot/RobotController.js

@@ -8,18 +8,26 @@
  * der Controller kennt nur strukturierte Befehle, keine rohen Textstrings.
  */
 const GCodeParser = require('./GCodeParser');
+const { motorStateFromPorts } = 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;
-        commands.forEach(parsed => this.applyCommand(robot, parsed));
+        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]);
     }
 
     /**
@@ -106,6 +114,12 @@ class RobotController {
             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') {
             robot.createMotorPosition();
             if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; }
@@ -121,6 +135,80 @@ class RobotController {
             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;

robot/TelnetSenderGRBL.js

@@ -54,6 +54,33 @@ module.exports = class TelnetSenderGRBL extends SenderInterface {
         this.autoConnect = options.autoConnect !== false;
         this.isTestMode = false;
 
+        // ── Hardware-Feedback (ToDo_9 Paket 1/3) ──────────────────────────────
+        // Eingehende GRBL/FluidNC-Antworten werden geparst (vorher verworfen).
+        this._rxBuffer = '';              // Zeilen-Puffer für fragmentierte data-Events
+        this.grblState = null;            // 'Idle' | 'Run' | 'Alarm' | 'Hold' | ...
+        this.machinePosition = null;      // [x, y, z, …] aus MPos (oder WPos)
+        this.machinePositionType = null;  // 'MPos' | 'WPos'
+        this.plannerBlocksFree = null;    // erste Bf-Zahl (freie Planner-Blöcke)
+        this.rxBytesFree = null;          // zweite Bf-Zahl (freie RX-Bytes)
+        this.lastResponse = null;         // letzte empfangene Zeile (roh)
+        this.lastError = null;            // letzte error:/ALARM:-Zeile
+        this.lastOk = 0;                  // Zeitstempel des letzten 'ok'
+        this.lastReportAt = 0;            // Zeitstempel des letzten <…>-Reports
+        this._statusWaiters = [];         // offene requestStatusReport()-Promises (Sync, Paket 4)
+
+        // Auto-Reporting (Paket 3) — opt-in, schreibt persistente FluidNC-Settings.
+        // Default AUS: ohne Flag wird KEIN $10/$Report-Kommando an die Hardware
+        // gesendet; der Treiber liest nur die Antworten des ohnehin laufenden
+        // ?-Heartbeats.
+        this.autoReport = options.autoReport !== undefined
+            ? !!options.autoReport
+            : (process.env.ROBOT_GRBL_AUTOREPORT === 'true');
+        this.reportInterval = Number.isFinite(options.reportInterval)
+            ? options.reportInterval
+            : (Number.isFinite(Number(process.env.ROBOT_GRBL_REPORT_INTERVAL))
+                ? Number(process.env.ROBOT_GRBL_REPORT_INTERVAL)
+                : 200);
+
         if (urlGRBL === "test.test") {
             this.tSocket = { written: "", write(txt){ this.written = txt; } };
             this.isTestMode = true;
@@ -128,6 +155,7 @@ module.exports = class TelnetSenderGRBL extends SenderInterface {
             this.tSocket.on('close', () => {
                 console.log("Telnet Closed " + this.urlGRBLstr);
                 this._stopHeartbeat();
+                this._rejectStatusWaiters(new Error(`${this.urlGRBLstr}: Verbindung geschlossen`));
                 this.tSocket = null;
                 this._rawSocket = null;
                 if (this.shouldReconnect) {
@@ -141,6 +169,13 @@ module.exports = class TelnetSenderGRBL extends SenderInterface {
             // Zeitstempel jeder eingehenden Nachricht festhalten (für Heartbeat-Timeout).
             socket.on('data', () => { this._lastDataAt = Date.now(); });
 
+            // Eingehende GRBL/FluidNC-Antworten lesen (ToDo_9 Paket 1).
+            // Vorher wurde dieser Kanal verworfen — jetzt werden ok/error/<…>-Reports
+            // geparst. Frischer Zeilen-Puffer pro Verbindung (Reste der toten
+            // Verbindung dürfen die neue nicht verfälschen).
+            this._rxBuffer = '';
+            this.tSocket.on('data', (chunk) => this._handleIncomingData(chunk));
+
             this.state = 'connected';
             this.error = null;
             this.reconnectAttempt = 0;
@@ -153,6 +188,9 @@ module.exports = class TelnetSenderGRBL extends SenderInterface {
 
             // Heartbeat starten: erkennt NotAus / tote Verbindungen.
             this._startHeartbeat();
+
+            // Auto-Reporting konfigurieren (Paket 3) — nur bei aktivem Opt-in.
+            this._configureAutoReport();
         });
 
         socket.on('error', (error) => {
@@ -238,6 +276,182 @@ module.exports = class TelnetSenderGRBL extends SenderInterface {
         }
     }
 
+    /**
+     * Konfiguriert FluidNC-Auto-Reporting (ToDo_9 Paket 3) — nur bei aktivem Opt-in.
+     *
+     * Schreibt persistente Controller-Settings:
+     *   $10=3                → Statusreport enthält MPos und Bf
+     *   $Report/Interval=N   → FluidNC pusht den Status während der Bewegung selbst
+     *
+     * Default AUS (ROBOT_GRBL_AUTOREPORT != 'true'): ohne Opt-in wird NICHTS gesendet,
+     * der Treiber liest nur die Antworten des ohnehin laufenden ?-Heartbeats.
+     * @private
+     */
+    _configureAutoReport() {
+        if (!this.autoReport || !this.tSocket) return;
+        try {
+            this.tSocket.write('$10=3\r\n');
+            this.tSocket.write(`$Report/Interval=${this.reportInterval}\r\n`);
+            console.log(
+                `[TelnetSenderGRBL] ${this.urlGRBLstr}: Auto-Reporting aktiviert ` +
+                `($10=3, $Report/Interval=${this.reportInterval})`
+            );
+        } catch (err) {
+            // Fehler kommt ohnehin über das 'error'-Event; hier nur defensiv loggen.
+            console.log(
+                `[TelnetSenderGRBL] ${this.urlGRBLstr}: Auto-Report-Setup fehlgeschlagen: ${err.message}`
+            );
+        }
+    }
+
+    /**
+     * Verarbeitet einen eingehenden data-Chunk vom TelnetSocket (ToDo_9 Paket 1).
+     * Puffert über Zeilengrenzen (TCP-Chunks zerteilen Nachrichten beliebig) und
+     * gibt jede vollständige Zeile an _handleResponseLine().
+     *
+     * Wirft nie — ein Parsefehler darf den data-Handler (und damit den Prozess)
+     * nicht abreißen lassen.
+     * @private
+     */
+    _handleIncomingData(chunk) {
+        try {
+            this._rxBuffer += chunk.toString('utf8');
+
+            // Schutz gegen unbegrenztes Wachstum, falls Zeilenenden ausbleiben.
+            if (this._rxBuffer.length > 8192) {
+                this._rxBuffer = this._rxBuffer.slice(-8192);
+            }
+
+            let idx;
+            while ((idx = this._rxBuffer.search(/\r?\n/)) !== -1) {
+                const line  = this._rxBuffer.slice(0, idx);
+                const nlLen = this._rxBuffer[idx] === '\r' ? 2 : 1;
+                this._rxBuffer = this._rxBuffer.slice(idx + nlLen);
+                this._handleResponseLine(line);
+            }
+        } catch (err) {
+            console.log(`[TelnetSenderGRBL] ${this.urlGRBLstr}: data-Parse-Fehler: ${err.message}`);
+        }
+    }
+
+    /**
+     * Klassifiziert eine einzelne Antwortzeile und aktualisiert den geparsten Zustand.
+     * Demultiplext nach Nachrichtentyp (ToDo_9, Protokoll-Fakt 5): toleriert fremde
+     * Zeilen (Cross-Channel-Bleed-Through), nimmt kein striktes 1:1 Request→Response an.
+     * @private
+     */
+    _handleResponseLine(line) {
+        const trimmed = line.trim();
+        if (!trimmed) return;
+        this.lastResponse = trimmed;
+
+        if (trimmed[0] === '<') {
+            this._parseStatusReport(trimmed);
+            return;
+        }
+        if (trimmed === 'ok') {
+            this.lastOk = Date.now();
+            return;
+        }
+        if (/^error:/i.test(trimmed) || /^ALARM/i.test(trimmed)) {
+            this.lastError = trimmed;
+            console.log(`[TelnetSenderGRBL] ${this.urlGRBLstr}: GRBL meldet ${trimmed}`);
+            return;
+        }
+        // Alles andere (Start-Banner, [MSG:…], Echo, fremde Kanäle): bewusst ignorieren.
+    }
+
+    /**
+     * Parst einen FluidNC-Statusreport: <State|MPos:x,y,z|Bf:blocks,bytes|…>.
+     * Speichert State, Maschinenposition (MPos bevorzugt, sonst WPos) und Bf.
+     * Defensiv: unvollständige/zerstörte Felder werden übersprungen, nie geworfen.
+     * @private
+     */
+    _parseStatusReport(line) {
+        const inner = line.replace(/^</, '').replace(/>$/, '');
+        const fields = inner.split('|');
+        if (!fields.length) return;
+
+        if (fields[0]) this.grblState = fields[0];
+
+        for (let i = 1; i < fields.length; i++) {
+            const sep = fields[i].indexOf(':');
+            if (sep === -1) continue;
+            const key   = fields[i].slice(0, sep);
+            const value = fields[i].slice(sep + 1);
+            if (!value) continue;
+
+            if (key === 'MPos' || key === 'WPos') {
+                const nums = value.split(',').map(Number);
+                if (nums.length && nums.every(Number.isFinite)) {
+                    this.machinePosition = nums;
+                    this.machinePositionType = key;
+                }
+            } else if (key === 'Bf') {
+                const nums = value.split(',').map(Number);
+                if (Number.isFinite(nums[0])) this.plannerBlocksFree = nums[0];
+                if (Number.isFinite(nums[1])) this.rxBytesFree = nums[1];
+            }
+        }
+        this.lastReportAt = Date.now();
+        this._resolveStatusWaiters();
+    }
+
+    /**
+     * Fordert einen frischen Statusreport an (ToDo_9 Paket 4): sendet das
+     * FluidNC-Realtime-Byte '?' und wartet auf den nächsten geparsten `<…>`-Report.
+     *
+     * Löst mit einem Snapshot ({grblState, machinePosition, …}) auf, sobald ein
+     * Report eintrifft, oder wirft nach `timeoutMs` ohne Antwort. Verändert den
+     * Roboterzustand NICHT — reines Lesen.
+     *
+     * @param {number} timeoutMs
+     * @returns {Promise<{grblState, machinePosition, machinePositionType, plannerBlocksFree, rxBytesFree}>}
+     */
+    requestStatusReport(timeoutMs = 1000) {
+        if (!this.tSocket || typeof this.tSocket.write !== 'function') {
+            return Promise.reject(new Error(`${this.urlGRBLstr}: not connected`));
+        }
+        return new Promise((resolve, reject) => {
+            const waiter = { resolve, reject, timer: null };
+            waiter.timer = this.setTimeoutFn(() => {
+                this._statusWaiters = this._statusWaiters.filter(w => w !== waiter);
+                reject(new Error(`${this.urlGRBLstr}: Statusreport-Timeout nach ${timeoutMs}ms`));
+            }, timeoutMs);
+            this._statusWaiters.push(waiter);
+            try {
+                this.tSocket.write('?');
+            } catch (err) {
+                this.clearTimeoutFn(waiter.timer);
+                this._statusWaiters = this._statusWaiters.filter(w => w !== waiter);
+                reject(err);
+            }
+        });
+    }
+
+    /** Löst alle offenen requestStatusReport()-Promises mit dem aktuellen Snapshot auf. @private */
+    _resolveStatusWaiters() {
+        if (!this._statusWaiters || this._statusWaiters.length === 0) return;
+        const snapshot = {
+            grblState: this.grblState,
+            machinePosition: this.machinePosition,
+            machinePositionType: this.machinePositionType,
+            plannerBlocksFree: this.plannerBlocksFree,
+            rxBytesFree: this.rxBytesFree,
+        };
+        const waiters = this._statusWaiters;
+        this._statusWaiters = [];
+        waiters.forEach(w => { this.clearTimeoutFn(w.timer); w.resolve(snapshot); });
+    }
+
+    /** Bricht offene requestStatusReport()-Promises ab (z. B. bei Verbindungsverlust). @private */
+    _rejectStatusWaiters(reason) {
+        if (!this._statusWaiters || this._statusWaiters.length === 0) return;
+        const waiters = this._statusWaiters;
+        this._statusWaiters = [];
+        waiters.forEach(w => { this.clearTimeoutFn(w.timer); w.reject(reason); });
+    }
+
     send(command) {
         if (!this.tSocket || typeof this.tSocket.write !== 'function') {
             return false;
@@ -259,12 +473,22 @@ module.exports = class TelnetSenderGRBL extends SenderInterface {
             error: this.error,
             isTestMode: !!this.isTestMode,
             reconnectAttempt: this.reconnectAttempt,
-            reconnectTimer: !!this.reconnectTimer
+            reconnectTimer: !!this.reconnectTimer,
+            // Hardware-Feedback (ToDo_9 Paket 1/3)
+            grblState: this.grblState,
+            machinePosition: this.machinePosition,
+            machinePositionType: this.machinePositionType,
+            plannerBlocksFree: this.plannerBlocksFree,
+            rxBytesFree: this.rxBytesFree,
+            lastError: this.lastError,
+            lastReportAt: this.lastReportAt,
+            autoReport: !!this.autoReport
         };
     }
 
     disconnect() {
         this._stopHeartbeat();
+        this._rejectStatusWaiters(new Error(`${this.urlGRBLstr}: disconnect`));
 
         if (this.isTestMode) {
             this.tSocket = null;

robot/portInverse.js

@@ -0,0 +1,38 @@
+/**
+ * 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 };