209 lines
9.5 KiB
Markdown
209 lines
9.5 KiB
Markdown
# G92 - Homing
|
||
|
||
die appRobotHoming ermittelt die Position der Gelenke (per Foto oder sonstigen Infos).
|
||
|
||
Diese werden wie folgt behandelt und umgerechnet.
|
||
|
||
---
|
||
|
||
## Befehlsformat
|
||
|
||
```
|
||
G92 X<mm> Y<°> Z<°> A<°> B<°> C<°> E<mm>
|
||
```
|
||
|
||
| Achse | Bedeutung | Einheit |
|
||
|-------|-----------------------------------|---------|
|
||
| X | Lineare Schiene (xMotor) | mm |
|
||
| Y | Schulterwinkel α (alpha) | Grad |
|
||
| Z | Ellenbogenwinkel β (beta) | Grad |
|
||
| A | Handgelenk 1 (a) | Grad |
|
||
| B | Handgelenk 2 (b) | Grad |
|
||
| C | Handgelenk 3 (c) | Grad |
|
||
| E | Greifer-Öffnung (ein Finger) | mm |
|
||
|
||
**Beispiel (tatsächlicher Homing-Aufruf):**
|
||
|
||
```
|
||
G92 X158.14 Y4.19 Z57.74 A91.85 B-45.46 C-69.92 E21.20
|
||
```
|
||
|
||
→ Y = 4,19°, Z = 57,74° usw. — alle Winkel direkt in Grad wie in FluidNC/GCode-Konvention.
|
||
|
||
---
|
||
|
||
## Geometrische Bedeutung der Winkel (Driver-Konvention)
|
||
|
||
> **Wichtig für appRobotHoming:** Der Driver interpretiert die G92-Winkel in einer
|
||
> **eigenen** Konvention. appRobotHoming muss die physisch gemessenen Gelenkwinkel
|
||
> **in diese Konvention umrechnen**, bevor sie als G92 gesendet werden. Die folgenden
|
||
> Tabellen sind aus der Kinematik (`Arm3SegmentLinearX`) **verifiziert** (jeweils eine
|
||
> Achse isoliert variiert).
|
||
|
||
### Koordinatenrahmen
|
||
|
||
- **z = 0** ist die Achse zwischen Base und Arm1 (Schulter) — kein Offset darunter.
|
||
- y = nach hinten (Hauptarbeitsrichtung), z = nach oben, x = Linearschiene.
|
||
- Alle Armwinkel liegen in der y-z-Ebene (bei fixer x-Schiene).
|
||
|
||
### Y = α (Oberarm) und Z = β (Unterarm) — ABSOLUT
|
||
|
||
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** (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`.
|
||
(Erst bei der Weiterleitung an FluidNC wird daraus `(β − α)` zurückgerechnet, siehe unten.)
|
||
|
||
### B = Handgelenk-Knick
|
||
|
||
Verifizierte Referenz (α=0, β=90, A=0, C=0):
|
||
|
||
| B (G92) | physischer Knick Unterarm↔Hand |
|
||
|---------|--------------------------------|
|
||
| 0° | 180° (Hand voll zurückgeklappt) |
|
||
| 90° | 90° (Hand ⊥ Unterarm) |
|
||
| 180° | 0° (Hand **gerade**, in Verlängerung des Unterarms) |
|
||
|
||
→ **Gerade Hand = B 180°.** Allgemein: `physischer Knick = 180° − B`, also `B = 180° − Knick`.
|
||
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), 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)
|
||
|
||
C dreht die Hand um ihre eigene Achse. Verifizierte Referenz (α=0, β=90, A=0, B=90),
|
||
nach Phase 1:
|
||
|
||
| C (G92) | Hand-Roll ψ |
|
||
|---------|-------------|
|
||
| 0° | +90° |
|
||
| 90° | 0° (neutral) |
|
||
| 180° | −90° |
|
||
|
||
→ In dieser Stellung gilt `ψ = 90° − C`. **Achtung:** der genaue Zusammenhang hängt von
|
||
der Armstellung ab — exakt `ψ = acos(cos β · sin A) − c` (Winkel in rad). C selbst ist der
|
||
physische Hand-Roll-Gelenkwinkel; nur der Bezug zum Welt-ψ verschiebt sich mit der Pose.
|
||
|
||
### E = Greifer-Öffnung (mm) + Sehnen-Kopplung
|
||
|
||
E ist die **Finger-Öffnung in mm** (ein Finger ab Null-Position) — keine Winkel-Umrechnung.
|
||
Der Driver leitet daraus den Motorwert ab:
|
||
|
||
```
|
||
eMotor = E − b − c (b, c in RADIANT!)
|
||
= E − B°/57.2958 − C°/57.2958
|
||
```
|
||
|
||
Grund: die Greifer-Sehne läuft durchs Handgelenk; Knick (B) und Roll (C) ziehen an der Sehne.
|
||
appRobotHoming sendet die **reine Öffnung** als E; die Kopplung macht der Driver. Bewegt sich
|
||
nur das Handgelenk, kompensiert eMotor, damit die Öffnung konstant bleibt.
|
||
|
||
### Zusammenfassung: was appRobotHoming senden muss
|
||
|
||
| Achse | Driver erwartet | Typische Falle |
|
||
|-------|------------------------------------------------|-----------------------------------------|
|
||
| X | xMotor in mm | — |
|
||
| Y | Oberarm absolut (0=waagerecht **−y**, 90=hoch) | — |
|
||
| Z | Unterarm **absolut** (0=−y; nicht relativ zum Oberarm) | relativ statt absolut gesendet |
|
||
| A | Unterarm-Dreher; **A=0 → Knick-Achse ∥ x** | Nullpunkt/Vorzeichen |
|
||
| B | **180° = gerade Hand**; Knick = 180° − B | gemessenen Knick direkt gesendet |
|
||
| C | Hand-Roll, **90° = neutral** | Nullpunkt/Vorzeichen |
|
||
| E | Öffnung in mm | Motorwert statt Öffnung gesendet |
|
||
|
||
---
|
||
|
||
## Interne Verarbeitung (`RobotController.js`)
|
||
|
||
Winkel-Achsen werden von Grad nach Radiant umgerechnet (D = 180/π):
|
||
|
||
```
|
||
robot.alpha = Y / D (intern: Radiant)
|
||
robot.beta = Z / D
|
||
robot.a = A / D
|
||
robot.b = B / D
|
||
robot.c = C / D
|
||
```
|
||
|
||
X bleibt mm, keine Umrechnung.
|
||
|
||
**Greifer E** wird **nach** B und C gesetzt, damit die kinematische Kopplung stimmt:
|
||
|
||
```
|
||
robot.e = E (Finger-Öffnung, mm)
|
||
robot.eMotor = gripperMotorFromOpening(e) (abgeleiteter Motorwert)
|
||
```
|
||
|
||
Bei `Arm3SegmentLinearX`: `eMotor = e − b − c` (Sehnenkompensation durch Handgelenk).
|
||
Bei `Arm3SegmentRotaryBase`: `eMotor = e` (keine Kopplung).
|
||
|
||
---
|
||
|
||
## Variante M92 (intern / Test)
|
||
|
||
```
|
||
M92 X<mm> Y<rad> Z<rad> A<rad> B<rad> C<rad> E<mm>
|
||
```
|
||
|
||
Winkel werden **roh als Radiant** übernommen. Für Skripte und Tests, nicht für Homing aus appRobotHoming.
|
||
|
||
---
|
||
|
||
## Weiterleitung an FluidNC-Instanzen
|
||
|
||
Nach dem Setzen der internen Motorslots ruft `robot.sendCommand('G92')` auf jedem registrierten `TelnetSenderGRBL` `execCommand('G92', mOld, mNew)` auf.
|
||
|
||
Jede Instanz bekommt ihren eigenen `G92`-Befehl mit den Port-Inverse-Achswerten (Rückumrechnung Radiant → Grad, mit Kopplung):
|
||
|
||
| Instanz | FluidNC-Achsen | Formel |
|
||
|---------|----------------------------|-----------------------------------------------------------|
|
||
| base | x = xMotor | direkt mm |
|
||
| | y = α → Grad | `alpha × D` |
|
||
| | z = β−α → Grad | `(beta − alpha) × D` |
|
||
| elbow | x = a → Grad | `a × D` |
|
||
| hand | x = c−b → Grad | `(c − b) × D` |
|
||
| | y = eMotor | direkt (mm oder gekoppelter Motorwert) |
|
||
| | z = b → Grad | `b × D` |
|
||
|
||
`G92` bekommt **kein** `G90`-Prefix und keinen Vorschub — nur die geänderten Achsen werden angehängt. Jede Instanz übernimmt den Werkstück-Koordinaten-Offset (WPos) ohne Bewegung.
|
||
|
||
**Hinweis:** Nur Achsen mit gesetztem `*MotorChanged`-Flag werden gesendet. Bleibt ein Wert gegenüber dem letzten Driver-Zustand unverändert, schickt die jeweilige Instanz keinen G92 für diese Achse. Nach einem Neustart des Drivers sind alle Flags gesetzt → alle Achsen werden gesendet.
|
||
|
||
---
|
||
|
||
## Reporting (`M114` / Web-UI)
|
||
|
||
| Feld | Quelle | Einheit | Anzeige in public/app.js |
|
||
|--------------------|----------------|---------|------------------------------------------|
|
||
| `position.x/y/z` | Workspace | mm | direkt |
|
||
| `position.a/b/c` | phi/theta/psi | rad | `× 180/π` → Grad |
|
||
| `position.e` | `robot.e` | mm | direkt (Greifer-Öffnung) |
|
||
| `motorCounts.x` | xMotor | mm | direkt |
|
||
| `motorCounts.y/z` | alpha/beta | rad | `× 180/π` → Grad |
|
||
| `motorCounts.a/b/c`| a/b/c | rad | `× 180/π` → Grad |
|
||
| `motorCounts.e` | `robot.eMotor` | mm | direkt (abgeleiteter Motorwert) |
|
||
|
||
---
|
||
|
||
## Behobene Fehler (Kontext)
|
||
|
||
**Ursprüngliches Problem:** `G92 X158.14 Y4.19 Z57.74 …` lieferte korrekte X-Werte, aber Y≈240 und Z≈3308 im Ergebnis. Ursache: Winkel wurden als Radiant interpretiert, intern aber mit `× D` auf Grad umgerechnet — doppelte Skalierung.
|
||
|
||
**Drei Korrekturen:**
|
||
|
||
1. **Grad-Interpretation:** G92 rechnet Eingabe-Winkel jetzt mit `÷ D` in Radiant um (statt roh zu übernehmen).
|
||
2. **Greifer-Motorwert:** `robot.e` (Öffnung) wurde gesetzt, aber `sendCommand()` überträgt `robot.eMotor`. Fix: `eMotor = gripperMotorFromOpening(e)` direkt im G92-Zweig, nach dem Setzen von B/C.
|
||
3. **Web-UI-Anzeige:** `state-e` zeigte `motorCounts.e × 180/π` (mm × 57,3 = Unsinn). Fix: zeigt jetzt `position.e` (mm direkt).
|