238 lines
13 KiB
Markdown
238 lines
13 KiB
Markdown
# 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.
|