# ToDo 9a — Umkehr-Rechnung GRBL-Port → Motorwerte (Herleitung)

> **Status: durchgerechnet und verifiziert.**
> Baustein für ToDo_9 / Paket 4 (Sync-Command). Verifikation:
> `test/Robot.PortInverse.test.js` (15 Tests grün).

## Zweck

Der Sync-Command (ToDo_9, Paket 4) liest die echten Achs-Positionen (`MPos`) der drei
GRBL/FluidNC-Controller und muss daraus die **sieben Motorwerte** des Roboters
rekonstruieren: `xMotor, alpha, beta, a, b, c, eMotor`. Diese werden dann auf den Roboter
geschrieben und per **Vorwärtskinematik** (`calculatePositionFromMotorAngles()`) in die
Pose `x/y/z/phi/theta/psi` überführt.

Dieses Dokument leitet die Umkehrung her und hält das (verifizierte) Ergebnis fest.

## Kernergebnis (vorweg)

**Für die produktive Verkabelung ist die Abbildung Motorwerte → gesendete GRBL-Achswerte
linear und eindeutig umkehrbar.** Es gibt auf Port-Ebene **keine** Mehrdeutigkeit.

> Die B3-Mehrdeutigkeit (Ellbogen oben/unten) steckt ausschließlich in der **kartesischen**
> Inverskinematik `calculateAngles3D()` (Pose → Gelenkwinkel). Der Sync nutzt diese Richtung
> **nicht** — er geht `MPos → Motorwerte → Vorwärtskinematik → Pose`, und beide Schritte sind
> eindeutige Funktionen. **Damit ist der Sync-Pfad als Ganzes eindeutig.**

---

## Ausgangslage

### Produktiv-Verkabelung (`startRobot.js`)

```js
new TelnetSenderClass(baseIP,  2300, 'x', 'y', 'z')      // Base
new TelnetSenderClass(elbowIP, 5000, 'a', null, null)    // Elbow
new TelnetSenderClass(handIP,  5000, 'c', 'e', 'b')      // Hand
```

### Bedeutung der Motor-Felder (`RobotMotorPosition`)

| Feld | Bedeutung |
|---|---|
| `x` | `xMotor` (Schulterposition auf X-Schiene, **mm**) |
| `y` | `alpha` (Schulterwinkel, **rad**) |
| `z` | `beta`  (Unterarm-Neigung, **rad**) |
| `a` | `a` (Ellbogen-Dreher, **rad**) |
| `b` | `b` (Handgelenk-Knick, **rad**) |
| `c` | `c` (Hand-Dreher, **rad**) |
| `e` | `eMotor` (Greifer) |

Mit `D = 180/π` (Grad-Faktor).

### Was jeder Controller real sendet (`execCommand`, Produktiv-Verkabelung)

Aus dem Sende-Pfad (`robot/TelnetSenderGRBL.js`) ergeben sich genau **sieben** GRBL-Achswerte:

| Controller | GRBL-Achse | gesendeter Wert | in Motorwerten |
|---|---|---|---|
| **Base**  | `x` | `xMotor`              | `xMotor` |
| **Base**  | `y` | `alpha·D`             | `α·D` |
| **Base**  | `z` | `(beta − alpha)·D`    | `(β − α)·D` |
| **Elbow** | `x` | `a·D`                 | `a·D` |
| **Hand**  | `x` | `(c − b)·D`           | `(c − b)·D` |
| **Hand**  | `y` | `eMotor·D`            | `e·D` |
| **Hand**  | `z` | `b·D`                 | `b·D` |

Drei Ports koppeln je zwei Motorwerte (`base.z`, `hand.x`), aber **jeder gekoppelte Wert
wird mit einem unabhängig gelesenen Wert kombiniert** (`alpha` bzw. `b`). Das System ist
damit *unteres Dreieckssystem* → trivial auflösbar.

---

## Herleitung der Umkehrung

Gegeben die GRBL-Readings `base.{x,y,z}`, `elbow.{x}`, `hand.{x,y,z}` (Grad bzw. mm):

```
xMotor = base.x                          // direkt
alpha  = base.y / D
beta   = (base.z + base.y) / D           // base.z = (β−α)·D  ⇒  β = base.z/D + α
a      = elbow.x / D
b      = hand.z / D
c      = (hand.x + hand.z) / D           // hand.x = (c−b)·D  ⇒  c = hand.x/D + b
eMotor = hand.y / D
```

Als Funktion (siehe Test, kann später 1:1 in Paket 4 übernommen werden):

```js
function motorStateFromPorts(r) {           // r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} }
  const D = 180 / Math.PI;
  return {
    xMotor: r.base.x,
    alpha:  r.base.y / D,
    beta:  (r.base.z + r.base.y) / D,
    a:      r.elbow.x / D,
    b:      r.hand.z / D,
    c:     (r.hand.x + r.hand.z) / D,
    eMotor: r.hand.y / D,
  };
}
```

---

## Verifikation

`test/Robot.PortInverse.test.js` — 15 Tests, fünf repräsentative Motorzustände
(Nullstellung, gemischt, negative/große Winkel, gekoppelt `c≈b`):

- **A) Exaktheit gegen `portValue`** (volle Präzision): Rückgewinnung auf 1e-9 genau.
- **B) Gegen den echten Sende-Pfad `execCommand`** (inkl. 2-Dezimal-Rundung der
  G-Code-Werte): Rückgewinnung innerhalb der Rundung (Winkel < 1e-3 rad, `xMotor` < 0.02 mm).
- **C) Voll-Kette Sync**: `Ports → motorStateFromPorts → calculatePositionFromMotorAngles`
  liefert dieselbe Pose `x/y/z/phi/theta/psi` wie die Original-Motorwerte (auf 1e-6).

---

## Konsequenzen für ToDo_9 / Paket 4

1. **Keine Zweig-Wahl auf Port-Ebene nötig.** Die frühere Annahme (B3-Disambiguierung im
   Baustein) trifft auf die Port-Rückrechnung **nicht** zu — sie ist linear und eindeutig.
2. **Rundung beachten.** `MPos` kommt mit endlicher Präzision; gekoppelte Werte (`beta`, `c`)
   summieren zwei gerundete Ports → Toleranz im Sub-Promille-Bereich, unkritisch.
3. **Achszahl je Controller.** FluidNC meldet `MPos` für **alle** in seiner Config definierten
   Achsen. Genutzt werden nur: Base `x,y,z` · Elbow `x` · Hand `x,y,z`. Beim Parsen die
   übrigen (falls vorhanden) ignorieren.
4. **Verkabelungs-Abhängigkeit.** `motorStateFromPorts()` gilt für die aktuelle Verkabelung.
   Ändert sich `startRobot.js` (andere Port-Zuordnung), muss die Umkehrung mitgezogen werden —
   der Round-Trip-Test (`portValue(motorStateFromPorts(p)) ≈ p`) schützt davor.