# ToDo 9 — Hardware-Feedback-Loop ## Ziel Der Roboter-Treiber soll nicht nur Befehle senden, sondern auch Antworten der Hardware lesen. Nur so können Fehler erkannt, Positionen verifiziert und Befehlsfolgen zuverlässig synchronisiert werden. Aktuell ist der Datenfluss vollständig blind: ``` WebSocket → GCode → calculateAngles3D → sendCommand → tSocket.write() ↓ GRBL antwortet mit "ok" / "error" → wird nie gelesen (data => {}) ``` > Der blinde Kanal ist genau eine Stelle: `socket.on('data', () => {})` in > `robot/TelnetSenderGRBL.js` (Z. ~123). Alles in diesem ToDo hängt daran. --- ## FluidNC-Protokoll: gesicherte Fakten (recherchiert) Quellen: [Serial Protocol](http://wiki.fluidnc.com/en/support/serial_protocol), [Automatic Reporting](http://wiki.fluidnc.com/en/support/interface/automatic_reporting), [Cross-Channel #750](https://github.com/bdring/FluidNC/issues/750). 1. **`?` funktioniert über Telnet.** FluidNC verarbeitet Realtime-Kommandos (`?`, Feed-Hold, Cycle-Start, Reset) über eine **Channel-Abstraktion** auf *allen* Kanälen (USB, WiFi, Telnet, WebUI). Der bestehende `telnet-stream`-Socket kann `?` senden und den `<…>`-Report zurücklesen. 2. **Statuswahl über `$10` (Bitmaske).** Bit0 = `MPos` statt `WPos`, Bit1 = `Bf` (Puffer). → **`$10=3` liefert `MPos` + `Bf`** (auf der eingesetzten FluidNC-Version verifizieren). - **`MPos` ist die richtige Quelle** für die Rückrechnung: der Treiber sendet die `portValue()`-Werte als *absolute* G-Code-Koordinaten, `MPos` ist offset-fest. - `WCO` (Work Coordinate Offset) erscheint periodisch; `WPos = MPos − WCO`. Für uns irrelevant, solange wir `MPos` lesen. - Beispiel: `` 3. **`Bf:` = das kanonische Flow-Control-Signal.** Erste Zahl = freie Planner-Blöcke, zweite = freie RX-Bytes. Damit liest man die **echte** Pufferfüllung, statt sie über `moveTime` zu *schätzen* (das ignoriert Beschleunigung). 4. **Auto-Reporting statt Polling.** `$Report/Interval=N` (ms) lässt FluidNC den Status während der Bewegung **selbst** pushen — **pro Kanal** einstellbar, im Stillstand nur bei Änderung. Ersetzt das `?`-Polling und hält die Netzlast vorhersehbar gedeckelt. 5. **Cross-Channel-Bleed-Through (Caveat).** `ok`/Reports können auf *anderen* Kanälen auftauchen als dem auslösenden (Issue #750). Da pro FluidNC mehrere Kanäle aktiv sind (Treiber-Telnet **und** die `appRobot_Access*`-WebUI-Container), muss der Parser nach **Nachrichtentyp** demultiplexen (`ok` / `error:` / `<…>`) und fremde Zeilen tolerieren — **kein** striktes 1:1 Request→Response annehmen. --- ## Designentscheidungen (festgeschrieben) **B3 — Umkehr-Kinematik ist disambiguierbar.** Global nicht eindeutig, aber im Arbeitsraum dieses Roboters per **dokumentierter physikalischer Zusatzbedingung** auflösbar (z. B. „Ellbogen höher als Hand" bzw. „Ellbogen hinter der x-Achse"). `motorStateFromPorts()` bekommt eine feste Zweig-Wahl-Regel; die exakte Vorzeichen-Konvention wird beim Herleiten der Umkehrung gepinnt. **B5 — Lockstep als abschaltbare Absicherung.** Durch die koordinierte Feedrate (ToDo_6a `correct`) treffen ohnehin alle Achsen *gleichzeitig* am nächsten Ziel ein — Lockstep ist die Absicherung, nicht der Hauptmechanismus. Umsetzung als Env-Schalter; **Freerun zuerst** (reines Zeit-/`Bf`-Pacing), echtes Lockstep erst nach Paket 3/5 (braucht den Feedback-Kanal). **B6 — Sync ist ein G-Code-Befehl.** Läuft durch `GCodeParser` + `RobotController` (nicht als Sonderfall in `InputWS` wie heute `M114`). **Folge:** Sync ist der erste *asynchrone* Befehl — er muss auf die `?`-Antworten aller drei Controller *warten*, bevor er den Roboterzustand setzt. Die Dispatch-Kette (`RobotController.applyCommand`) muss dafür erstmals einen Promise zurückgeben/awaiten können. ### Schalter-Übersicht (Namen sind Vorschläge) | Env | Werte | Default | Wirkung | |---|---|---|---| | `ROBOT_SPEED_MODE` | `legacy` / `correct` | `legacy` | koordinierte Feedrate (ToDo_6a) | | `ROBOT_USE_QUEUE` | `false` / `true` | `false` | zeitgesteuerte Sende-Queue (Paket 6) | | `ROBOT_MOTION_SYNC` | `freerun` / `lockstep` | `freerun` | Schritt-für-Schritt-Synchronisation der 3 Controller | > **Konsistenz-Regel:** `ROBOT_MOTION_SYNC=lockstep` ergibt nur mit `ROBOT_SPEED_MODE=correct` > Sinn (sonst kommen die Controller zu unterschiedlichen Zeiten an und Lockstep müsste hart > warten). Beim Start einmal prüfen und ggf. warnen. --- ## Paket 1: GRBL-Antworten lesen - [ ] `connection.on('data', data => {})` in `TelnetSenderGRBL` ersetzen durch echtes Lesen - GRBL antwortet auf jeden G-Code-Befehl mit `ok` oder `error: ` - Antworten parsen und ins Log schreiben - [ ] Fehlerantworten nach außen meldbar machen - an `InfoServer` oder über einen EventEmitter - damit der WebSocket-Client Feedback bekommt, ob ein Befehl angenommen wurde ## Paket 2: Command-Queue mit ok-Handshake > **Verhältnis zu Paket 6:** Dies ist die *einfache, synchrone* Variante (ein Befehl pro `ok`). > Die ausgebaute, `Bf`-basierte und abschaltbare Queue steht in **Paket 6** und löst dasselbe > Problem flüssiger. Paket 2 kann als Zwischenschritt dienen oder direkt in Paket 6 aufgehen. - [ ] Sendepuffer einführen: Befehle erst abschicken, wenn das vorherige `ok` eingegangen ist - GRBL hat intern ~128 Byte Puffer — bei schnellen Befehlsfolgen (Datei abspielen) droht sonst Puffer-Überlauf und stille Befehlsverwerfung - Alternative: GRBL Line-Counting-Protokoll (sendet mehrere Befehle, zählt Zeichen im Puffer) → in Paket 6 über die zweite `Bf`-Zahl (freie RX-Bytes) abgedeckt - [ ] Timeout für ausbleibende `ok`-Antworten definieren - nach X ms ohne Antwort: Fehler loggen, ggf. Verbindung zurücksetzen ## Paket 3: Hardwareposition lesen (Auto-Report statt Polling) - [ ] Beim Verbindungsaufbau je Controller konfigurieren: - `$10=3` setzen → Report enthält `MPos` **und** `Bf` (Protokoll-Fakt 2) - `$Report/Interval=N` setzen (z. B. `N=100…200`) → FluidNC **pusht** den Status während der Bewegung selbst (Protokoll-Fakt 4). Kein `?`-Polling-Loop nötig; `?` bleibt nur als Einzelabfrage on demand (z. B. für Sync, Paket 4). - [ ] `data`-Handler (Paket 1) parst die gepushten `<…>`-Reports: `state`, `MPos`, `Bf` - robust gegen Cross-Channel-Fremdzeilen (Protokoll-Fakt 5) — nach Typ demultiplexen - [ ] Gemeldete Hardware-Position (`MPos`) mit Softwareposition vergleichen - bei Abweichung: warnen oder synchronisieren (→ Paket 4) - schützt gegen Drift durch Endschalter-Auslösung, Motor-Stall, Verbindungsunterbrechung - [ ] Status (`Idle`, `Run`, `Alarm`, `Hold`) + `Bf` für den `InfoServer` bereitstellen - `/api/status` um GRBL-Zustand erweitern --- ## Baustein für Paket 4 + 5: Rückabbildung Port → Motorwerte Beide folgenden Pakete brauchen denselben Baustein: aus den von GRBL gemeldeten `MPos`-Werten der drei Controller die **sieben Motorwerte des Roboters** rekonstruieren (`xMotor, alpha, beta, a, b, c, eMotor`). Das ist die **Umkehrung von `portValue()`** (`robot/TelnetSenderGRBL.js`). `portValue()` bildet *eine Roboter-Achse → einen GRBL-Port-Wert* ab, dabei koppeln einige Ports mehrere Achsen (z. B. der z-Port der Hand mischt `c, b, z, y`). Die Rückrichtung muss diese Kopplung **explizit auflösen** — sie ergibt sich nicht automatisch. - [ ] `motorStateFromPorts(portReadings)` definieren — algebraische Umkehrung von `portValue()` - Eingang: pro Sender die gelesenen Port-Werte (`{x, y, z}` Base, `{a}` Elbow, `{c, e, b}` Hand) - Ausgang: `{xMotor, alpha, beta, a, b, c, eMotor}` - Grad→Rad zurückrechnen, `factorTurnLift`/`handOpenInMM` herausrechnen, gekoppelte Ports auflösen - **Zweig-Wahl (B3):** wo die Lösung mehrdeutig ist, die dokumentierte physikalische Zusatzbedingung anwenden (z. B. „Ellbogen höher als Hand"). Konvention im Code festhalten. - [ ] **Round-Trip-Invariante** als Test: `portValue(motorStateFromPorts(p)) ≈ p` - dasselbe Muster wie `test/Robot.Kinematics.RoundTrip.test.js` - schützt die Umkehrfunktion gegen Drift gegenüber `portValue()` > Hinweis: Gelesen wird auf dem **aktiven** Sender `TelnetSenderGRBL` (im `data`-Handler, > siehe Paket 1) — nicht auf `FluidNCClient.js`. --- ## Paket 4: Hardware-Position auslesen und übernehmen (Sync-Command) **Ziel:** Ein Befehl liest die echten Motor-Koordinaten aller drei Controller aus, übernimmt sie als neuen Soll-Zustand und passt die berechnete Roboter-Pose entsprechend an. Nötig nach Homing, manuellem Jog, Endschalter-Auslösung oder Reconnect — die Software weiß sonst nicht, wo der Roboter physisch wirklich steht. - [ ] **G-Code-Befehl** (B6), z. B. `M114 R` (Read-Hardware) — durch `GCodeParser` + `RobotController` geroutet, **nicht** als Sonderfall in `InputWS` wie heute `M114` - klar abgegrenzt vom bestehenden `M114`, das nur die **Software**-Position zurückgibt (`GCode.getM114(robot)` in `server/InputWS.js`) - [ ] **Async-Dispatch (B6-Folge):** `RobotController.applyCommand` muss für diesen Befehl einen Promise zurückgeben und auf die `?`-Antworten warten — der erste asynchrone Befehl im bisher synchronen Dispatch-Pfad. - [ ] Ablauf des Sync: 1. an alle drei Sender einmalig `?` senden, je `MPos` aus der Antwort parsen (Paket 3) 2. `motorStateFromPorts(...)` → sieben Motorwerte rekonstruieren (Baustein oben, inkl. B3-Zweigwahl) 3. diese auf den Roboter schreiben: `robot.xMotor/alpha/beta/a/b/c/eMotor = …` 4. **Vorwärtskinematik** anstoßen: `robot.calculatePositionFromMotorAngles()` → füllt `robot.x/y/z` und `phi/theta/psi` aus den Hardwarewerten 5. `motorPosition`/`motorPositionOld` zurücksetzen, damit der nächste Move sauber von der echten Position aus rechnet (sonst falscher Speed-Delta im Korrekt-Modus) - [ ] dem anfragenden Client die übernommene Pose zurückmelden (`reply(ws, …)`) - [ ] **kein** automatisches Nachfahren — Sync ändert nur den Soll-Zustand, sendet keinen Move > Warum nicht einfach „letzten gesendeten Wert" merken? Weil die Hardware nach Homing/Jog/ > Stall von dem abweicht, was zuletzt gesendet wurde — genau diese Differenz soll Sync auflösen. --- ## Paket 5: Bewegungs-Fortschritt ermitteln (Move-Progress) **Ziel:** Herausfinden, wie weit FluidNC einen laufenden Move bereits abgearbeitet hat — also wie weit sich jeder Controller schon zur Ziel-Position bewegt hat (0…100 %). - [ ] Beim Absenden eines Moves Start und Ziel je Controller festhalten - `mStart` = Port-Werte vor dem Move, `mTarget` = gesendete Port-Werte - liegt bereits vor: `robot.motorPositionOld` (Start) und `robot.motorPosition` (Ziel), über `portValue()` in Port-Werte umgerechnet - [ ] Aus den **auto-gepushten** Reports (Paket 3, `$Report/Interval`) je Controller berechnen — **kein** eigenes Polling: ``` fortschritt_i = |MPos_jetzt − Start_i| / |Ziel_i − Start_i| (auf 0…1 geklemmt) ``` - [ ] Aggregat-Fortschritt + Fertig-Erkennung: - **Fertig**, wenn alle drei Controller `state == Idle` UND `MPos ≈ Ziel` - der Gesamt-Fortschritt eines Schritts = **Minimum** über die Controller (der langsamste bestimmt, wann der Schritt fertig ist) - im **Korrekt-Modus** (ToDo_6a) sollten alle Controller etwa gleich schnell fertig sein — eine große Spreizung der Einzel-Fortschritte ist dort ein Warnsignal (Feedrate-Fehler) - [ ] **Lockstep-Gate (B5, nur bei `ROBOT_MOTION_SYNC=lockstep`):** den nächsten Schritt erst freigeben, wenn der langsamste Controller `Idle`/Ziel erreicht hat. In `freerun` entfällt das Gate — die Koordination kommt allein aus der Feedrate (ToDo_6a `correct`). - [ ] Fortschritt + Status nach außen geben - über `InfoServer` (`/api/status`) und/oder als WS-Push an die Clients - ermöglicht eine Fortschrittsanzeige beim Abspielen von Dateien (ToDo_6b) ### Offener Kernpunkt: Was, wenn währenddessen der nächste Befehl kommt? Heute ist der Treiber **fire-and-forget**: ein neuer Befehl wird sofort an alle drei GRBLs geschickt und landet in deren Planner-Puffer. Das hat zwei Folgen, die Paket 5 für sich genommen nicht löst: 1. **Fortschritt wird mehrdeutig.** `motorPositionOld`/`motorPosition` halten nur die *letzten zwei* Stellungen. Sind mehrere Moves gleichzeitig im GRBL-Puffer, weiß der Treiber nicht mehr, *welcher* gerade fährt — der `?`-Fortschritt bezieht sich dann auf das falsche Start/Ziel-Paar. 2. **Stille Befehlsverwerfung.** Kommen Befehle dauerhaft schneller als sie ausgeführt werden, läuft GRBLs 128-Byte-RX-Puffer über → Befehle gehen verloren (vgl. Paket 2). Die saubere Lösung ist eine **zeitgesteuerte Sende-Queue** — bewusst als größerer, **abschaltbarer** Umbau in Paket 6. --- ## Paket 6: Zeitgesteuerte Sende-Queue (`ROBOT_USE_QUEUE`) **Ziel:** Befehle nicht mehr blind sofort raushauen, sondern in einer Queue puffern und **zeitlich getaktet** absenden — jeden Befehl gerade rechtzeitig, bevor der vorige fertig ist. So bleibt die Bewegung flüssig (GRBL-Planner läuft nie leer), nichts geht verloren (kein Puffer-Überlauf), und das Netz wird nicht mit Dauer-Polling oder Befehls-Salven belastet. ### Schalter `ROBOT_USE_QUEUE` (siehe zentrale Schalter-Übersicht oben). `false` = **exakt heutiges Fire-and-forget**, byte-identisch zu vorher; `true` = Queue-Logik aktiv. Wie `ROBOT_SPEED_MODE` greift der Umbau **nur** bei `true` — das bestehende Sender-Test-Sicherheitsnetz bleibt gültig. ```yaml # docker-compose.yaml → appRobotDriver environment: - ROBOT_USE_QUEUE=false # oder: true ``` ### Takt: `Bf` ist die Wahrheit, `moveTime` nur der Vorhersage-Hint Die Recherche ändert den ursprünglichen „reine Zeitschätzung"-Entwurf: FluidNC liefert die **echte** Pufferfüllung (`Bf`) ohnehin frei Haus über Auto-Reporting (Protokoll-Fakt 3+4). Damit muss nichts mehr aufwändig geschätzt werden. - **`Bf`-basierter Haupttakt (echte Wahrheit, kein Extra-Traffic):** Aus den auto-gepushten Reports (Paket 3) kennt der Treiber je Controller die freien Planner-Blöcke. Regel: ``` solange (freie Planner-Blöcke > schwelle) → nächsten Queue-Eintrag senden ``` Hält den Planner gefüllt (flüssige Bewegung) und kann **nie** überlaufen — das ist das Standard-GRBL-Streaming, nur mit gepushtem statt gepolltem Status. - **`moveTime` als prädiktiver Zusatz (aus ToDo_6a):** zwischen zwei Reports überbrückt die geschätzte Ausführzeit die Lücke (z. B. um zu entscheiden, ob *jetzt schon* der nächste Eintrag sinnvoll ist). Korrigiert wird die Schätzung laufend durch den nächsten `Bf`-Push; sie ist nie die alleinige Quelle. ### Eintrag in der Queue - [ ] Queue-Eintrag hält: geparster Befehl / Motorziel, `moveTime` (Hint), die je Sender resultierenden G-Code-Strings, Status (`pending → sent → done`) - [ ] `done` wird aus dem `Bf`/`state`-Report abgeleitet (`Idle` am Ziel), nicht geraten ### Pacing-Schleife - [ ] je Controller die freien Planner-Blöcke aus dem letzten Report führen; senden, solange über Schwelle — In-Flight-Tiefe damit implizit begrenzt (flüssig, aber steuerbar) - [ ] `moveTime` nur zur Überbrückung zwischen Reports nutzen - [ ] **Sicherheitsboden:** zusätzlich freie RX-Bytes aus `Bf` beachten (zweite Zahl) — nie mehr senden, als in den RX-Puffer passt (Character-Counting fällt damit faktisch ab) ### Verhalten bei „neuer Befehl kommt mitten in der Bewegung" Zwei Betriebsarten — je nach Quelle der Befehle: - [ ] **Anhängen (Datei abspielen / ToDo_6b):** Reihenfolge erhalten, nach Schätzung takten, In-Flight-Tiefe begrenzen. Kein Befehl wird verworfen. - [ ] **Neuester gewinnt (interaktives Streaming, z. B. 3D-Input / Bodytracker):** Trifft ein neues Ziel ein, während noch ungesendete (`pending`) Einträge in der Queue liegen, den veralteten Schwanz **ersetzen** statt anzuhängen (Coalescing) → niedrige Latenz, kein Auflaufen veralteter Zwischenziele. (Kann als Unter-Schalter / spätere Ausbaustufe kommen.) - [ ] **Backpressure:** maximale Queue-Länge festlegen. Bei Dauer-Überlauf entweder Ältestes-verwerfen (Streaming) oder Druck an den WS-Client zurückgeben (Datei) — nie unbegrenzt wachsen lassen. ### Fortschritt mit Pipelining (behebt die Mehrdeutigkeit aus Paket 5) - [ ] Den „aktuell fahrenden" Eintrag in der Queue markieren (Kopf vorrücken, wenn geschätzte Uhr abgelaufen **oder** `?`=`Idle`). Erst dieser Kopf liefert das richtige Start/Ziel-Paar für die `?`-Fortschrittsrechnung aus Paket 5. ### Kanten / Sonderfälle - [ ] **Alarm/Error** mitten in der Queue (Paket 1) → Queue leeren, Senden stoppen, Fehler melden - [ ] **Sync-Command (Paket 4)** bei nicht-leerer Queue → erst Queue leeren; die gepufferten Ziele sind nach einem Re-Homing veraltet - [ ] **Lockstep vs. Freerun (B5):** In `freerun` taktet jeder Controller eigenständig nach seinem `Bf`. In `lockstep` kommt zusätzlich das Gate aus Paket 5 dazu — der nächste Schritt wird erst gesendet, wenn der *langsamste* Controller `Idle`/Ziel meldet. - [ ] **Drei Controller driften auseinander:** Im Korrekt-Modus (ToDo_6a) sollten die `Bf`-Stände zusammenbleiben; große Spreizung ist ein Warnsignal (Feedrate-Fehler) — in `lockstep` zudem ein Auslöser, härter zu warten. - [ ] **`moveTime` nur Hint:** Da `Bf` die echte Wahrheit liefert, ist die Schätzungenauigkeit (`dist/feedrate` ignoriert Beschleunigung) unkritisch — sie überbrückt nur die Lücke zwischen zwei Reports. Keine Trapez-Profil-Verfeinerung nötig. --- ## Hinweis zur Implementierung - **Aktiver Pfad:** Gelesen und konfiguriert (`$10`, `$Report/Interval`) wird auf dem produktiv genutzten `robot/TelnetSenderGRBL.js` (Telnet-Kanal) — der `data`-Handler dort ist die zentrale Stelle (Paket 1). - `robot/fluidnc/FluidNCClient.js` (bidirektionale WebSocket-Anbindung, Port 81, Reconnect + `EventEmitter`) ist eine *alternative* Anbindung. Nicht der aktive Pfad, kann aber bei `ToDo_2` (Sender-Interface) als Referenz für das Event-Modell mit evaluiert werden. ## Reihenfolge / Abhängigkeiten 1. **Paket 1** (lesen) ist die Basis für alles Weitere. 2. **Paket 3** (Auto-Report `$10=3` + `$Report/Interval`, `MPos`/`Bf` parsen) liefert die Daten für Paket 4, 5, 6. 3. **Baustein** (Umkehr-Kinematik inkl. B3-Zweigwahl) → dann **Paket 4** (Sync, async). 4. **Paket 5** (Fortschritt) und **Paket 6** (Queue) bauen auf Paket 3 auf; **Freerun zuerst**, **Lockstep** (B5) erst danach.