# 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 => {})
```

---

## 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: <Meldung>`
  - 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

- [ ] 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)
- [ ] Timeout für ausbleibende `ok`-Antworten definieren
  - nach X ms ohne Antwort: Fehler loggen, ggf. Verbindung zurücksetzen

## Paket 3: Hardwareposition abfragen (`?`-Status)

- [ ] Periodisch GRBL-Statusabfrage senden: `?`
  - GRBL antwortet mit `<Idle|MPos:0.000,0.000,0.000|WPos:0.000,0.000,0.000>`
  - Alternative: nach jedem abgeschlossenen Move abfragen
- [ ] Gemeldete Hardware-Position mit Softwareposition (`robot.x/y/z`) vergleichen
  - bei Abweichung: warnen oder synchronisieren
  - schützt gegen Drift durch Endschalter-Auslösung, Motor-Stall, Verbindungsunterbrechung
- [ ] Status (`Idle`, `Run`, `Alarm`, `Hold`) 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
- [ ] **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.

- [ ] Neuer Eingabe-Befehl, z. B. `M114 R` (Read-Hardware) oder WS-Message `syncFromHardware`
  - klar abgegrenzt vom bestehenden `M114`, das nur die **Software**-Position zurückgibt
    (`GCode.getM114(robot)` in `server/InputWS.js`)
- [ ] Ablauf des Sync:
  1. an alle drei Sender `?` senden, je `MPos` aus der Antwort parsen (Paket 3)
  2. `motorStateFromPorts(...)` → sieben Motorwerte rekonstruieren (Baustein oben)
  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
- [ ] Während der Bewegung periodisch `?` pollen (Paket 3) und je Controller berechnen:
  ```
  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)
- [ ] 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)
- [ ] Zusammenspiel mit der Command-Queue (Paket 2): erst den nächsten Move senden, wenn
  der vorige `Idle` erreicht hat → verhindert, dass Fortschritt mehrerer Moves verschwimmt

### 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 (Pflicht — Absicherung wie bei ToDo_6a)

| Env | Default | Wirkung |
|---|---|---|
| `ROBOT_USE_QUEUE` | `false` | **Aus = exakt heutiges Fire-and-forget.** Jeder Befehl geht sofort an alle Sender, kein Pacing, keine Queue. Byte-identisch zu vorher. |
| `ROBOT_USE_QUEUE` | `true` | Neue zeitgesteuerte Queue-Logik aktiv. |

```yaml
# docker-compose.yaml → appRobotDriver
environment:
  - ROBOT_USE_QUEUE=false   # oder: true
```

> Wie `ROBOT_SPEED_MODE` greift der Umbau **nur** bei `true`. Solange `false`, ist der
> Sende-Pfad unverändert — das bestehende Sicherheitsnetz (Sender-Tests) bleibt gültig.

### Idee: zwei Uhren — eine geschätzte (gratis), eine gemessene (kostet Netz)

Der Trick, das Netz **nicht** zu überlasten: primär nach einer **geschätzten** Uhr takten,
die `?`-Messung nur sparsam zur Korrektur einsetzen.

- **Geschätzte Uhr (Haupttakt, kein Netzverkehr):** Jeder Queue-Eintrag trägt seine
  **voraussichtliche Ausführzeit** — die liegt mit `moveTime` aus ToDo_6a bereits vor.
  Der Treiber führt je Controller einen lokalen Zeitstempel `controllerFreiAb`:
  ```
  beim Senden:  controllerFreiAb += moveTime_dieses_Befehls
  nächster Send, wenn:  jetzt >= controllerFreiAb − vorlauf
  ```
  `vorlauf` = kleine Sicherheitsmarge, damit immer ~1 Move im GRBL-Planner wartet und die
  Bewegung nicht stockt. Das ist ein reiner Timer → **null Zusatz-Traffic**.

- **Gemessene Uhr (Korrektur, sparsam):** Die Schätzung driftet (Beschleunigung/Abbremsen
  stecken nicht in `dist/feedrate`, kurze Moves dauern real länger). Deshalb ab und zu —
  **nicht** bei jedem Move — per `?` (Paket 3) den echten Stand holen und `controllerFreiAb`
  nachjustieren. Auslöser: Queue läuft fast leer, ein langer Move (einmal mittendrin prüfen),
  oder ein fester Maximaltakt. **`?` strikt raten-begrenzen** (z. B. ≤ 5 Hz, GRBL-üblich) →
  Netzlast bleibt gedeckelt.

### Eintrag in der Queue

- [ ] Queue-Eintrag hält: geparster Befehl / Motorziel, `moveTime` (geschätzt), die je
  Sender resultierenden G-Code-Strings, Status (`pending → sent → done`)
- [ ] `done` wird gesetzt durch geschätzte Uhr **oder** (falls gepollt) durch `?`=`Idle` am Ziel

### Pacing-Schleife

- [ ] je Controller `controllerFreiAb` führen, Sendezeitpunkt aus `moveTime − vorlauf` ableiten
- [ ] Tiefe im GRBL-Planner begrenzen (z. B. ≤ 1–2 vorausgesendete Moves) — flüssig, aber
  noch steuerbar
- [ ] Drift-Korrektur per ratenbegrenztem `?`; bei großer Abweichung Schätzung neu setzen
- [ ] **Optionaler Sicherheitsboden:** zusätzlich Character-Counting (Summe ungequittierter
  Zeilenlängen < 128 B) als Netz gegen Puffer-Überlauf, falls die Schätzung mal stark danebenliegt

### 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
- [ ] **Drei Controller driften auseinander:** Im Korrekt-Modus (ToDo_6a) laufen sie auf
  dieselbe `moveTime` → ihre `controllerFreiAb` sollten zusammenbleiben; große Spreizung ist
  ein Warnsignal (Feedrate-/Schätzfehler)
- [ ] **Schätzgüte:** `moveTime = dist/feedrate` ignoriert Beschleunigung; kurze Moves dauern
  real länger. Vorlauf-Marge muss das abfangen; später ggf. Trapez-Profil-Schätzung verfeinern

---

## Hinweis zur Implementierung

`robot/fluidnc/FluidNCClient.js` ist eine bidirektionale WebSocket-Anbindung an FluidNC (Port 81) mit Reconnect-Logik und `EventEmitter`-Interface — diese Klasse ist eine gute Grundlage für alle drei Pakete und sollte bei der Umsetzung von `ToDo_2` (Sender-Interface) mit evaluiert werden.