diff --git a/README.md b/README.md index ad2630f..2639dc2 100644 --- a/README.md +++ b/README.md @@ -76,9 +76,18 @@ Die Eingaben kommen per WebSocket an den HTTPS-Server und werden in `server/Inpu - `ROBOT_DEFAULT_FEEDRATE` - Standard: `1000` (mm/min) - Default-Feedrate für `G1`-Befehle, wenn keine `F`-Angabe vorhanden ist +- `ROBOT_SPEED_MODE` + - Werte: `legacy` (Standard) oder `correct` + - `legacy`: alte Speed-Regelung — jeder Sender erhält die kartesische Feedrate `F` + unverändert (Verhalten exakt wie bisher). + - `correct`: koordinierte Feedrate pro Sender, sodass alle Controller den Bewegungs­schritt + gleichzeitig beenden. Aktiviert automatisch `calculateSpeeds()`. + - Details: `doc/ToDo_6a_Speed.md` - `ROBOT_USE_SPEED_CALC` - Werte: `true`, `1` oder sonst leer - - Wenn gesetzt, werden `robot.calculateSpeeds()` und interne Motor-Geschwindigkeiten verwendet + - Interner Schalter, ob `robot.calculateSpeeds()` rechnet. Ändert allein **nicht** die + Sender-Ausgabe — dafür ist `ROBOT_SPEED_MODE=correct` nötig. Vom Korrekt-Modus + automatisch aktiviert. ### HTTPS-Konfiguration @@ -120,8 +129,10 @@ Die Achszuordnung kann in `robot/TelnetSenderGRBL.js` durch Anpassung der Konstr - `startRobot.js` - `server/InputWS.js` - `server/InfoServer.js` -- `robot/Robot.js` -- `robot/GCode.js` +- `robot/Robot.js` — Modell + Kinematik +- `robot/GCodeParser.js` — wandelt rohe Nachrichten in strukturierte Befehlsobjekte +- `robot/RobotController.js` — wendet geparste Befehle auf das Modell an (Steuerlogik) +- `robot/GCode.js` — Fassade + Datei-Befehle - `robot/TelnetSenderGRBL.js` - `robot/fluidnc/FluidNCClient.js` — alternative WebSocket-basierte FluidNC-Anbindung mit Reconnect-Logik (noch nicht integriert) - `GCodeFiles/` — enthalten Beispiel- und Log-G-Code-Dateien @@ -136,22 +147,22 @@ Die Achszuordnung kann in `robot/TelnetSenderGRBL.js` durch Anpassung der Konstr Architektur- und Refactoring-Aufgaben sind in `doc/ToDo_*.md` dokumentiert: -| Datei | Thema | -|---|---| -| `doc/ToDo_1_Parsing.md` | G-Code-Parser-Schicht einführen | -| `doc/ToDo_2_Anbindung.md` | Sender-Interface und Orchestrierung | -| `doc/ToDo_3_Config.md` | Zentralisierte Konfiguration | -| `doc/ToDo_4_GCode.md` | G-Code- und Datei-Handling trennen | -| `doc/ToDo_5_API.md` | WebSocket-Antwortlogik strukturieren | -| `doc/ToDo_6_RobotController.md` | RobotController-Klasse einführen | -| `doc/ToDo_6a_Speed.md` | Speed-Steuerung: `calculateSpeeds()` reparieren, per-Achse Feedrate | -| `doc/ToDo_6b_FileHandling.md` | File-Handling: fehlende Befehle, Cursor im Speicher, Fehler-Feedback | -| `doc/ToDo_7_Tests.md` | Testabdeckung und Stabilität | -| `doc/ToDo_8_Bugs.md` | Bekannte konkrete Bugs | -| `doc/ToDo_9_HardwareFeedback.md` | Hardware-Feedback-Loop (GRBL-Antworten, Command-Queue, Positionsabgleich) | -| `doc/ToDo_10_VerbindungsVerlust.md` | Verbindungsverlust erkennen, Watchdog, UI-Statusanzeige | -| `doc/ToDo_12_InverseKinematikConfig_ROADMAP.md` | Austauschbare Kinematik: RobotBase + Robot7M, Env-Konfiguration | -| `doc/ToDo_49_Cleanup.md` | Pre-Release-Cleanup: tote Code, Zertifikate, ToDos, README | +| Datei | Thema | Status | +|---|---|---| +| `doc/ToDo_1_Parsing.md` | G-Code-Parser-Schicht einführen | ✅ erledigt | +| `doc/ToDo_2_Anbindung.md` | Sender-Interface und Orchestrierung | ✅ erledigt | +| `doc/ToDo_3_Config.md` | Zentralisierte Konfiguration | offen | +| `doc/ToDo_4_GCode.md` | G-Code- und Datei-Handling trennen | offen | +| `doc/ToDo_5_API.md` | WebSocket-Antwortlogik strukturieren | ✅ erledigt | +| `doc/ToDo_6_RobotController.md` | RobotController-Klasse einführen | ✅ erledigt | +| `doc/ToDo_6a_Speed.md` | Speed-Steuerung: Schalter, `calculateSpeeds()`-Fix, koordinierte Feedrate | ✅ erledigt (WS-Sender offen) | +| `doc/ToDo_6b_FileHandling.md` | File-Handling: fehlende Befehle, Cursor im Speicher, Fehler-Feedback | offen | +| `doc/ToDo_7_Tests.md` | Testabdeckung und Stabilität | teilweise | +| `doc/ToDo_8_Bugs.md` | Bekannte konkrete Bugs | teilweise | +| `doc/ToDo_9_HardwareFeedback.md` | Hardware-Feedback-Loop (GRBL-Antworten, Command-Queue, Positionsabgleich) | offen | +| `doc/ToDo_10_VerbindungsVerlust.md` | Verbindungsverlust erkennen, Watchdog, UI-Statusanzeige | offen | +| `doc/ToDo_12_InverseKinematikConfig_ROADMAP.md` | Austauschbare Kinematik: RobotBase + Robot7M, Env-Konfiguration | offen | +| `doc/ToDo_49_Cleanup.md` | Pre-Release-Cleanup: tote Code, Zertifikate, ToDos, README | offen | ### Empfohlene Bearbeitungsreihenfolge diff --git a/doc/ToDo_6_RobotController.md b/doc/ToDo_6_RobotController.md index de452b9..fb21e67 100644 --- a/doc/ToDo_6_RobotController.md +++ b/doc/ToDo_6_RobotController.md @@ -1,17 +1,35 @@ # ToDo 6 — RobotController +> ## ✅ Status: erledigt +> +> Steuerlogik in `robot/RobotController.js` ausgelagert; `GCode.receiveGCode()` ist +> nur noch Fassade. Tests: `test/RobotController.test.js` (+ alle bestehenden +> `GCode.*`-Tests laufen unverändert über die Fassade). + ## Ziel der Verbesserung Der `RobotController` fasst die Steuerlogik zusammen und hält den Roboterzustand vom G-Code-Parsing getrennt. So wird die Architektur modularer und leichter wartbar. ## Aufgaben -- [ ] `RobotController` einführen - - verarbeitet strukturierte G-Code-/Befehlsobjekte - - steuert `moveRelative`, `calculateAngles3D()`, `calculateSpeeds()` und `sendCommand()` -- [ ] `Robot.js` als reines Modell-/Kinematik-Modul nutzen +- [x] `RobotController` einführen (`robot/RobotController.js`) + - verarbeitet strukturierte G-Code-/Befehlsobjekte (`applyCommand`, `receive`) + - steuert `moveRelative`, `calculateAngles3D()` und `sendCommand()` +- [x] `Robot.js` als reines Modell-/Kinematik-Modul nutzen - Zustand, Längen, Winkel, Kinematik-Berechnungen -- [ ] den Übergang von Befehlsobjekt zu Motorpositionen sauber definieren -- [ ] Logik für Bewegungsbefehle zentralisieren: `G1`, `G28`, `G90`, `G91`, `G92`, `M1` -- [ ] rohe Textstrings aus der Controller-Schicht entfernen -- [ ] Zustandsänderungen konsistent an die Sender weiterreichen +- [x] den Übergang von Befehlsobjekt zu Motorpositionen sauber definieren +- [x] Logik für Bewegungsbefehle zentralisieren: `G1`, `G28`, `G90`, `G91`, `G92`(=`M92`), `M1` +- [x] rohe Textstrings aus der Controller-Schicht entfernen (Controller arbeitet nur auf geparsten Objekten) +- [x] Zustandsänderungen konsistent an die Sender weiterreichen (über `robot.sendCommand()`) + +## Status + +- Erledigt. Die Dispatch-Logik wurde aus `GCode.receiveGCode()` in `RobotController` + ausgelagert. `GCode.receiveGCode()` bleibt als dünne Fassade erhalten (delegiert an + `RobotController.receive()`), damit bestehende Aufrufer und Datei-Befehle unverändert + funktionieren. +- `calculateSpeeds()` wird bewusst weiterhin innerhalb von `robot.sendCommand()` ausgelöst + (Geschwindigkeit ist eine Modell-Eigenschaft, die der Controller über `sendCommand` + anstößt — siehe ToDo_6a). +- Tests: `test/RobotController.test.js` (Controller direkt), alle bestehenden + `GCode.*`-Tests laufen unverändert über die Fassade. diff --git a/doc/ToDo_6a_Speed.md b/doc/ToDo_6a_Speed.md index 4c67ada..cafabee 100644 --- a/doc/ToDo_6a_Speed.md +++ b/doc/ToDo_6a_Speed.md @@ -1,114 +1,129 @@ # ToDo 6a — Speed-Steuerung -## Ist-Zustand und Defizite +> ## ✅ Status: erledigt (Pakete 1–2) +> +> - **Schalter `ROBOT_SPEED_MODE`** (`legacy` Default = exakt wie bisher, `correct` = koordiniert) +> - **`calculateSpeeds()`-NaN-Bug behoben** + `moveTime` eingeführt +> - **Koordinierte Feedrate** im `TelnetSenderGRBL` (Korrekt-Modus), Legacy-Pfad unangetastet +> - Tests: `test/Robot.calculateSpeeds.test.js`, `test/Sender.Telnet.speedMode.test.js`, +> `test/Speed.coordination.test.js` +> +> **Offen:** WS-Sender (Paket 3), `FPoint`-Feedrate (Paket 4 → ToDo_6b). -Die Feedrate-Logik ist auf zwei Ebenen defekt: +## Der Schalter: `ROBOT_SPEED_MODE` -### Ebene 1: `calculateSpeeds()` berechnet NaN +Die Speed-Regelung ist über **eine einzige Umgebungsvariable** umschaltbar: -`Robot.calculateSpeeds(oldPos, newPos)` liest `oldPos.xMotor`, `oldPos.alpha`, `oldPos.beta` — -diese Properties existieren in `RobotMotorPosition` **nicht**. Dort heißen sie `x`, `y`, `z`. -Ergebnis: alle `motorSpeeds`-Werte sind `NaN`. Die Methode ist seit ihrer Einführung kaputt -und fällt nur nicht auf, weil `ROBOT_USE_SPEED_CALC` standardmäßig `false` ist. +| Wert | Bedeutung | +|---|---| +| `legacy` (Default) | **Alte Speed-Regelung — alles läuft exakt wie bisher.** Jeder Sender sendet die kartesische Feedrate `F` (aus dem G-Code bzw. `robot.feedrate`) unverändert an alle seine Achsen. | +| `correct` | **Korrekte Speed-Regelung.** Jeder Sender erhält eine eigene, koordinierte Feedrate, sodass alle Controller die Bewegung gleichzeitig beenden. | -```js -// Zeile ~214 Robot.js — falsch: -this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time; // oldPos.xMotor → undefined -this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time; // oldPos.alpha → undefined -this.motorSpeeds.z = (this.beta - oldPos.beta) / time; // oldPos.beta → undefined - -// Richtig (RobotMotorPosition-Felder): -this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; -this.motorSpeeds.y = (this.alpha - oldPos.y) / time; -this.motorSpeeds.z = (this.beta - oldPos.z) / time; +```yaml +# docker-compose.yml +environment: + ROBOT_SPEED_MODE: legacy # oder: correct ``` -### Ebene 2: `motorSpeeds` werden von keinem Sender gelesen +### Garantie für `legacy` -Selbst wenn `calculateSpeeds()` korrekte Werte lieferte, ignorieren beide Sender die -berechneten Geschwindigkeiten vollständig: +Im Legacy-Modus wird der **Sende-Pfad der Sender nicht angefasst**. Die Feedrate-Zeile +entsteht über exakt denselben Code wie bisher. Die bestehenden Sender-Tests +(`Sender.Telnet.test.js`, `Sender.Telnet.caseBackward.test.js`) prüfen die Ausgabe +zeichengenau und sind das Sicherheitsnetz: solange sie grün sind, ist Legacy +byte-identisch zu vorher. -- **`TelnetSenderGRBL`**: sendet `mNew.feedrate` (Kartesisch, mm/min) an alle Achsen gleich -- **`WSSenderGrbl`**: sendet immer `this.maxSpeedF`, ignoriert `mNew.feedrate` komplett +Der Korrekt-Modus fügt **nur zusätzlich** eine alternative Feedrate-Berechnung hinzu, +die ausschließlich greift, wenn `ROBOT_SPEED_MODE=correct`. -Das ist grundsätzlich falsch: Der Roboter hat drei unabhängige GRBL-Controller. Damit die -Fingerspitze mit `F1000 mm/min` fährt, muss jede Achse mit einer **anderen** Winkelgeschwindigkeit -drehen — je nachdem, wie weit sie sich für diesen Schritt bewegt. Alle Achsen mit derselben -Feedrate zu befehlen führt zu nicht-linearen Werkzeugbahnen. +### Abgrenzung zu `ROBOT_USE_SPEED_CALC` -### Ebene 3: `FPoint` kodiert Feedrate hart - -```js -// GCode.js FPoint — immer f1000, egal was robot.feedrate ist: -var strGCode = `G90 G1 x${robot.x} ... f1000` -``` +`ROBOT_USE_SPEED_CALC` bleibt der interne Schalter dafür, ob `Robot.calculateSpeeds()` +überhaupt rechnet. Der Korrekt-Modus aktiviert diese Berechnung automatisch. +`ROBOT_USE_SPEED_CALC` allein ändert **nicht** die Sender-Ausgabe — nur +`ROBOT_SPEED_MODE=correct` tut das. Damit bleibt bestehendes Verhalten erhalten. --- -## Konzept: Korrekte Feedrate-Verteilung +## Hintergrund: Warum die alte Regelung falsch ist -Gesamtziel: Die Kartesische Feedrate `F` (mm/min) des G-Code-Befehls bestimmt, wie lange -der Bewegungsschritt dauert. Diese Zeit wird auf alle Achsen aufgeteilt. +Der Roboter hat drei unabhängige GRBL/FluidNC-Controller. Im Legacy-Modus bekommen alle +dieselbe kartesische Feedrate (z. B. `f1000`) — unabhängig davon, wie weit sich die +jeweilige Achse in diesem Schritt tatsächlich bewegt. Folge: die Achse mit kurzer +Bewegung ist viel früher fertig als die mit langer Bewegung, die Bewegungen laufen nicht +koordiniert, und die Werkzeugbahn der Fingerspitze ist nicht linear. + +**Korrekt:** Die kartesische Feedrate `F` bestimmt die Gesamt-Zeit des Schritts: ``` -Zeit = kartesische_Distanz / F_mm_per_min +Bewegungszeit = kartesische_Distanz / F -Achsen-Feedrate[i] = Achsen-Delta[i] / Zeit - (in Grad/min, da GRBL-Achsen typisch in Grad konfiguriert) +Sender-Feedrate = Distanz_dieses_Senders / Bewegungszeit ``` -Der Sender kennt seine Achse (`xAxisGrbl`, `yAxisGrbl`, `zAxisGrbl`) und kann die passende -Geschwindigkeit aus `motorPosition.speeds` lesen, wenn diese korrekt befüllt sind. +So braucht jeder Controller dieselbe Zeit → die Bewegungen sind koordiniert. + +--- + +## Behobene Defizite (Implementierung dieses ToDos) + +### 1. `calculateSpeeds()` berechnete NaN — behoben + +`Robot.calculateSpeeds()` las `oldPos.xMotor`, `oldPos.alpha`, `oldPos.beta` — Felder, die +in `RobotMotorPosition` nicht existieren (dort `x`, `y`, `z`). Ergebnis: `NaN`. +Korrigiert auf `oldPos.x/y/z`. Zusätzlich speichert die Methode jetzt die `Bewegungszeit` +(`moveTime`), die der Sender für die koordinierte Feedrate braucht. + +### 2. Koordinierte Feedrate im Sender (Korrekt-Modus) + +`TelnetSenderGRBL` berechnet im Korrekt-Modus die Feedrate als +`Distanz_dieses_Senders / moveTime`. Die Distanz wird über `portValue()` ermittelt — +eine reine Funktion, die für jede (GRBL-Port, Roboter-Achse)-Zuordnung den gesendeten +Wert liefert (dieselben Formeln wie der Sende-Pfad, aber als isolierte, testbare Funktion). --- ## Pakete -### Paket 1: `calculateSpeeds()` reparieren +### Paket 1: `calculateSpeeds()` reparieren — ✅ ERLEDIGT -- [ ] Property-Namen korrigieren: `oldPos.x/y/z/a/b/c/e` statt `oldPos.xMotor/alpha/beta/...` -- [ ] Einheit klären: `motorSpeeds` in rad/min oder direkt in Grad/min? - - Sender wandeln Motorwinkel bereits von rad → Grad (`* 180/π`) für Positionen - - Einheitlichste Lösung: `motorSpeeds` ebenfalls in Grad/min speichern, oder - die Umrechnung konsistent im Sender vornehmen -- [ ] Grenzfall: Wenn kartesische Distanz null ist (reine Gelenkbewegung), Distanz - über Handgelenk-Punkt verwenden — das ist bereits so angelegt, aber mit den - falschen Property-Namen -- [ ] Unit-Test: `calculateSpeeds` mit bekannten Werten, prüfen dass keine NaN entstehen +- [x] Property-Namen korrigiert: `oldPos.x/y/z/a/b/c/e` +- [x] `moveTime` berechnen und auf `motorPosition` ablegen +- [x] Unit-Tests: NaN-Freiheit, exakte Werte, Handgelenk-/Finger-Zweige, Guards + (`test/Robot.calculateSpeeds.test.js`) -### Paket 2: `motorSpeeds` in den Sender durchreichen +### Paket 2: Schalter + koordinierte Feedrate — ✅ ERLEDIGT -- [ ] `motorPosition.speeds` korrekt befüllen (passiert bereits via `this.motorPosition.speeds = {...this.motorSpeeds}` nach `calculateSpeeds`) -- [ ] Sender-Interface (`execCommand`): wenn `ROBOT_USE_SPEED_CALC` aktiv und - `motorPosition.speeds[achse]` verfügbar und > 0 → diesen Wert als `F` verwenden -- [ ] Jeder Sender (`TelnetSenderGRBL`, `WSSenderGrbl`) kennt seine Achse und holt - die passende Geschwindigkeit aus `speeds`: - ```js - // Beispiel: Sender ist für Achse "a" zuständig - const f = (useSpeedCalc && mNew.speeds.a > 0) ? mNew.speeds.a : mNew.feedrate; - data += ` f${f.toFixed(2)}`; - ``` -- [ ] Fallback: wenn `ROBOT_USE_SPEED_CALC=false` oder Speeds nicht berechnet → - `mNew.feedrate` wie bisher (Rückwärtskompatibilität) +- [x] `ROBOT_SPEED_MODE`-Schalter (`legacy` Default, `correct`) +- [x] `TelnetSenderGRBL`: koordinierte Feedrate im Korrekt-Modus, Legacy-Pfad unverändert +- [x] `portValue()` als isolierte Funktion, per Kreuzprobe gegen den echten Sende-Pfad getestet +- [x] Korrekt-Modus-Tests + Koordinations-Invariante über alle drei Sender + (`test/Sender.Telnet.speedMode.test.js`, `test/Speed.coordination.test.js`) -### Paket 3: `WSSenderGrbl` Feedrate-Handling vereinheitlichen +### Paket 3: `WSSenderGrbl` — ⬜ OFFEN (Folge-Schritt) -- [ ] `WSSenderGrbl.execCommand()` auf dasselbe Feedrate-Schema wie `TelnetSenderGRBL` umstellen - - aktuell: immer `this.maxSpeedF` → ignoriert das `F` aus dem G-Code-Befehl komplett - - richtig: `mNew.feedrate` verwenden (bzw. per-Achse aus Paket 2) -- [ ] `maxSpeedF` als Obergrenze behalten (Clamp), nicht als feste Ausgabe +- [ ] `WSSenderGrbl` auf denselben Schalter + koordinierte Feedrate bringen + - WS-Sender ist aktuell nicht in `startRobot.js` aktiv → kein Produktiv-Risiko + - Legacy-Verhalten des WS-Senders zunächst unverändert lassen -### Paket 4: Kleinere Korrekturen +### Paket 4: Aufräumen — ⬜ OFFEN (Folge-Schritt) -- [ ] `FPoint` in `GCode.receiveFC()`: `robot.feedrate` statt hardcodierten `1000` speichern -- [ ] `ROBOT_USE_SPEED_CALC`-Flag dokumentieren: was ändert sich mit/ohne Flag? - Klarer Kommentar in `Robot.js` und im README +- [ ] `FPoint` in `GCode.receiveFC()`: `robot.feedrate` statt hardcodiertem `1000` + → gehört thematisch zur Datei-Logik, wird in **ToDo_6b** behandelt +- [ ] `portValue()` perspektivisch auch im Sende-Pfad nutzen (Dedupe der Formeln); + bewusst aufgeschoben, um den Legacy-Pfad in diesem Schritt nicht anzufassen + +### Bekannte Grenze des Korrekt-Modus v1 + +Die koordinierte Feedrate bildet die euklidische Norm über die Achsen eines Senders. +Mischt ein Sender lineare (mm) und winklige (Grad) Achsen, mischt die Norm diese +Einheiten — dieselbe Vereinfachung, die GRBL bei gemischten Achsen ohnehin macht. +Für eine spätere, einheiten-saubere Behandlung ggf. eigenes ToDo. ## Betroffene Dateien -- `robot/Robot.js` — `calculateSpeeds()` bugfix, Einheitenwahl -- `robot/RobotMotorPosition.js` — ggf. `speeds`-Feld klarer benennen -- `robot/TelnetSenderGRBL.js` — per-Achse Feedrate aus `speeds` -- `robot/WSSenderGrbl.js` — Feedrate auf `mNew.feedrate` umstellen + Clamp -- `robot/GCode.js` — `FPoint`: `robot.feedrate` statt `1000` -- `test/GCode.speed.test.js` — Tests für NaN-freie Berechnung ergänzen +- `robot/Robot.js` — `calculateSpeeds()` Fix, `moveTime`, Schalter→`useSpeedCalc` +- `robot/RobotMotorPosition.js` — `moveTime`-Feld +- `robot/TelnetSenderGRBL.js` — `portValue()`, koordinierte Feedrate, Schalter +- `test/Robot.calculateSpeeds.test.js` — neu +- `test/Sender.Telnet.speedMode.test.js` — neu diff --git a/logs/gcode_commands.log b/logs/gcode_commands.log index 43dea3c..7ddcc65 100644 --- a/logs/gcode_commands.log +++ b/logs/gcode_commands.log @@ -10269,3 +10269,73 @@ 2026-06-08T17:01:57.362Z ::ffff:127.0.0.1: M114 2026-06-08T17:01:57.582Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 2026-06-08T17:01:57.813Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:21:58.752Z ::ffff:127.0.0.1: M114 +2026-06-09T10:21:58.971Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:21:59.204Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:21:59.494Z ::ffff:127.0.0.1: M114 +2026-06-09T10:21:59.505Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:23:56.576Z ::ffff:127.0.0.1: M114 +2026-06-09T10:23:56.792Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:23:57.007Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:23:57.900Z ::ffff:127.0.0.1: M114 +2026-06-09T10:23:57.907Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:24:56.076Z ::ffff:127.0.0.1: M114 +2026-06-09T10:24:56.093Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:24:56.450Z ::ffff:127.0.0.1: M114 +2026-06-09T10:24:56.670Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:24:56.900Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:25:21.354Z ::ffff:127.0.0.1: M114 +2026-06-09T10:25:21.373Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:25:21.638Z ::ffff:127.0.0.1: M114 +2026-06-09T10:25:21.864Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:25:22.092Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:25:47.666Z ::ffff:127.0.0.1: M114 +2026-06-09T10:25:47.884Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:25:48.115Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:25:49.421Z ::ffff:127.0.0.1: M114 +2026-06-09T10:25:49.429Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:26:07.232Z ::ffff:127.0.0.1: M114 +2026-06-09T10:26:07.253Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:26:07.426Z ::ffff:127.0.0.1: M114 +2026-06-09T10:26:07.655Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:26:07.884Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:26:20.226Z ::ffff:127.0.0.1: M114 +2026-06-09T10:26:20.245Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:26:20.452Z ::ffff:127.0.0.1: M114 +2026-06-09T10:26:20.667Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:26:20.894Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:26:33.300Z ::ffff:127.0.0.1: M114 +2026-06-09T10:26:33.518Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:26:33.747Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:26:33.979Z ::ffff:127.0.0.1: M114 +2026-06-09T10:26:33.985Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:29:43.175Z ::ffff:127.0.0.1: M114 +2026-06-09T10:29:43.394Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:29:43.621Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:29:44.159Z ::ffff:127.0.0.1: M114 +2026-06-09T10:29:44.165Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:37:57.971Z ::ffff:127.0.0.1: M114 +2026-06-09T10:37:57.990Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:37:58.081Z ::ffff:127.0.0.1: M114 +2026-06-09T10:37:58.317Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:37:58.555Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:39:15.320Z ::ffff:127.0.0.1: M114 +2026-06-09T10:39:15.338Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:39:15.579Z ::ffff:127.0.0.1: M114 +2026-06-09T10:39:15.804Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:39:16.030Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:39:38.781Z ::ffff:127.0.0.1: M114 +2026-06-09T10:39:38.805Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:39:38.958Z ::ffff:127.0.0.1: M114 +2026-06-09T10:39:39.183Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:39:39.408Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:39:57.292Z ::ffff:127.0.0.1: M114 +2026-06-09T10:39:57.508Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:39:57.737Z ::ffff:127.0.0.1: G1 X1 +2026-06-09T10:39:57.872Z ::ffff:127.0.0.1: M114 +2026-06-09T10:39:57.877Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:41:44.504Z ::ffff:127.0.0.1: M114 +2026-06-09T10:41:44.520Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:41:44.720Z ::ffff:127.0.0.1: M114 +2026-06-09T10:41:44.937Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-09T10:41:45.162Z ::ffff:127.0.0.1: G1 X1 diff --git a/logs/pings.log b/logs/pings.log index 0f128a1..57901bc 100644 --- a/logs/pings.log +++ b/logs/pings.log @@ -14582,3 +14582,31 @@ 2026-06-08T17:01:45.128Z ::ffff:127.0.0.1 : Ping 2026-06-08T17:01:57.161Z ::ffff:127.0.0.1 : Ping 2026-06-08T17:01:57.231Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:21:58.539Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:21:59.485Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:23:56.359Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:23:57.891Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:24:56.048Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:24:56.216Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:25:21.323Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:25:21.407Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:25:47.453Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:25:49.403Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:26:07.185Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:26:07.195Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:26:20.179Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:26:20.206Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:26:33.080Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:26:33.971Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:29:42.949Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:29:44.153Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:37:57.831Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:37:57.933Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:39:15.280Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:39:15.324Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:39:38.709Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:39:38.743Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:39:57.078Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:39:57.863Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:41:44.479Z ::ffff:127.0.0.1 : Ping +2026-06-09T10:41:44.499Z ::ffff:127.0.0.1 : Ping diff --git a/robot/GCode.js b/robot/GCode.js index c6e9559..67fdcdf 100755 --- a/robot/GCode.js +++ b/robot/GCode.js @@ -2,7 +2,7 @@ * Receives GCode, processes it and moves the Data to the Roboter-Class */ const fs = require('fs'); -const GCodeParser = require('./GCodeParser'); +const RobotController = require('./RobotController'); class GCode{ @@ -81,106 +81,14 @@ class GCode{ return newGString.trim(); }; + /** + * Verarbeitet eine rohe G-Code-Nachricht. Parsing + Steuerlogik liegen jetzt im + * GCodeParser bzw. RobotController (ToDo_6); diese Methode bleibt als Fassade + * erhalten, damit bestehende Aufrufer (InputWS, Datei-Befehle) unverändert + * funktionieren. + */ static receiveGCode(robot, g){ - const commands = GCodeParser.parse(g); - if (!commands.length) return; - - commands.forEach(parsed => { - if (!parsed || !parsed.command) { - return; - } - - const cmd = parsed.command; - const params = parsed.params || {}; - - if (cmd === 'G90') { - robot.moveRelative = false; - return; - } - - if (cmd === 'G91') { - robot.moveRelative = true; - return; - } - - if (cmd === 'G28') { - robot.x = 0; - robot.y = robot.l1 + robot.l2 + robot.l3; - robot.z = 0; - robot.phi = -Math.PI / 2; - robot.theta = Math.PI / 2; - robot.psi = 0; - robot.e = 0; - robot.calculateAngles3D(); - robot.sendCommand(); - return; - } - - if (cmd === 'G1') { - if (robot.moveRelative) { - if (Number.isFinite(params.X)) { robot.x += params.X; robot.xMotorChanged = true; } - if (Number.isFinite(params.Y)) { robot.y += params.Y; robot.yMotorChanged = true; } - if (Number.isFinite(params.Z)) { robot.z += params.Z; robot.zMotorChanged = true; } - if (Number.isFinite(params.A)) { robot.phi += params.A; robot.aMotorChanged = true; } - if (Number.isFinite(params.B)) { robot.theta += params.B; robot.bMotorChanged = true; } - if (Number.isFinite(params.C)) { robot.psi += params.C; robot.cMotorChanged = true; } - if (Number.isFinite(params.E)) { robot.e += params.E; robot.eMotorChanged = true; } - if (Number.isFinite(params.F)) { robot.feedrate = params.F; } - } else { - if (Number.isFinite(params.X)) { robot.x = params.X; robot.xMotorChanged = true; } - if (Number.isFinite(params.Y)) { robot.y = params.Y; robot.yMotorChanged = true; } - if (Number.isFinite(params.Z)) { robot.z = params.Z; robot.zMotorChanged = true; } - if (Number.isFinite(params.A)) { robot.phi = params.A; robot.aMotorChanged = true; } - if (Number.isFinite(params.B)) { robot.theta = params.B; robot.bMotorChanged = true; } - if (Number.isFinite(params.C)) { robot.psi = params.C; robot.cMotorChanged = true; } - if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; } - if (Number.isFinite(params.F)) { robot.feedrate = params.F; } - } - - robot.calculateAngles3D(); - robot.sendCommand(); - return; - } - - if (cmd === 'M1') { - if (robot.moveRelative) { - if (Number.isFinite(params.X)) { robot.xMotor += params.X; robot.xMotorChanged = true; } - if (Number.isFinite(params.Y)) { robot.alpha += params.Y; robot.yMotorChanged = true; } - if (Number.isFinite(params.Z)) { robot.beta += params.Z; robot.zMotorChanged = true; } - if (Number.isFinite(params.A)) { robot.a += params.A; robot.aMotorChanged = true; } - if (Number.isFinite(params.B)) { robot.b += params.B; robot.bMotorChanged = true; } - if (Number.isFinite(params.C)) { robot.c += params.C; robot.cMotorChanged = true; } - if (Number.isFinite(params.E)) { robot.e += params.E; robot.eMotorChanged = true; } - } else { - if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; } - if (Number.isFinite(params.Y)) { robot.alpha = params.Y; robot.yMotorChanged = true; } - if (Number.isFinite(params.Z)) { robot.beta = params.Z; robot.zMotorChanged = true; } - if (Number.isFinite(params.A)) { robot.a = params.A; robot.aMotorChanged = true; } - if (Number.isFinite(params.B)) { robot.b = params.B; robot.bMotorChanged = true; } - if (Number.isFinite(params.C)) { robot.c = params.C; robot.cMotorChanged = true; } - if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; } - } - - robot.calculatePositionFromMotorAngles(); - robot.sendCommand(); - return; - } - - if (cmd === 'M92') { - robot.createMotorPosition(); - if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; } - if (Number.isFinite(params.Y)) { robot.alpha = params.Y; robot.yMotorChanged = true; } - if (Number.isFinite(params.Z)) { robot.beta = params.Z; robot.zMotorChanged = true; } - if (Number.isFinite(params.A)) { robot.a = params.A; robot.aMotorChanged = true; } - if (Number.isFinite(params.B)) { robot.b = params.B; robot.bMotorChanged = true; } - if (Number.isFinite(params.C)) { robot.c = params.C; robot.cMotorChanged = true; } - if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; } - - robot.calculatePositionFromMotorAngles(); - robot.sendCommand('G92'); - return; - } - }); + RobotController.receive(robot, g); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////77 diff --git a/robot/Robot.js b/robot/Robot.js index 712433e..53f2d0b 100755 --- a/robot/Robot.js +++ b/robot/Robot.js @@ -7,8 +7,16 @@ class Robot{ // Umgebungsvariablen-Logik const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ? Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000; - this.useSpeedCalc = process.env.ROBOT_USE_SPEED_CALC === 'true' || + // Speed-Regelung-Schalter: 'legacy' (Default — exakt wie bisher) oder 'correct'. + // Siehe doc/ToDo_6a_Speed.md. + this.speedMode = (process.env.ROBOT_SPEED_MODE || 'legacy').toLowerCase(); + // ROBOT_USE_SPEED_CALC bleibt der interne Schalter für calculateSpeeds(); + // der Korrekt-Modus aktiviert die Berechnung automatisch. + this.useSpeedCalc = this.speedMode === 'correct' || + process.env.ROBOT_USE_SPEED_CALC === 'true' || process.env.ROBOT_USE_SPEED_CALC === '1'; + /** @type {number} Bewegungszeit des letzten Schritts in Minuten (für koordinierte Feedrate) */ + this.lastMoveTime = 0; /** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */ this.speedX = 200; @@ -131,6 +139,10 @@ class Robot{ // Setze Geschwindigkeiten this.motorPosition.speeds = {...this.motorSpeeds}; this.motorPosition.feedrate = this.feedrate || 200; + + // Speed-Regelung: Modus und (vorläufige) Bewegungszeit für die Sender + this.motorPosition.speedMode = this.speedMode; + this.motorPosition.moveTime = this.lastMoveTime || 0; } // Berechnet aus XYZ die Motor-Winkel für den GCode @@ -203,6 +215,8 @@ class Robot{ if (!this.useSpeedCalc) return; // Neue Logik nur aktivieren, wenn Flag gesetzt if (!oldPos || !newPos || this.feedrate <= 0) return; + this.lastMoveTime = 0; // wird unten gesetzt, sobald eine Bewegung erkannt wird + // 1. Berechne xyz-Distanz (primär) const dx = newPos.x - oldPos.x; const dy = newPos.y - oldPos.y; @@ -211,9 +225,10 @@ class Robot{ if (xyz_dist > 0.001) { const time = xyz_dist / this.feedrate; - this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time; - this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time; - this.motorSpeeds.z = (this.beta - oldPos.beta) / time; + this.lastMoveTime = time; + this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; + this.motorSpeeds.y = (this.alpha - oldPos.y) / time; + this.motorSpeeds.z = (this.beta - oldPos.z) / time; this.motorSpeeds.a = (this.a - oldPos.a) / time; this.motorSpeeds.b = (this.b - oldPos.b) / time; this.motorSpeeds.c = (this.c - oldPos.c) / time; @@ -229,9 +244,10 @@ class Robot{ if (handgelenk_dist > 0.001) { const time = handgelenk_dist / this.feedrate; - this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time; - this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time; - this.motorSpeeds.z = (this.beta - oldPos.beta) / time; + this.lastMoveTime = time; + this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; + this.motorSpeeds.y = (this.alpha - oldPos.y) / time; + this.motorSpeeds.z = (this.beta - oldPos.z) / time; this.motorSpeeds.a = (this.a - oldPos.a) / time; this.motorSpeeds.b = (this.b - oldPos.b) / time; this.motorSpeeds.c = (this.c - oldPos.c) / time; @@ -243,9 +259,10 @@ class Robot{ const de = Math.abs(this.eMotor - oldPos.e); if (de > 0.001) { const time = de / this.feedrate; - this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time; - this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time; - this.motorSpeeds.z = (this.beta - oldPos.beta) / time; + this.lastMoveTime = time; + this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; + this.motorSpeeds.y = (this.alpha - oldPos.y) / time; + this.motorSpeeds.z = (this.beta - oldPos.z) / time; this.motorSpeeds.a = (this.a - oldPos.a) / time; this.motorSpeeds.b = (this.b - oldPos.b) / time; this.motorSpeeds.c = (this.c - oldPos.c) / time; @@ -336,6 +353,8 @@ class Robot{ // Berechne Geschwindigkeiten this.calculateSpeeds(this.motorPositionOld, this.motorPosition); this.motorPosition.speeds = {...this.motorSpeeds}; + // moveTime nach der Berechnung aktualisieren (createMotorPosition lief davor) + this.motorPosition.moveTime = this.lastMoveTime || 0; console.log("Robot.sendCommand: ", cmd.toString(), " Motor-Pos: x=", this.motorPosition.x.toFixed(3), "yMotor=",this.motorPosition.y.toFixed(3), "zMotor=",this.motorPosition.z.toFixed(3), "aM=", this.motorPosition.a.toFixed(3), "bM=", this.motorPosition.b.toFixed(3), "cM=", this.motorPosition.c.toFixed(3), " e=", this.motorPosition.e.toFixed(3)); diff --git a/robot/RobotController.js b/robot/RobotController.js new file mode 100644 index 0000000..ddddc5b --- /dev/null +++ b/robot/RobotController.js @@ -0,0 +1,126 @@ +/*** + * RobotController — Steuerlogik (ToDo_6). + * + * Verarbeitet bereits geparste G-Code-/Befehlsobjekte (aus GCodeParser) und wendet + * sie auf das Robotermodell an: Bewegungsmodus, Kinematik, sendCommand(). + * + * Bewusst getrennt vom Parsing (GCodeParser) und vom Datei-Handling (GCode): + * der Controller kennt nur strukturierte Befehle, keine rohen Textstrings. + */ +const GCodeParser = require('./GCodeParser'); + +class RobotController { + + /** + * Parst eine rohe Nachricht und wendet alle enthaltenen Befehle der Reihe nach an. + * @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)); + } + + /** + * Wendet einen einzelnen, bereits geparsten Befehl auf das Robotermodell an. + * @param {object} robot + * @param {{command: string, params: object}} parsed + */ + static applyCommand(robot, parsed) { + if (!parsed || !parsed.command) return; + + const cmd = parsed.command; + const params = parsed.params || {}; + + if (cmd === 'G90') { + robot.moveRelative = false; + return; + } + + if (cmd === 'G91') { + robot.moveRelative = true; + return; + } + + if (cmd === 'G28') { + robot.x = 0; + robot.y = robot.l1 + robot.l2 + robot.l3; + robot.z = 0; + robot.phi = -Math.PI / 2; + robot.theta = Math.PI / 2; + robot.psi = 0; + robot.e = 0; + robot.calculateAngles3D(); + robot.sendCommand(); + return; + } + + if (cmd === 'G1') { + if (robot.moveRelative) { + if (Number.isFinite(params.X)) { robot.x += params.X; robot.xMotorChanged = true; } + if (Number.isFinite(params.Y)) { robot.y += params.Y; robot.yMotorChanged = true; } + if (Number.isFinite(params.Z)) { robot.z += params.Z; robot.zMotorChanged = true; } + if (Number.isFinite(params.A)) { robot.phi += params.A; robot.aMotorChanged = true; } + if (Number.isFinite(params.B)) { robot.theta += params.B; robot.bMotorChanged = true; } + if (Number.isFinite(params.C)) { robot.psi += params.C; robot.cMotorChanged = true; } + if (Number.isFinite(params.E)) { robot.e += params.E; robot.eMotorChanged = true; } + if (Number.isFinite(params.F)) { robot.feedrate = params.F; } + } else { + if (Number.isFinite(params.X)) { robot.x = params.X; robot.xMotorChanged = true; } + if (Number.isFinite(params.Y)) { robot.y = params.Y; robot.yMotorChanged = true; } + if (Number.isFinite(params.Z)) { robot.z = params.Z; robot.zMotorChanged = true; } + if (Number.isFinite(params.A)) { robot.phi = params.A; robot.aMotorChanged = true; } + if (Number.isFinite(params.B)) { robot.theta = params.B; robot.bMotorChanged = true; } + if (Number.isFinite(params.C)) { robot.psi = params.C; robot.cMotorChanged = true; } + if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; } + if (Number.isFinite(params.F)) { robot.feedrate = params.F; } + } + + robot.calculateAngles3D(); + robot.sendCommand(); + return; + } + + if (cmd === 'M1') { + if (robot.moveRelative) { + if (Number.isFinite(params.X)) { robot.xMotor += params.X; robot.xMotorChanged = true; } + if (Number.isFinite(params.Y)) { robot.alpha += params.Y; robot.yMotorChanged = true; } + if (Number.isFinite(params.Z)) { robot.beta += params.Z; robot.zMotorChanged = true; } + if (Number.isFinite(params.A)) { robot.a += params.A; robot.aMotorChanged = true; } + if (Number.isFinite(params.B)) { robot.b += params.B; robot.bMotorChanged = true; } + if (Number.isFinite(params.C)) { robot.c += params.C; robot.cMotorChanged = true; } + if (Number.isFinite(params.E)) { robot.e += params.E; robot.eMotorChanged = true; } + } else { + if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; } + if (Number.isFinite(params.Y)) { robot.alpha = params.Y; robot.yMotorChanged = true; } + if (Number.isFinite(params.Z)) { robot.beta = params.Z; robot.zMotorChanged = true; } + if (Number.isFinite(params.A)) { robot.a = params.A; robot.aMotorChanged = true; } + if (Number.isFinite(params.B)) { robot.b = params.B; robot.bMotorChanged = true; } + if (Number.isFinite(params.C)) { robot.c = params.C; robot.cMotorChanged = true; } + if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; } + } + + robot.calculatePositionFromMotorAngles(); + robot.sendCommand(); + return; + } + + if (cmd === 'M92') { + robot.createMotorPosition(); + if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; } + if (Number.isFinite(params.Y)) { robot.alpha = params.Y; robot.yMotorChanged = true; } + if (Number.isFinite(params.Z)) { robot.beta = params.Z; robot.zMotorChanged = true; } + if (Number.isFinite(params.A)) { robot.a = params.A; robot.aMotorChanged = true; } + if (Number.isFinite(params.B)) { robot.b = params.B; robot.bMotorChanged = true; } + if (Number.isFinite(params.C)) { robot.c = params.C; robot.cMotorChanged = true; } + if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; } + + robot.calculatePositionFromMotorAngles(); + robot.sendCommand('G92'); + return; + } + } +} + +module.exports = RobotController; diff --git a/robot/RobotMotorPosition.js b/robot/RobotMotorPosition.js index e5b0dd2..fa8bfc5 100755 --- a/robot/RobotMotorPosition.js +++ b/robot/RobotMotorPosition.js @@ -32,6 +32,12 @@ module.exports = class RobotMotorPosition{ // Feedrate in mm/min this.feedrate = 0; + + // Bewegungszeit des Schritts in Minuten (für koordinierte Feedrate, ToDo_6a) + this.moveTime = 0; + + // Speed-Regelung-Modus, vom Robot gesetzt: 'legacy' | 'correct' + this.speedMode = 'legacy'; } } diff --git a/robot/TelnetSenderGRBL.js b/robot/TelnetSenderGRBL.js index 20cf26e..dca6ac2 100755 --- a/robot/TelnetSenderGRBL.js +++ b/robot/TelnetSenderGRBL.js @@ -235,6 +235,116 @@ module.exports = class TelnetSenderGRBL extends SenderInterface { this.execCommand("G1", mOld, mNew) } + /** + * Liefert den Wert, der an einen GRBL-Port gesendet wird, wenn dieser auf eine + * Roboter-Achse abgebildet ist — als reine Funktion der Positions-/Geschwindigkeits- + * Quelle `pos` ({x,y,z,a,b,c,e}). Dieselben Formeln wie der Sende-Pfad, aber isoliert + * und testbar. Wird für die koordinierte Feedrate (ToDo_6a) verwendet. + * @returns {number|null} + */ + portValue(grblPort, robotAxis, pos) { + if (!robotAxis || !pos) return null; + const D = 180 / Math.PI; + const factorTurnLift = 1.2; + const handOpenInMM = 1.0; + const { x, y, z, a, b, c, e } = pos; + + switch (grblPort) { + case 'x': + switch (robotAxis) { + case 'x': return x; + case 'y': return y * D; + case 'z': return (z - y) * D; + case 'a': return a * D; + case 'b': return (b + z - y) * D; + case 'c': return (-b + c) * D; + case 'e': return -1.0 * (-1.0 * (b * D * factorTurnLift) + c * D) + e * handOpenInMM; + } + break; + case 'y': + switch (robotAxis) { + case 'x': return x; + case 'y': return y * D; + case 'z': return (z - y) * D; + case 'a': return a * D; + case 'b': return (b + z - y) * D; + case 'c': return -1.0 * (b * D * factorTurnLift) + c * D; + case 'e': return e * D; + } + break; + case 'z': + switch (robotAxis) { + case 'x': return x; + case 'y': return y * D; + case 'z': return (z - y) * D; + case 'a': return a * D; + case 'b': return b * D; + case 'c': return (c + b + z - y) * D; + case 'e': return e * D; + } + break; + case 'a': + switch (robotAxis) { + case 'x': return y * D; // Quirk: a-Port auf robotAxis 'x' nutzt y (wie im Sende-Pfad) + case 'y': return y * D; + case 'z': return (z - y) * D; + case 'a': return a * D; + case 'b': return (b + z - y) * D; + case 'c': return (c + b + z - y) * D; + case 'e': return e * D; + } + break; + case 'b': + switch (robotAxis) { + case 'x': return x; + case 'y': return y * D; + case 'z': return (z - y) * D; + case 'a': return a * D; + case 'b': return b * D; + case 'c': return (c + b + z - y) * D; + case 'e': return e * D; + } + break; + } + return null; + } + + /** + * Koordinierte Feedrate für diesen Sender (Korrekt-Modus): die euklidische Strecke, + * die dieser Sender in diesem Schritt zurücklegt, geteilt durch die Bewegungszeit. + * So beenden alle Controller den Schritt gleichzeitig. + * @returns {number|null} Feedrate oder null, wenn nicht bestimmbar (→ Legacy-Fallback) + */ + computeCoordinatedFeedrate(mOld, mNew) { + const moveTime = mNew && Number.isFinite(mNew.moveTime) ? mNew.moveTime : 0; + if (!(moveTime > 0) || !mOld) return null; + + const ports = [ + ['x', this.xAxisGrbl], + ['y', this.yAxisGrbl], + ['z', this.zAxisGrbl], + ['a', this.aAxisGrbl], + ['b', this.bAxisGrbl], + ]; + + let sumSq = 0; + let any = false; + for (const [port, robotAxis] of ports) { + if (!robotAxis) continue; + const vNew = this.portValue(port, robotAxis, mNew); + const vOld = this.portValue(port, robotAxis, mOld); + if (!Number.isFinite(vNew) || !Number.isFinite(vOld)) continue; + const d = vNew - vOld; + sumSq += d * d; + any = true; + } + if (!any) return null; + + const dist = Math.sqrt(sumSq); + if (!(dist > 0)) return null; + return dist / moveTime; + } + execCommand(strCommand = "G1", mOld, mNew ){ // The Hand-Turn is not 1:1 to the Hand-Lift ° @@ -377,14 +487,22 @@ module.exports = class TelnetSenderGRBL extends SenderInterface { if(this.tSocket && data.length > 3){ if(strCommand == "G1" && mNew){ - const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ? + const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ? Number(process.env.ROBOT_DEFAULT_FEEDRATE) : this.maxSpeedF; - const feedrate = mNew.feedrate !== undefined ? mNew.feedrate : DEFAULT_FEEDRATE; - data += " f"+(feedrate.toFixed(2).toString()) - // ToDo: Aus motorSpeed mit einzelnenen Werten muss noch die feedrate berechnet werden. - // hier bin ich unsicher, ob das nicht in den Sender rein sollte, da es eventuell - // abhngig vom FluidNC und dessen speed interpretation ist. + let feedrate; + if (mNew.speedMode === 'correct') { + // Korrekt-Modus: koordinierte Feedrate; Fallback auf Legacy-Wert, + // wenn (noch) nicht bestimmbar (z. B. erster Schritt ohne moveTime). + const coordinated = this.computeCoordinatedFeedrate(mOld, mNew); + feedrate = (coordinated != null && Number.isFinite(coordinated) && coordinated > 0) + ? coordinated + : (mNew.feedrate !== undefined ? mNew.feedrate : DEFAULT_FEEDRATE); + } else { + // Legacy-Pfad — unverändert: kartesische Feedrate an alle Achsen + feedrate = mNew.feedrate !== undefined ? mNew.feedrate : DEFAULT_FEEDRATE; + } + data += " f"+(feedrate.toFixed(2).toString()) } if(data.indexOf("G90") == -1 && data.indexOf("G1 ") !== -1){ diff --git a/test/Robot.calculateSpeeds.test.js b/test/Robot.calculateSpeeds.test.js new file mode 100644 index 0000000..0fc19c0 --- /dev/null +++ b/test/Robot.calculateSpeeds.test.js @@ -0,0 +1,128 @@ +const Robot = require('../robot/Robot'); +const MotorPosition = require('../robot/RobotMotorPosition'); + +describe('Robot.calculateSpeeds (ToDo_6a)', () => { + + beforeAll(() => { + jest.spyOn(console, 'log').mockImplementation(() => {}); + }); + afterAll(() => { + jest.restoreAllMocks(); + }); + + function makeRobot() { + const robot = new Robot(250, 264, 100); + robot.useSpeedCalc = true; + robot.feedrate = 1000; + return robot; + } + + test('erzeugt keine NaN-Werte (Property-Bug behoben) und setzt moveTime', () => { + const robot = makeRobot(); + robot.xMotor = 10; robot.alpha = 1; robot.beta = 1; + robot.a = 0.5; robot.b = 0.5; robot.c = 0.5; robot.eMotor = 0.2; + + const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0); + const newPos = new MotorPosition(10, 1, 1, 0.5, 0.5, 0.5, 0.2); + + robot.calculateSpeeds(oldPos, newPos); + + for (const k of ['x', 'y', 'z', 'a', 'b', 'c', 'e']) { + expect(Number.isFinite(robot.motorSpeeds[k])).toBe(true); + } + expect(robot.motorSpeeds.x).toBeGreaterThan(0); + expect(robot.lastMoveTime).toBeGreaterThan(0); + }); + + test('exakte Werte: motorSpeeds = Delta/Zeit, moveTime = Distanz/Feedrate', () => { + const robot = makeRobot(); + // reine X-Motor-Bewegung um 30 mm + robot.xMotor = 30; robot.alpha = 0; robot.beta = 0; + + const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0); + const newPos = new MotorPosition(30, 0, 0, 0, 0, 0, 0); + + robot.calculateSpeeds(oldPos, newPos); + + // Distanz = 30, feedrate = 1000 → moveTime = 0.03 min + expect(robot.lastMoveTime).toBeCloseTo(0.03, 6); + // motorSpeeds.x = 30 / 0.03 = 1000 + expect(robot.motorSpeeds.x).toBeCloseTo(1000, 4); + // unbewegte Achsen → 0 + expect(robot.motorSpeeds.y).toBeCloseTo(0, 6); + expect(robot.motorSpeeds.z).toBeCloseTo(0, 6); + }); + + test('Handgelenk-Zweig: xyz-Motoren unverändert, Handgelenk-Punkt bewegt sich', () => { + const robot = makeRobot(); + robot.xMotor = 0; robot.alpha = 0; robot.beta = 0; + + const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0); + oldPos.pX = 0; oldPos.pY = 0; oldPos.pZ = 0; + + const newPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0); + newPos.pX = 4; newPos.pY = 0; newPos.pZ = 3; // Handgelenk-Distanz = 5 + + robot.calculateSpeeds(oldPos, newPos); + + // xyz-Distanz ist 0 → der Handgelenk-Zweig greift: moveTime = 5 / 1000 + expect(robot.lastMoveTime).toBeCloseTo(0.005, 6); + }); + + test('Finger-Zweig: nur e bewegt sich', () => { + const robot = makeRobot(); + robot.eMotor = 2; + + const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0); + const newPos = new MotorPosition(0, 0, 0, 0, 0, 0, 2); + + robot.calculateSpeeds(oldPos, newPos); + + expect(Number.isFinite(robot.motorSpeeds.e)).toBe(true); + expect(robot.lastMoveTime).toBeCloseTo(0.002, 6); // 2 / 1000 + }); + + test('Guard: useSpeedCalc deaktiviert → keine Änderung (Legacy)', () => { + const robot = makeRobot(); + robot.useSpeedCalc = false; + robot.xMotor = 10; + + const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0); + const newPos = new MotorPosition(10, 0, 0, 0, 0, 0, 0); + + robot.calculateSpeeds(oldPos, newPos); + + expect(robot.motorSpeeds.x).toBe(0); + expect(robot.lastMoveTime).toBe(0); + }); + + test('Guard: feedrate <= 0 → keine Berechnung', () => { + const robot = makeRobot(); + robot.feedrate = 0; + robot.xMotor = 10; + + const oldPos = new MotorPosition(0, 0, 0, 0, 0, 0, 0); + const newPos = new MotorPosition(10, 0, 0, 0, 0, 0, 0); + + robot.calculateSpeeds(oldPos, newPos); + + expect(robot.motorSpeeds.x).toBe(0); + expect(robot.lastMoveTime).toBe(0); + }); + + test('Guard: fehlende Positionen → kein Fehler, keine Änderung', () => { + const robot = makeRobot(); + expect(() => robot.calculateSpeeds(null, null)).not.toThrow(); + expect(robot.motorSpeeds.x).toBe(0); + }); + + test('keine Bewegung (alle Distanzen 0) → moveTime bleibt 0', () => { + const robot = makeRobot(); + const pos = new MotorPosition(5, 0.1, 0.2, 0, 0, 0, 0); + robot.xMotor = 5; robot.alpha = 0.1; robot.beta = 0.2; + + robot.calculateSpeeds(pos, pos); + + expect(robot.lastMoveTime).toBe(0); + }); +}); diff --git a/test/RobotController.test.js b/test/RobotController.test.js new file mode 100644 index 0000000..cd9c091 --- /dev/null +++ b/test/RobotController.test.js @@ -0,0 +1,72 @@ +const RobotController = require('../robot/RobotController'); +const createDummyRobot = require('./helpers/createDummyRobot'); + +describe('RobotController (ToDo_6)', () => { + + test('applyCommand: G90 setzt Absolutmodus, ohne Bewegung', () => { + const robot = createDummyRobot(); + robot.moveRelative = true; + + RobotController.applyCommand(robot, { command: 'G90', params: {} }); + + expect(robot.moveRelative).toBe(false); + expect(robot.calculateAngles3D).not.toHaveBeenCalled(); + expect(robot.sendCommand).not.toHaveBeenCalled(); + }); + + test('applyCommand: G1 absolut setzt Koordinaten und bewegt', () => { + const robot = createDummyRobot(); + robot.moveRelative = false; + + RobotController.applyCommand(robot, { command: 'G1', params: { X: 10, Y: 20, Z: 30 } }); + + expect(robot.x).toBe(10); + expect(robot.y).toBe(20); + expect(robot.z).toBe(30); + expect(robot.calculateAngles3D).toHaveBeenCalledTimes(1); + expect(robot.sendCommand).toHaveBeenCalledTimes(1); + }); + + test('applyCommand: G1 relativ addiert', () => { + const robot = createDummyRobot(); + robot.moveRelative = true; + robot.x = 5; + + RobotController.applyCommand(robot, { command: 'G1', params: { X: 2 } }); + + expect(robot.x).toBe(7); + }); + + test('applyCommand: M1 verändert Motorwerte und rechnet vorwärts', () => { + const robot = createDummyRobot(); + robot.moveRelative = false; + + RobotController.applyCommand(robot, { command: 'M1', params: { X: 1, Y: 2, Z: 3, A: 4, B: 5, C: 6 } }); + + expect(robot.xMotor).toBe(1); + expect(robot.alpha).toBe(2); + expect(robot.beta).toBe(3); + expect(robot.a).toBe(4); + expect(robot.calculatePositionFromMotorAngles).toHaveBeenCalled(); + expect(robot.sendCommand).toHaveBeenCalled(); + }); + + test('applyCommand: ungültiger Befehl wird ignoriert', () => { + const robot = createDummyRobot(); + RobotController.applyCommand(robot, null); + RobotController.applyCommand(robot, { command: 'G99', params: {} }); + expect(robot.sendCommand).not.toHaveBeenCalled(); + }); + + test('receive: parst rohe Nachricht und wendet mehrere Befehle an', () => { + const robot = createDummyRobot(); + robot.moveRelative = true; + + RobotController.receive(robot, 'G90 G1 X12 Y34'); + + expect(robot.moveRelative).toBe(false); + expect(robot.x).toBe(12); + expect(robot.y).toBe(34); + expect(robot.sendCommand).toHaveBeenCalledTimes(1); + }); +}); diff --git a/test/Sender.Telnet.speedMode.test.js b/test/Sender.Telnet.speedMode.test.js new file mode 100644 index 0000000..5244c70 --- /dev/null +++ b/test/Sender.Telnet.speedMode.test.js @@ -0,0 +1,175 @@ +const TelnetSender = require('../robot/TelnetSenderGRBL.js'); + +describe('TelnetSenderGRBL — ROBOT_SPEED_MODE (ToDo_6a)', () => { + + beforeAll(() => { + jest.spyOn(console, 'log').mockImplementation(() => {}); + }); + afterAll(() => { + jest.restoreAllMocks(); + }); + + function makeSender() { + const sender = new TelnetSender('test.test', 2300, 'x', 'y', 'z'); + sender.tSocket = { written: '', write(txt) { this.written = txt; } }; + return sender; + } + + // Gemeinsame Bewegungsdaten: x, alpha(=90°), beta bleibt + function makeMotion(extra) { + const mOld = { x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0 }; + const mNew = { + x: 12.34, y: Math.PI / 2, z: 0, a: 0, b: 0, c: 0, e: 0, + xMotorChanged: true, yMotorChanged: true, zMotorChanged: true, + feedrate: 999, + ...extra + }; + return { mOld, mNew }; + } + + test('legacy (Default): kartesische Feedrate, exakt wie bisher', () => { + const sender = makeSender(); + const { mOld, mNew } = makeMotion({ speedMode: 'legacy' }); + + sender.execCommand('G1', mOld, mNew); + + // Achsenwerte wie gehabt, Feedrate = mNew.feedrate + expect(sender.tSocket.written).toBe('G90 G1 x12.34 y90.00 z-90.00 f999.00\r\n'); + }); + + test('ohne speedMode-Feld → ebenfalls Legacy-Verhalten', () => { + const sender = makeSender(); + const { mOld, mNew } = makeMotion(); // kein speedMode + + sender.execCommand('G1', mOld, mNew); + + expect(sender.tSocket.written).toContain('f999.00'); + }); + + test('correct: koordinierte Feedrate ersetzt die kartesische', () => { + const sender = makeSender(); + const { mOld, mNew } = makeMotion({ speedMode: 'correct', moveTime: 0.5 }); + + const expectedF = sender.computeCoordinatedFeedrate(mOld, mNew); + + sender.execCommand('G1', mOld, mNew); + + // Achsenwerte identisch zu Legacy — nur die Feedrate unterscheidet sich + expect(sender.tSocket.written).toContain('x12.34 y90.00 z-90.00'); + expect(sender.tSocket.written).toContain('f' + expectedF.toFixed(2)); + // und sie ist NICHT die kartesische 999 + expect(sender.tSocket.written).not.toContain('f999.00'); + }); + + test('correct ohne moveTime → Fallback auf Legacy-Feedrate', () => { + const sender = makeSender(); + const { mOld, mNew } = makeMotion({ speedMode: 'correct' }); // kein moveTime + + sender.execCommand('G1', mOld, mNew); + + expect(sender.tSocket.written).toContain('f999.00'); + }); + + test('correct ohne Bewegung (alt == neu) → null → Fallback', () => { + const sender = makeSender(); + const { mNew } = makeMotion({ speedMode: 'correct', moveTime: 0.5 }); + + // gleiche Position als alt UND neu → Strecke 0 + expect(sender.computeCoordinatedFeedrate(mNew, mNew)).toBeNull(); + + sender.execCommand('G1', mNew, mNew); + expect(sender.tSocket.written).toContain('f999.00'); + }); + + test('computeCoordinatedFeedrate = Sender-Strecke / moveTime', () => { + const sender = makeSender(); + const { mOld, mNew } = makeMotion({ speedMode: 'correct', moveTime: 0.5 }); + + // erwartete Strecke: x-Port=12.34, y-Port=90, z-Port=(0-90)=-90 + const dist = Math.sqrt(12.34 ** 2 + 90 ** 2 + (-90) ** 2); + const expected = dist / 0.5; + + expect(sender.computeCoordinatedFeedrate(mOld, mNew)).toBeCloseTo(expected, 4); + }); + + test('Hand-Sender (c,e,b) liefert im Korrekt-Modus koordinierte Feedrate', () => { + const hand = new TelnetSender('test.test', 5000, 'c', 'e', 'b'); + hand.tSocket = { written: '', write(t) { this.written = t; } }; + + const mOld = { x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0 }; + const mNew = { + x: 0, y: 0, z: 0, a: 0, b: 0.5, c: 0.8, e: 0.3, + bMotorChanged: true, cMotorChanged: true, eMotorChanged: true, + feedrate: 1234, speedMode: 'correct', moveTime: 0.2 + }; + + const expectedF = hand.computeCoordinatedFeedrate(mOld, mNew); + expect(expectedF).toBeGreaterThan(0); + + hand.execCommand('G1', mOld, mNew); + expect(hand.tSocket.written).toContain('f' + expectedF.toFixed(2)); + expect(hand.tSocket.written).not.toContain('f1234.00'); + }); +}); + +/** + * Kreuzprobe: portValue() muss exakt das liefern, was der echte Sende-Pfad + * (execCommand, Legacy) an den jeweiligen GRBL-Port schreibt. Schützt die in + * portValue dupliziert gehaltenen Formeln gegen Drift. + */ +describe('TelnetSenderGRBL.portValue ↔ Sende-Pfad (Kreuzprobe)', () => { + + beforeAll(() => { + jest.spyOn(console, 'log').mockImplementation(() => {}); + }); + afterAll(() => { + jest.restoreAllMocks(); + }); + + const POS = { x: 1.1, y: 0.3, z: 0.7, a: 0.9, b: 1.3, c: 0.5, e: 0.4 }; + + function mNewAllChanged() { + return { + ...POS, + xMotorChanged: true, yMotorChanged: true, zMotorChanged: true, + aMotorChanged: true, bMotorChanged: true, cMotorChanged: true, eMotorChanged: true, + feedrate: 100 + }; + } + + function axisVal(line, axis) { + const m = line.match(new RegExp(axis + '(-?\\d+(?:\\.\\d+)?)')); + return m ? parseFloat(m[1]) : undefined; + } + + // Baut einen Sender, der genau EINEN GRBL-Port auf eine Roboter-Achse abbildet, + // führt execCommand aus und vergleicht den gesendeten Wert mit portValue(). + function crossCheck(grblPort, robotAxis) { + const ctorArgs = ['test.test', 100, null, null, null, null, null]; + // Position des Ports in der Konstruktor-Signatur: x=2,y=3,z=4,a=5,b=6 + const idx = { x: 2, y: 3, z: 4, a: 5, b: 6 }[grblPort]; + ctorArgs[idx] = robotAxis; + + const sender = new TelnetSender(...ctorArgs); + sender.tSocket = { written: '', write(t) { this.written = t; } }; + + const mNew = mNewAllChanged(); + const mOld = { x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0 }; + sender.execCommand('G1', mOld, mNew); + + const emitted = axisVal(sender.tSocket.written, grblPort); + expect(emitted).toBeDefined(); + expect(emitted).toBeCloseTo(sender.portValue(grblPort, robotAxis, mNew), 2); + } + + // x-Port deckt alle sieben Roboter-Achsen ab (inkl. der kniffligen c/e-Formeln) + for (const r of ['x', 'y', 'z', 'a', 'b', 'c', 'e']) { + test(`x-Port ← robotAxis '${r}'`, () => crossCheck('x', r)); + } + + // Port-abhängige Formel-Varianten gezielt prüfen + test("y-Port ← 'c' (factorTurnLift-Variante)", () => crossCheck('y', 'c')); + test("z-Port ← 'c' (c+b+z-y-Variante)", () => crossCheck('z', 'c')); + test("z-Port ← 'b' (reines b, abweichend vom x-Port)", () => crossCheck('z', 'b')); + test("a-Port ← 'x' (Quirk: nutzt y)", () => crossCheck('a', 'x')); +}); diff --git a/test/Speed.coordination.test.js b/test/Speed.coordination.test.js new file mode 100644 index 0000000..89beff2 --- /dev/null +++ b/test/Speed.coordination.test.js @@ -0,0 +1,102 @@ +/** + * Integrationstest der Speed-Koordination (ToDo_6a). + * + * Kernaussage des Korrekt-Modus: Alle Controller müssen den Bewegungsschritt in + * DERSELBEN Zeit beenden. Dieser Test prüft genau das — komplett unabhängig von der + * internen Berechnung: er liest die tatsächlich gesendeten G-Code-Zeilen, rechnet aus + * den absoluten Achswerten die Strecke jedes Senders aus und teilt durch die gesendete + * Feedrate. Das Ergebnis (= Zeit) muss für jeden Sender gleich der robot-moveTime sein. + * + * Damit werden gleichzeitig abgesichert: moveTime-Propagation Robot→Sender, portValue + * gegen den echten Sende-Pfad, und die Feedrate-Auswahl im Korrekt-Modus. + */ +const Robot = require('../robot/Robot'); +const GCode = require('../robot/GCode'); +const TelnetSender = require('../robot/TelnetSenderGRBL'); + +function parseLine(line) { + const out = {}; + const re = /([a-z])(-?\d+(?:\.\d+)?)/g; + let m; + while ((m = re.exec(line)) !== null) out[m[1]] = parseFloat(m[2]); + return out; +} + +function makeSender(...args) { + const s = new TelnetSender(...args); // "test.test" → isTestMode + s.writes = []; + s.tSocket = { write(t) { s.writes.push(t); } }; + return s; +} + +// Produktiv-Verkabelung wie in startRobot.js +function buildRobot(speedMode) { + const robot = new Robot(250, 264, 100); + robot.speedMode = speedMode; + robot.useSpeedCalc = (speedMode === 'correct'); + robot.moveRelative = false; // M1 absolut + + const base = makeSender('test.test', 2300, 'x', 'y', 'z'); + const elbow = makeSender('test.test', 5000, 'a', null, null); + const hand = makeSender('test.test', 5000, 'c', 'e', 'b'); + robot.cmdReceivers.push(base, elbow, hand); + + return { robot, base, elbow, hand }; +} + +// drei absolute Motor-Stellungen; Schritt 0 = Warmup (erster Aufruf → Fallback) +function runMoves(robot) { + GCode.receiveGCode(robot, 'M1 X5 Y0.10 Z0.20 A0.30 B0.40 C0.50'); // 0 warmup + GCode.receiveGCode(robot, 'M1 X10 Y0.20 Z0.30 A0.40 B0.50 C0.60'); // 1 baseline + GCode.receiveGCode(robot, 'M1 X22 Y0.45 Z0.50 A0.65 B0.85 C0.90'); // 2 gemessen +} + +describe('Speed-Koordination (ToDo_6a)', () => { + beforeAll(() => { jest.spyOn(console, 'log').mockImplementation(() => {}); }); + afterAll(() => { jest.restoreAllMocks(); }); + + test('correct: alle Sender beenden den Schritt in gleicher Zeit (= moveTime)', () => { + const { robot, base, elbow, hand } = buildRobot('correct'); + runMoves(robot); + + const moveTime = robot.motorPosition.moveTime; + expect(moveTime).toBeGreaterThan(0); + + for (const s of [base, elbow, hand]) { + const v1 = parseLine(s.writes[1]); // baseline + const v2 = parseLine(s.writes[2]); // gemessen + const axes = Object.keys(v2).filter(k => k !== 'f'); + + let sumSq = 0; + for (const ax of axes) sumSq += (v2[ax] - v1[ax]) ** 2; + const dist = Math.sqrt(sumSq); + + const time = dist / v2.f; // unabhängig aus Output berechnet + // jeder Controller braucht dieselbe Zeit (Rundung auf 2 Dezimalen → 2% Toleranz) + expect(Math.abs(time - moveTime) / moveTime).toBeLessThan(0.02); + } + }); + + test('correct: die Sender bekommen UNTERSCHIEDLICHE Feedrates', () => { + const { robot, base, elbow, hand } = buildRobot('correct'); + runMoves(robot); + + const fBase = parseLine(base.writes[2]).f; + const fElbow = parseLine(elbow.writes[2]).f; + const fHand = parseLine(hand.writes[2]).f; + + // Genau das ist der Unterschied zu legacy: nicht alle gleich. + expect(fBase).not.toBeCloseTo(fElbow, 2); + expect(fBase).not.toBeCloseTo(fHand, 2); + }); + + test('legacy (Kontrast): alle Sender bekommen dieselbe kartesische Feedrate', () => { + const { robot, base, elbow, hand } = buildRobot('legacy'); + runMoves(robot); + + // robot.feedrate ist 1000 (kein F in M1) → alle Sender f1000.00 + expect(parseLine(base.writes[2]).f).toBeCloseTo(1000, 2); + expect(parseLine(elbow.writes[2]).f).toBeCloseTo(1000, 2); + expect(parseLine(hand.writes[2]).f).toBeCloseTo(1000, 2); + }); +});