Files

chk d906b094b1 Umbau 12: Robot-Kinematics als extends RobotBase

2026-06-10 23:18:29 +02:00

13 KiB

Raw Blame History

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:

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.
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.

# 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.

13 KiB Raw Blame History Unescape Escape