From 7205b9d913697e1a3c6de19043c0e4ecc4645f3c Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Fri, 26 Jun 2026 08:27:50 +0200 Subject: [PATCH] y war falsch --- doc/{ToDo_3_Config.md => Info_Config.md} | 0 doc/Info_G92.md | 18 +-- doc/Info_Koordinaten.md | 149 +++++++++++++++++++++++ doc/ToDo_1_Parsing.md | 21 ---- doc/ToDo_8_Bugs.md | 98 --------------- logs/gcode_commands.log | 90 ++++++++++++++ logs/pings.log | 20 +++ robot/RobotController.js | 6 +- robot/kinematics/Arm3SegmentLinearX.js | 36 +++++- test/GCode.receiveGCode.test.js | 4 +- test/Robot.02_UpperArm.test.js | 14 +-- test/Robot.Kinematics.NegativeY.test.js | 66 ++++++++++ 12 files changed, 382 insertions(+), 140 deletions(-) rename doc/{ToDo_3_Config.md => Info_Config.md} (100%) create mode 100644 doc/Info_Koordinaten.md delete mode 100644 doc/ToDo_1_Parsing.md delete mode 100644 doc/ToDo_8_Bugs.md create mode 100644 test/Robot.Kinematics.NegativeY.test.js diff --git a/doc/ToDo_3_Config.md b/doc/Info_Config.md similarity index 100% rename from doc/ToDo_3_Config.md rename to doc/Info_Config.md diff --git a/doc/Info_G92.md b/doc/Info_G92.md index 0da1306..175efd9 100644 --- a/doc/Info_G92.md +++ b/doc/Info_G92.md @@ -48,12 +48,15 @@ G92 X158.14 Y4.19 Z57.74 A91.85 B-45.46 C-69.92 E21.20 ### Y = α (Oberarm) und Z = β (Unterarm) — ABSOLUT -Beide werden **absolut gegen die Horizontale (+y)** gemessen, **nicht** relativ zueinander: +Beide werden **absolut gegen die Horizontale** gemessen, **nicht** relativ zueinander. +Seit **Phase 1** (Weg 2, siehe doc/Info_Koordinaten.md) zeigt **0° nach −y** +(Arbeitsrichtung); verifiziert: `FK(α=0, β=0, gerade Hand) → y = −590`. -| Wert | Oberarm (Y) bzw. Unterarm (Z) | -|------|-------------------------------| -| 0° | waagerecht nach +y | -| 90° | senkrecht nach oben | +| Wert | Oberarm (Y) bzw. Unterarm (Z) | +|-------|----------------------------------------| +| 0° | waagerecht nach **−y** (Grundstellung) | +| 90° | senkrecht nach oben | +| 180° | waagerecht nach +y | ⚠️ **Z ist der absolute Unterarmwinkel**, nicht der Ellbogen-Knick gegen den Oberarm. Misst appRobotHoming den Ellbogen relativ zum Oberarm: `Z = Oberarmwinkel + Ellbogen_relativ`. @@ -75,8 +78,9 @@ Der Driver (IK) erzeugt B nur im Bereich [0°, 180°]. ### A = Unterarm-Dreher (Ellbogen-Roll) A dreht die **Richtung**, in die das Handgelenk knickt, um die Unterarm-Längsachse — -die Knick-**Größe** bleibt dabei gleich. Verifiziert (α=0, β=90, B=90, C=0): -A=0 → Fingerspitze auf der −y-Seite, A=90 → −x-Seite, A=180 → +y-Seite; Knick konstant 90°. +die Knick-**Größe** bleibt dabei gleich. Verifiziert (α=0, β=90, B=90, C=0), nach Phase 1: +A=0 → Fingerspitze Richtung Schulter (y=−160), A=90 → −x-Seite (x=−90), +A=180 → von der Schulter weg (y=−340); Knick konstant 90°. ### C = Hand-Dreher (Roll) diff --git a/doc/Info_Koordinaten.md b/doc/Info_Koordinaten.md new file mode 100644 index 0000000..2ad61c7 --- /dev/null +++ b/doc/Info_Koordinaten.md @@ -0,0 +1,149 @@ +# Koordinatensystem, Roboter-Aufstellung & Nullstellung + +Diese Datei beschreibt +1. das Koordinatensystem, +2. wie der Roboter darin steht, +3. die angestrebte **ideale Nullstellung** und +4. die nötigen Schritte, um den Driver auf diese Konvention zu bringen (**Weg 2: + Modell auf −Y drehen**). + +> **Hintergrund / Problem:** Das Kinematik-Modell (`Arm3SegmentLinearX`) misst die +> Armwinkel aktuell von **+Y** (α=0 → Arm zeigt nach +y). Der reale Roboter steht und +> arbeitet aber in **−Y** (robot.json: `Arm1.skeleton.to = [0,-250,0]`, +> `coordinateSystem.y = "backward"`). Dadurch landet eine fast-waagerechte +> Grundstellung im Driver bei **y ≈ +590** statt **−590** — Modell und Hardware sind an +> der y-Achse gespiegelt. + +--- + +## 1. Koordinatensystem + +Aus robot.json (`coordinateSystem`): **rechtshändig**, Längen **mm**, Winkel **Grad**. + +| Achse | Richtung | Bedeutung im Aufbau | +|-------|------------|-------------------------------------------| +| x | rechts | entlang der Linearschiene | +| y | „backward" | Arm-Arbeitsrichtung: Arm streckt nach **−y** | +| z | oben | Höhe | + +``` +Seitenansicht (Blick entlang +x): + + z + ▲ + | ■══════════════════════● Arm waagerecht ausgestreckt → Fingerspitze + | Schulter (Ursprung, z=0) + └───────────────────────────────► −y (Arbeitsrichtung) + +Draufsicht (Blick von oben, −z): + + −y ▲ ● Fingerspitze (Grundstellung y ≈ −590) + │ │ + │ │ Arm (Ober- + Unterarm, gestreckt) + │ │ + ───┼────────────────■─────────────► x (Schiene) + │ Schulter + +y │ +``` + +--- + +## 2. Wie der Roboter darin steht + +Gelenk-Kette (robot.json `links`): +**Board → Base (Schiene x) → Arm1 (Oberarm) → Ellbow → Arm2 (Unterarm) → Hand → Palm → Finger.** + +- **Linearschiene** entlang x; die Base fährt darauf (`xMotor`, mm). +- **Schultergelenk** (Base→Arm1, `variable y`): Drehung in der y-z-Ebene → **α**. + Bei Gelenkwinkel 0 zeigt Arm1 entlang **−y** (`skeleton.to = [0,-250,0]`). +- **Ellbogen** (`variable z`) → **β**; **Unterarm-Dreher** (`variable a`) → **a**; + **Handgelenk-Knick** (`variable b`) → **b**; **Hand-Roll** (`variable c`) → **c**; + **Greifer** (`variable e`) → **e**. +- **z = 0 im Driver-Modell = Schulterachse.** (In der Welt liegt sie ~45 mm über dem + Brett — robot.json Joint1-Origin z=45 —, der Driver rechnet schulter-relativ.) +- Armlängen (aus `links` abgeleitet): **l1 = 250** (Oberarm), **l2 = 250** (Unterarm), + **l3 ≈ 90** (Hand). Σ ≈ **590 mm**. + +--- + +## 3. Ideale Nullstellung (Grundstellung) + +**Definition:** Arm **waagerecht voll ausgestreckt entlang −y**, Hand **gerade** in +Verlängerung des Unterarms, Greifer geschlossen. + +**Ziel:** in dieser Stellung sind **alle Gelenkwinkel = 0°**. + +| Achse | Ideal-Wert | Bedeutung | +|--------|-----------|-------------------------------------------| +| X | (frei) | Schienenposition `xMotor` in mm | +| Y (α) | **0°** | Oberarm waagerecht entlang −y | +| Z (β) | **0°** | Unterarm waagerecht entlang −y (gestreckt) | +| A (a) | **0°** | kein Unterarm-Dreh | +| B (b) | **0°** | Hand gerade | +| C (c) | **0°** | kein Hand-Roll | +| E (e) | **0** | Greifer geschlossen / Referenz | + +→ Resultierende Fingerspitze: **(xMotor, −(l1+l2+l3), 0) ≈ (x, −590, 0)**. +(Beobachtet ~−550; Differenz steckt in der l3-Ableitung / Resthandstellung.) + +--- + +## 4. Ist-Zustand vs. Ideal + +| Aspekt | aktuell (Modell +Y) | ideal (−Y) | +|----------------------------|---------------------------|-------------------| +| α=0 zeigt nach | +y (FK: y=+410) | −y (y=−410) | +| Grundstellung Y (α) | ≈175° (=180−4,5) | **0°** | +| gerade Hand B (b) | 180° | **0°** | +| neutraler Roll C (c) | 90° (posenabhängig) | **0°** | +| G92 der Grundstellung → y | ≈ +590 | **≈ −590** | + +Verifiziert per FK: `α=β=0 → y=+410`, `α=β=180 → y=−410`. Die +Y/−Y-Spiegelung +entspricht `α→180−α, β→180−β`. + +--- + +## 5. Schritte, um das zu erreichen (Weg 2) + +Reihenfolge nach Workflow: **erst Tests (rot), dann Code, dann grün.** + +### Phase 1 — y-Flip (behebt den gemeldeten Bug) + +1. **Tests schreiben** (zunächst rot): + - G92 der Grundstellung (α=β=a=b=c=0) → erwartet **y ≈ −590** (statt +590). + - Round-Trip: `IK(x=0, y=−550, z, φ, θ)` → `FK` → identische Pose zurück. +2. **`Arm3SegmentLinearX` umstellen** (α/β von −y aus messen): + - **FK** (`calculatePositionFromMotorAngles`): y-Komponenten von `vecBizeps` und + `vecUnterarm` negieren; Handgelenk-Vektoren (`n`, `vHand`) und `φ = atan2(vHand.y,…)` + konsistent nachziehen. + - **IK** (`calculateAngles3D`): `pY`, `gamma = atan2(pZ, pY)`, die `n`-Konstruktion + und `φ` entsprechend spiegeln. +3. **Bestehende +Y-Tests auf −Y migrieren** (erwartete y-Werte spiegeln). +4. **`doc/Info_G92.md`** aktualisieren: Y/Z-Konvention auf „0° = waagerecht entlang −y". +5. Suite grün; Round-Trip + Grundstellung grün. + +### Phase 2 — Handgelenk auf Null (optional, für echte All-Zero-Grundstellung) + +6. **B-Konvention drehen:** gerade Hand = **0°** statt 180° (`b → 180°−b` an der + Schnittstelle; FK/IK + Greifer-Kopplung `eMotor = e − b − c` mit anpassen). +7. **C-Nullpunkt:** neutral = **0°** statt 90°. + ⚠️ Der C↔ψ-Offset ist **posenabhängig** (`acos(cos β · sin a)`); ein global sauberes + `c=0=neutral` braucht ggf. eine tiefere Umparametrierung des Handgelenks — **vor** + der Umsetzung bewerten. +8. Tests + `Info_G92.md` nachziehen. + +### Verifikation (Definition of Done) + +- G92 der Grundstellung (alle Winkel 0) → Driver meldet **y ≈ −590, z ≈ 0**. +- appRobotHoming kann die **physisch gemessenen Winkel direkt** senden (ohne Spiegelung). +- Volle Test-Suite grün, inkl. Round-Trip und Grundstellungs-Tests. + +--- + +## Anhang: Stand der bisherigen Arbeit + +- Der atan2-Fix in der IK (`gamma = Math.atan2(pZ, pY)`, [Arm3SegmentLinearX.js:59](../robot/kinematics/Arm3SegmentLinearX.js#L59)) + ist umgesetzt und macht die IK mathematisch für −Y-Eingaben korrekt — eine + Voraussetzung für Phase 1. Er betrifft **nicht** den G92/FK-Pfad. +- Die Winkel-Konventionen (B/C/E) sind in [doc/Info_G92.md](Info_G92.md) dokumentiert; + die Y/Z-Konvention dort wird in Phase 1, Schritt 4 angepasst. diff --git a/doc/ToDo_1_Parsing.md b/doc/ToDo_1_Parsing.md deleted file mode 100644 index 4020a19..0000000 --- a/doc/ToDo_1_Parsing.md +++ /dev/null @@ -1,21 +0,0 @@ -# ToDo 1 — Parsing - -## Ziel der Verbesserung - -Klare Trennung zwischen G-Code-Parsing und Robotersteuerlogik. Der Parser soll nur lesen und strukturieren, nicht direkt den Roboterzustand verändern. - -## Aufgaben - -- [x] `GCodeParser` einführen, das G-Code und Nachrichten in strukturierte Befehlsobjekte übersetzt -- [x] Parsing-Regeln definieren für `G90`, `G91`, `G1`, `G28`, `G92`, `M1` und `$J=` sowie Parameter `X`, `Y`, `Z`, `A`, `B`, `C`, `E`, `F` -- [x] Raw-String-Verarbeitung aus `GCode.receiveGCode()` entfernen -- [x] Parser-Resultate als Objekte an den Controller übergeben, nicht als rohe Textbefehle -- [x] Parser-Fehlerfälle klar behandeln: ungültige Syntax, fehlende Werte, unbrauchbare Befehle - -## Status - -- [x] Implementierung abgeschlossen -- [x] Tests erfolgreich: `npx jest --runInBand` - - -Erledigt von VStudio Chatbot unter Aufsicht ChK \ No newline at end of file diff --git a/doc/ToDo_8_Bugs.md b/doc/ToDo_8_Bugs.md deleted file mode 100644 index c9887f9..0000000 --- a/doc/ToDo_8_Bugs.md +++ /dev/null @@ -1,98 +0,0 @@ -# ToDo 8 — Bekannte Bugs - -## Ziel - -Konkrete, im Code identifizierte Fehler beheben — unabhängig von den Architektur-Refactorings in den anderen ToDo-Dateien. - ---- - -## Bug 1: `TelnetSenderGRBL` — `close`-Event verliert `this`-Kontext ✅ ERLEDIGT - -> Behoben im ToDo-2-Refactoring: Der `close`-Handler nutzt jetzt eine Arrow-Function -> (`robot/TelnetSenderGRBL.js`), `this` zeigt korrekt auf die Sender-Instanz. - -**Datei:** `robot/TelnetSenderGRBL.js`, Zeile 54–57 - -**Problem:** Das `close`-Event verwendet eine reguläre `function()` statt einer Arrow Function. Dadurch zeigt `this` innerhalb des Handlers auf das EventEmitter-Objekt, nicht auf die `TelnetSenderGRBL`-Instanz. `this.tSocket = null` hat keinen Effekt — nach einer Verbindungstrennung bleibt `tSocket` auf dem alten, ungültigen Objekt. - -```js -// Falsch: -this.tSocket.on("close", function () { - this.tSocket = null; // 'this' ist hier NICHT TelnetSenderGRBL -}); - -// Richtig: -this.tSocket.on("close", () => { - this.tSocket = null; -}); -``` - ---- - -## Bug 2: `FFirst` und `FLast` sind nicht implementiert - -**Datei:** `robot/GCode.js` - -**Problem:** `ContainsFilesCommand()` erkennt `FFirst` und `FLast` und leitet sie an `receiveFC()` weiter. `receiveFC()` behandelt sie aber nicht — die Befehle werden stillschweigend ignoriert und es wird nur `getM114` zurückgegeben. - -**Erwartetes Verhalten:** -- `FFirst` — Cursor auf den ersten Eintrag der Log-Datei setzen und die Position anfahren -- `FLast` — Cursor auf den letzten Eintrag setzen und die Position anfahren - ---- - -## Bug 3: G92/M92-Mismatch - -**Datei:** `robot/GCode.js` - -**Problem:** `containsCommand()` erkennt `G92`, aber `receiveGCode()` prüft auf `g[0] == "M92"`. Ein eingehender Befehl `G92 X10` wird als G-Code erkannt, fällt dann aber durch alle Bedingungen in `receiveGCode()`, und löst unbeabsichtigt `calculateAngles3D()` + `sendCommand()` aus, ohne die Position zu setzen. - -**Klärungsbedarf:** Ist G92 oder M92 der korrekte Eingabe-Befehl? Beides konsistent machen. - ---- - -## Bug 4: `logs/`-Verzeichnis wird nicht sichergestellt ✅ ERLEDIGT - -> Behoben: `initInputWS()` ruft `ensureLogDir()` (`fs.mkdirSync('./logs', { recursive: true })`) -> beim Start auf. `ensureLogDir` ist exportiert und idempotent. -> Test: `test/InputWS.logDir.test.js`. - -**Datei:** `server/InputWS.js`, Zeilen 66–67 und 77–78 - -**Problem:** `fs.appendFileSync('./logs/gcode_commands.log', ...)` und `fs.appendFileSync('./logs/pings.log', ...)` crashen beim ersten Aufruf, wenn das `logs/`-Verzeichnis nicht existiert. - -**Fix:** Beim Start `fs.mkdirSync('./logs', { recursive: true })` aufrufen, z. B. in `startRobot.js` oder am Anfang von `initInputWS`. - ---- - -## Bug 5: Falscher Finitude-Check in `TelnetSenderGRBL.execCommand` - -**Datei:** `robot/TelnetSenderGRBL.js`, Zeile 161 - -**Problem:** -```js -if(this.aAxisGrbl == "x" && mNew.xMotorChanged && Number.isFinite(mNew.y)){ -``` -Der Check prüft `mNew.y` statt `mNew.x`. Wenn `mNew.x` `NaN` oder `Infinity` wäre, würde das trotzdem durchgehen. - ---- - -## Bug 6: `containsMCode` matcht zu breit ✅ ERLEDIGT - -> Behoben: `containsMCode` nutzt jetzt `s === 'M1' || s.startsWith('M1 ')`. -> Test: `test/GCode.containsMCode.test.js`. -> (Hinweis bleibt: Methode wird im Produktivcode noch nicht aufgerufen.) - -**Datei:** `robot/GCode.js`, Zeile 12 - -**Problem:** `s.indexOf('M1') == 0` trifft auch auf `M10`, `M11`, `M12` usw. zu. - -```js -// Aktuell: -static containsMCode(s){ return s.indexOf('M1') == 0 } - -// Präziser: -static containsMCode(s){ return s === 'M1' || s.startsWith('M1 ') } -``` - -Hinweis: Diese Methode wird im aktuellen Code nicht aufgerufen — sie hat keine Wirkung, ist aber irreführend. diff --git a/logs/gcode_commands.log b/logs/gcode_commands.log index c5f478a..f7b5643 100644 --- a/logs/gcode_commands.log +++ b/logs/gcode_commands.log @@ -11038,3 +11038,93 @@ 2026-06-26T04:36:17.472Z ::ffff:127.0.0.1: M114 2026-06-26T04:36:17.716Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 2026-06-26T04:36:17.971Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:18:31.826Z ::ffff:127.0.0.1: M114 +2026-06-26T06:18:31.879Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:18:31.899Z ::ffff:127.0.0.1: FList +2026-06-26T06:18:31.947Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:18:31.976Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:18:31.997Z ::ffff:127.0.0.1: FShow +2026-06-26T06:18:32.115Z ::ffff:127.0.0.1: M114 +2026-06-26T06:18:32.356Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:18:32.614Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:20:45.304Z ::ffff:127.0.0.1: M114 +2026-06-26T06:20:45.334Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:20:45.336Z ::ffff:127.0.0.1: FList +2026-06-26T06:20:45.381Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:20:45.398Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:20:45.411Z ::ffff:127.0.0.1: FShow +2026-06-26T06:20:45.539Z ::ffff:127.0.0.1: M114 +2026-06-26T06:20:45.769Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:20:46.021Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:21:16.481Z ::ffff:127.0.0.1: FList +2026-06-26T06:21:16.529Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:21:16.537Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:16.548Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:21:16.560Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:16.567Z ::ffff:127.0.0.1: FShow +2026-06-26T06:21:16.765Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:17.013Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:17.267Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:21:22.686Z ::ffff:127.0.0.1: FList +2026-06-26T06:21:22.742Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:21:22.759Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:21:22.782Z ::ffff:127.0.0.1: FShow +2026-06-26T06:21:22.918Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:23.161Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:23.179Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:23.201Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:23.437Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:21:27.584Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:27.607Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:27.776Z ::ffff:127.0.0.1: FList +2026-06-26T06:21:27.827Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:21:27.846Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:21:27.859Z ::ffff:127.0.0.1: FShow +2026-06-26T06:21:27.938Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:28.209Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:28.470Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:21:32.539Z ::ffff:127.0.0.1: FList +2026-06-26T06:21:32.593Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:21:32.617Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:21:32.636Z ::ffff:127.0.0.1: FShow +2026-06-26T06:21:32.677Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:32.696Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:32.907Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:33.145Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:33.394Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:21:49.876Z ::ffff:127.0.0.1: FList +2026-06-26T06:21:49.917Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:21:49.927Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:49.936Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:21:49.943Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:49.954Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:49.962Z ::ffff:127.0.0.1: FShow +2026-06-26T06:21:50.224Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:50.481Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:21:53.338Z ::ffff:127.0.0.1: FList +2026-06-26T06:21:53.386Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:21:53.406Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:21:53.422Z ::ffff:127.0.0.1: FShow +2026-06-26T06:21:53.525Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:53.556Z ::ffff:127.0.0.1: M114 +2026-06-26T06:21:53.580Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:53.772Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:21:54.016Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:23:06.580Z ::ffff:127.0.0.1: FList +2026-06-26T06:23:06.624Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:23:06.647Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:23:06.666Z ::ffff:127.0.0.1: FShow +2026-06-26T06:23:06.693Z ::ffff:127.0.0.1: M114 +2026-06-26T06:23:06.714Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:23:06.966Z ::ffff:127.0.0.1: M114 +2026-06-26T06:23:07.218Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:23:07.463Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T06:25:53.094Z ::ffff:127.0.0.1: FList +2026-06-26T06:25:53.137Z ::ffff:127.0.0.1: M114 +2026-06-26T06:25:53.152Z ::ffff:127.0.0.1: FPlus +2026-06-26T06:25:53.163Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:25:53.174Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T06:25:53.196Z ::ffff:127.0.0.1: FShow +2026-06-26T06:25:53.335Z ::ffff:127.0.0.1: M114 +2026-06-26T06:25:53.565Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T06:25:53.811Z ::ffff:127.0.0.1: G1 X1 diff --git a/logs/pings.log b/logs/pings.log index 0b54aa7..6d712cb 100644 --- a/logs/pings.log +++ b/logs/pings.log @@ -14814,3 +14814,23 @@ 2026-06-26T04:00:23.430Z ::ffff:127.0.0.1 : Ping 2026-06-26T04:36:17.220Z ::ffff:127.0.0.1 : Ping 2026-06-26T04:36:17.238Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:18:31.738Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:18:31.869Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:20:45.255Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:20:45.281Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:16.495Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:16.497Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:22.653Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:23.102Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:27.537Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:27.675Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:32.633Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:32.640Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:49.688Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:49.886Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:53.274Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:21:53.507Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:23:06.652Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:23:06.656Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:25:53.069Z ::ffff:127.0.0.1 : Ping +2026-06-26T06:25:53.087Z ::ffff:127.0.0.1 : Ping diff --git a/robot/RobotController.js b/robot/RobotController.js index ff26e95..77e7faa 100644 --- a/robot/RobotController.js +++ b/robot/RobotController.js @@ -52,10 +52,12 @@ class RobotController { } if (cmd === 'G28') { + // Home = Grundstellung: Arm voll ausgestreckt entlang -y (siehe + // doc/Info_Koordinaten.md). y und phi in der -y-Konvention. robot.x = 0; - robot.y = robot.l1 + robot.l2 + robot.l3; + robot.y = -(robot.l1 + robot.l2 + robot.l3); robot.z = 0; - robot.phi = -Math.PI / 2; + robot.phi = Math.PI / 2; robot.theta = Math.PI / 2; robot.psi = 0; robot.e = 0; diff --git a/robot/kinematics/Arm3SegmentLinearX.js b/robot/kinematics/Arm3SegmentLinearX.js index 3aee2bf..96d37cb 100644 --- a/robot/kinematics/Arm3SegmentLinearX.js +++ b/robot/kinematics/Arm3SegmentLinearX.js @@ -33,8 +33,38 @@ class Arm3SegmentLinearX extends RobotBase { this.l3 = l3; } - // Berechnet aus XYZ die Motor-Winkel für den GCode - calculateAngles3D(verbose){ + // ───────────────────────────────────────────────────────────────────── + // Y-Konvention: Der reale Roboter steht/arbeitet in -y (robot.json: + // Arm1 -> [0,-250,0], coordinateSystem.y = "backward"). Die interne + // Kinematik (_ikPlusY/_fkPlusY) rechnet historisch in +y. Beide + // öffentlichen Methoden spiegeln daher die Workspace-Pose an der x-z-Ebene + // (y, pY, phi, psi; theta bleibt), sodass alpha=0 nach -y zeigt. + // Siehe doc/Info_Koordinaten.md (Weg 2, Phase 1). + // ───────────────────────────────────────────────────────────────────── + + /** Reflexion der Workspace-Pose an der x-z-Ebene (Involution: zweimal = Identität). */ + _mirrorWorkspaceY() { + this.y = -this.y; + this.pY = -this.pY; + this.phi = -this.phi; + this.psi = -this.psi; + } + + calculateAngles3D(verbose) { + // -y-Eingabe in den internen +y-Frame spiegeln, rechnen, dann die + // Workspace-Felder zurückspiegeln (Motorwerte bleiben unberührt). + this._mirrorWorkspaceY(); + this._ikPlusY(verbose); + this._mirrorWorkspaceY(); + } + + calculatePositionFromMotorAngles(verbose = false) { + this._fkPlusY(verbose); + this._mirrorWorkspaceY(); // +y-Ergebnis -> -y Workspace + } + + // Berechnet aus XYZ die Motor-Winkel für den GCode (interne +y-Mathematik) + _ikPlusY(verbose){ while(this.phi > Math.PI){this.phi -= 2*Math.PI} while(this.phi < -Math.PI){this.phi += 2*Math.PI} while(this.theta > Math.PI){this.theta -= 2*Math.PI} @@ -108,7 +138,7 @@ class Arm3SegmentLinearX extends RobotBase { return e - this.b - this.c; } - calculatePositionFromMotorAngles(verbose = false) { + _fkPlusY(verbose = false) { const vecBizeps = {x: this.xMotor, y: this.l1 * Math.cos(this.alpha), z: this.l1 * Math.sin(this.alpha)} const vecUnterarm = {x: 0, y: Math.cos(this.beta), z: Math.sin(this.beta)} diff --git a/test/GCode.receiveGCode.test.js b/test/GCode.receiveGCode.test.js index 6e65323..6f19d68 100644 --- a/test/GCode.receiveGCode.test.js +++ b/test/GCode.receiveGCode.test.js @@ -97,8 +97,8 @@ describe('GCode.receiveGCode', () => { expect(robot.x).toBe(0) expect(robot.z).toBe(0) - expect(robot.y).toBe(robot.l1 + robot.l2 + robot.l3) - expect(robot.phi).toBeCloseTo(-Math.PI / 2) + expect(robot.y).toBe(-(robot.l1 + robot.l2 + robot.l3)) // -y Grundstellung + expect(robot.phi).toBeCloseTo(Math.PI / 2) expect(robot.theta).toBeCloseTo(Math.PI / 2) expect(robot.calculateAngles3D).toHaveBeenCalledTimes(1) expect(robot.sendCommand).toHaveBeenCalledTimes(1) diff --git a/test/Robot.02_UpperArm.test.js b/test/Robot.02_UpperArm.test.js index a2d7f04..a605f28 100755 --- a/test/Robot.02_UpperArm.test.js +++ b/test/Robot.02_UpperArm.test.js @@ -4,16 +4,16 @@ test('Grade ausgestreckt', () => { robot = new Robot(300,290,10) robot.x = 0 ; - robot.y = 600; + robot.y = -600; // -y: voll ausgestreckte Grundstellung robot.z = 0; - robot.phi = -Math.PI/2; + robot.phi = Math.PI/2; // gespiegelt zu -y (siehe doc/Info_Koordinaten.md) robot.theta = Math.PI/2; - + robot.calculateAngles3D(); expect(robot.pX).toBeLessThanOrEqual(0.00001) - expect(robot.pY).toBe(590) + expect(robot.pY).toBe(-590) expect(robot.pZ).toBeLessThanOrEqual(0.00001) expect(robot.alpha).toBeLessThanOrEqual(0.00001) @@ -24,9 +24,9 @@ test('Grade gewinkelt', () => { robot = new Robot(300,290,10) robot.x = 0 ; - robot.y = 300; + robot.y = -300; // -y robot.z = 0; - robot.phi = -Math.PI/2; + robot.phi = Math.PI/2; // gespiegelt zu -y robot.theta = Math.PI/2 - Math.PI/3; @@ -43,7 +43,7 @@ test('schräg gewinkelt 1', () => { robot = new Robot(300,300,10) robot.x = 0 ; - robot.y = 310; + robot.y = -310; // -y (phi=0 ist spiegel-invariant) robot.z = 0; robot.phi = 0; robot.theta = Math.PI/2; diff --git a/test/Robot.Kinematics.NegativeY.test.js b/test/Robot.Kinematics.NegativeY.test.js new file mode 100644 index 0000000..54950f1 --- /dev/null +++ b/test/Robot.Kinematics.NegativeY.test.js @@ -0,0 +1,66 @@ +// Phase 1: Der reale Roboter arbeitet in -Y (robot.json: Arm1 -> [0,-250,0]). +// alpha=0 muss nach -y zeigen, nicht nach +y. Siehe doc/Info_Koordinaten.md. +const Robot = require('../robot/kinematics/Arm3SegmentLinearX'); +const D = 180 / Math.PI; + +describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => { + beforeAll(() => jest.spyOn(console, 'log').mockImplementation(() => {})); + afterAll(() => jest.restoreAllMocks()); + + const L1 = 250, L2 = 250, L3 = 90; + + function fkFromMotors(alphaDeg, betaDeg, aDeg, bDeg, cDeg, xMotor = 0) { + const r = new Robot(L1, L2, L3); + r.xMotor = xMotor; + r.alpha = alphaDeg / D; r.beta = betaDeg / D; + r.a = aDeg / D; r.b = bDeg / D; r.c = cDeg / D; + r.calculatePositionFromMotorAngles(); + return r; + } + + test('voll ausgestreckt (alpha=beta=0, Hand gerade) -> y ~ -590', () => { + // B=180 = aktuelle "gerade Hand"-Konvention (Phase 2 macht daraus 0) + const r = fkFromMotors(0, 0, 0, 180, 0); + expect(r.y).toBeLessThan(0); + expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 0); // ~ -590 + expect(r.z).toBeCloseTo(0, 6); + }); + + test('gemeldete Homing-Pose landet in -y (war faelschlich +405)', () => { + // G92 X160.53 Y4.53 Z13.93 A124.04 (B=C=0) + const r = fkFromMotors(4.53, 13.93, 124.04, 0, 0, 160.53); + expect(r.y).toBeLessThan(0); + expect(r.y).toBeCloseTo(-405, 0); + expect(r.z).toBeCloseTo(58, 0); + }); + + test('IK der -y-Grundstellung liefert alpha~0 / beta~0 (nicht ~180)', () => { + const ref = fkFromMotors(0, 0, 0, 180, 0); // -y-ausgestreckte Pose als Referenz + const r = new Robot(L1, L2, L3); + r.x = ref.x; r.y = ref.y; r.z = ref.z; + r.phi = ref.phi; r.theta = ref.theta; r.psi = ref.psi; r.e = 0; + r.calculateAngles3D(); + expect(r.alpha).toBeCloseTo(0, 3); + expect(r.beta).toBeCloseTo(0, 3); + }); + + test('Round-Trip im -y-Arbeitsraum bleibt konsistent', () => { + const A = new Robot(L1, L2, L3); + A.x = 10; A.y = -430; A.z = 30; + A.phi = Math.PI / 7; A.theta = Math.PI / 2; A.psi = Math.PI / 6; A.e = 0; + A.calculateAngles3D(); + + const B = new Robot(L1, L2, L3); + B.xMotor = A.xMotor; B.alpha = A.alpha; B.beta = A.beta; + B.a = A.a; B.b = A.b; B.c = A.c; + B.calculatePositionFromMotorAngles(); + + const EPS = 4; + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + expect(B.phi).toBeCloseTo(A.phi, EPS); + expect(B.theta).toBeCloseTo(A.theta, EPS); + expect(B.psi).toBeCloseTo(A.psi, EPS); + }); +});