272 lines
10 KiB
Markdown
272 lines
10 KiB
Markdown
# ToDo 15 — Beschleunigungssensor LIS3DH (Unterarm / Hand-Controller)
|
|
|
|
## Kontext
|
|
|
|
Der „Hand"-FluidNC-Controller (`fluidNcHand.local`) hat einen LIS3DH I²C-Sensor
|
|
an Adresse `0x18` (dezimal 24). Die angepasste FluidNC-Firmware unterstützt M260/M261
|
|
für I²C-Zugriff (siehe `FluidNC/docs/i2c-gcode-integration.md`).
|
|
|
|
Ziel: X/Y/Z-Beschleunigungsdaten periodisch auslesen, in der Info-Page anzeigen,
|
|
und später den daraus berechneten Winkel mit dem Position-Driver-Winkel `b` vergleichen.
|
|
|
|
---
|
|
|
|
## Designentscheidung: WSSenderGrbl statt TelnetSenderGRBL
|
|
|
|
**Gewählt:** `WSSenderGrbl` (WebSocket, Port 81 = FluidNC WebUI-Protokoll).
|
|
|
|
**Begründung:**
|
|
- Genau **eine** Verbindung pro Controller, die G-Code-Moves und Sensordaten gemeinsam
|
|
trägt — kein zweiter paralleler Socket.
|
|
- Die WebSocket-Verbindung empfängt dieselben Ausgaben wie Telnet: Status-Reports
|
|
`<Idle|MPos:…>`, `ok`/`error`-Zeilen, und `log_info`-Nachrichten wie `I2CRESP`.
|
|
- Der `grblState === 'Idle'`-Check (aus den Status-Reports) verhindert I²C-Polling
|
|
während eines laufenden Moves — genau wie TelnetSenderGRBL es macht.
|
|
|
|
**Nicht gewählt:** TelnetSenderGRBL — hätte zwar weniger Umbauaufwand (Response-Parser
|
|
existiert dort bereits), aber der Umbau von WSSenderGrbl ist sinnvoll, weil die
|
|
Response-Infrastruktur dort früher oder später sowieso gebraucht wird.
|
|
|
|
**Hinweis:** FluidNC sendet auf Port 81 pro WebSocket-Frame eine vollständige Zeile
|
|
(kein TCP-Fragmentierungsproblem). Das vereinfacht den Parser gegenüber Telnet.
|
|
|
|
---
|
|
|
|
## Phase 1 — Sensor auslesen (Backend)
|
|
|
|
### 1.1 Initialisierung des LIS3DH (einmalig nach Connect)
|
|
|
|
```
|
|
M260 P24 Q32 R87 → CTRL_REG1 0x20 = 0x57 (100 Hz, alle drei Achsen aktiv)
|
|
```
|
|
|
|
Muss gesendet werden, sobald der Hand-Controller verbunden ist (nach dem ersten Connect
|
|
oder Reconnect). Ohne diesen Schritt liefern OUT_X/Y/Z nur Nullen.
|
|
|
|
### 1.2 Lesesequenz (wiederholt, z. B. alle 500 ms)
|
|
|
|
```
|
|
M260 P24 Q168 → Registerzeiger auf 0xA8 (= OUT_X_L | 0x80 Auto-Increment)
|
|
M261 P24 L6 → 6 Bytes lesen: X_L X_H Y_L Y_H Z_L Z_H
|
|
```
|
|
|
|
Antwortformat in der WebSocket-Ausgabe (via FluidNC `log_info`):
|
|
```
|
|
I2CRESP B0 A0x18: C0 01 40 F8 80 3F
|
|
```
|
|
|
|
### 1.3 Byte → g-Wert Umrechnung (JavaScript)
|
|
|
|
```js
|
|
function parseLIS3DH(hexBytes) { // z. B. ['C0','01','40','F8','80','3F']
|
|
function toSigned16(lo, hi) {
|
|
const raw = (parseInt(hi, 16) << 8) | parseInt(lo, 16);
|
|
return raw > 32767 ? raw - 65536 : raw;
|
|
}
|
|
const x = toSigned16(hexBytes[0], hexBytes[1]) >> 4; // 12-bit rechtsbündig
|
|
const y = toSigned16(hexBytes[2], hexBytes[3]) >> 4;
|
|
const z = toSigned16(hexBytes[4], hexBytes[5]) >> 4;
|
|
// ±2 g Bereich: 1 g ≈ 1024 LSB
|
|
return { x: x / 1024, y: y / 1024, z: z / 1024 };
|
|
}
|
|
```
|
|
|
|
---
|
|
|
|
## Phase 2 — Winkelberechnung (später, wenn Achsenorientierung bekannt)
|
|
|
|
Wenn Schwerkraft = einzige Beschleunigung (Arm ruhig):
|
|
|
|
```js
|
|
const roll = Math.atan2(y, z); // Rotation um X-Achse
|
|
const pitch = Math.atan2(-x, Math.hypot(y, z)); // Rotation um Y-Achse
|
|
```
|
|
|
|
Welche Winkelachse dem Motor `b` entspricht, muss durch Ausprobieren (Arm drehen,
|
|
Sensor beobachten) bestimmt werden.
|
|
|
|
Vergleich mit Position Driver:
|
|
- Sensorwinkel (rad) ↔ `robot.b` (rad, Konvention: gerade Hand = π)
|
|
- Differenz = Schlupf / Kalibrierungsfehler
|
|
|
|
---
|
|
|
|
## ToDo-Liste: Programmänderungen
|
|
|
|
### A — `robot/WSSenderGrbl.js` — Response-Infrastruktur ✅ erledigt
|
|
|
|
**A1 — EventEmitter einbinden** ✅
|
|
- `SenderInterface` erbt jetzt von `EventEmitter` → WSSenderGrbl und TelnetSenderGRBL
|
|
bekommen die Fähigkeit automatisch, ohne eigene `class`-Zeile zu ändern.
|
|
|
|
**A2 — Eingehende Nachrichten empfangen** ✅
|
|
- `ws.on('message', (data) => this._handleMessage(data))` in `_tryConnect()`
|
|
- `_handleMessage(data)`: trimmt, verwirft Leerzeilen, ruft `_handleResponseLine()`
|
|
|
|
**A3 — Response-Zeilen auswerten** ✅
|
|
- `_handleResponseLine(line)`: dispatcht auf `_parseStatusReport`, `_handleI2CResp`,
|
|
oder setzt `lastError` + emittiert `'error-report'`
|
|
- Neue Felder im Konstruktor: `grblState`, `machinePosition`, `lastReportAt`, `lastError`,
|
|
`_i2cRespResolve`, `_i2cRespReject`
|
|
|
|
**A4 — Status-Reports parsen** ✅
|
|
- `_parseStatusReport(line)`: extrahiert `grblState` + `machinePosition`, setzt
|
|
`lastReportAt`, emittiert `'status'`
|
|
- `getStatus()` gibt jetzt auch `grblState`, `machinePosition`, `lastReportAt`,
|
|
`lastError` zurück
|
|
|
|
**A5 — Heartbeat (Idle-Erkennung)** ✅
|
|
- `_startHeartbeat()` / `_stopHeartbeat()` via injizierbare `setIntervalFn` /
|
|
`clearIntervalFn`; sendet `?` im Intervall (default 10 s)
|
|
- Startet in `ws.on('open')`, stoppt in `ws.on('close')` und `disconnect()`
|
|
|
|
**Unit-Tests:** `test/Sender.WS.responseParsing.test.js` — alle Tests grün.
|
|
|
|
---
|
|
|
|
### B — `robot/WSSenderGrbl.js` — I²C / LIS3DH
|
|
|
|
(Baut auf Abschnitt A auf; `grblState` muss verfügbar sein.)
|
|
|
|
**B1 — I2CRESP-Handler**
|
|
- [ ] Neue Felder:
|
|
```js
|
|
this._i2cRespResolve = null;
|
|
this._i2cRespReject = null;
|
|
```
|
|
- [ ] Methode `_handleI2CResp(line)`:
|
|
- Bytes-String extrahieren (nach dem letzten `:`)
|
|
- als Array von Hex-Strings splitten
|
|
- `_i2cRespResolve(bytes)` aufrufen, Felder auf `null` zurücksetzen
|
|
|
|
**B2 — Async-Helfer**
|
|
- [ ] Methode `async _waitI2CResp(timeoutMs = 500)` → Promise:
|
|
- setzt `_i2cRespResolve` / `_i2cRespReject`
|
|
- Timeout löst mit `null` auf (kein Fehler-Throw, damit Polling weiterläuft)
|
|
- [ ] Methode `async sendAndWaitI2CResp(gcode, timeoutMs = 500)`:
|
|
- prüft `grblState === 'Idle'` → bei nicht-Idle: return `null`
|
|
- startet `_waitI2CResp()`
|
|
- sendet `gcode` via `this.send(gcode)`
|
|
- `await`-et das Promise, gibt Bytes-Array zurück
|
|
|
|
**B3 — Sensor-Methoden**
|
|
- [ ] Methode `async initLIS3DH()`:
|
|
```
|
|
M260 P24 Q32 R87
|
|
```
|
|
Sendet Konfigurationsbefehl, wartet auf `ok` (oder kurzes Delay).
|
|
- [ ] Methode `async readLIS3DH()` → `{ x, y, z }` oder `null`:
|
|
1. `M260 P24 Q168` senden (Registerzeiger), auf `ok` warten
|
|
2. `M261 P24 L6` senden, auf `I2CRESP` warten
|
|
3. Bytes mit `parseLIS3DH()` umrechnen
|
|
|
|
**B4 — Polling-Loop**
|
|
- [ ] Methode `startAccelerometerPolling(intervalMs = 1000)`:
|
|
- ruft `initLIS3DH()` einmalig auf
|
|
- startet `setInterval` mit `readLIS3DH()`
|
|
- speichert Ergebnis in `this.accelerometer = { x, y, z, timestamp }`
|
|
- `emit('accelerometer', this.accelerometer)`
|
|
- [ ] Methode `stopAccelerometerPolling()`: löscht Interval
|
|
- [ ] `startAccelerometerPolling()` in `ws.on('open', …)` nach `_startHeartbeat()` aufrufen,
|
|
nur wenn `this.controllerRole === 'hand'`
|
|
- [ ] Bei `ws.on('close', …)`: `stopAccelerometerPolling()` aufrufen
|
|
|
|
---
|
|
|
|
### C — `startRobot.js` — Hand-Controller auf WSSenderGrbl umstellen
|
|
|
|
Aktuell verwendet `startRobot.js` für alle Controller denselben `TelnetSenderClass`.
|
|
|
|
- [ ] `WSSenderGrbl` importieren:
|
|
```js
|
|
const WSSender = require('./robot/WSSenderGrbl');
|
|
```
|
|
- [ ] Beim Erstellen der Sender-Instanzen: für den Hand-Controller `WSSenderGrbl`
|
|
verwenden, für Base und Elbow weiterhin `TelnetSenderClass`:
|
|
```js
|
|
const SenderClass = key === 'hand' ? WSSender : TelnetSenderClass;
|
|
const instance = new SenderClass(ctrl.ip, ctrl.port, ...axes7, { … });
|
|
```
|
|
(WSSenderGrbl ignoriert `port` im Konstruktor und nutzt immer Port 81 — ggf.
|
|
`wsPort` als separates Option-Feld in `robot.json` ergänzen.)
|
|
- [ ] Alternativ: `WSSenderClass` als injizierbaren Parameter in `startRobot()` ergänzen
|
|
(analog zu `TelnetSenderClass`), damit Tests mocken können.
|
|
|
|
---
|
|
|
|
### D — `robot/AccelerometerService.js` (neue Datei)
|
|
|
|
- [ ] Klasse `AccelerometerService` anlegen
|
|
- [ ] Konstruktor nimmt `handSender` (WSSenderGrbl-Instanz)
|
|
- [ ] `start()`: lauscht auf `handSender.on('accelerometer', …)` und speichert Daten
|
|
- [ ] `getLatest()`: gibt `{ x, y, z, timestamp }` zurück (oder `null`)
|
|
- [ ] (Phase 2) `getAngle()`: berechnet Roll/Pitch, gibt `{ roll, pitch }` zurück
|
|
|
|
---
|
|
|
|
### E — `server/InfoServer.js`
|
|
|
|
- [ ] `AccelerometerService` importieren, Instanz mit Hand-Sender erzeugen, `start()` aufrufen
|
|
- [ ] Neuer Endpunkt `GET /api/acceleration`:
|
|
```js
|
|
app.get('/api/acceleration', (req, res) => {
|
|
res.json(accelerometerService.getLatest() ?? { x: null, y: null, z: null });
|
|
});
|
|
```
|
|
- [ ] (Phase 2) Endpunkt um `angle` erweitern
|
|
|
|
---
|
|
|
|
### F — `public/index.html`
|
|
|
|
- [ ] Neuen Abschnitt **Acceleration** in der Info-Page einfügen (nach „Position Driver"):
|
|
```html
|
|
<div class="section">
|
|
<h3>Acceleration (LIS3DH)</h3>
|
|
<table>
|
|
<tr><td>X</td><td id="acc-x">—</td><td>g</td></tr>
|
|
<tr><td>Y</td><td id="acc-y">—</td><td>g</td></tr>
|
|
<tr><td>Z</td><td id="acc-z">—</td><td>g</td></tr>
|
|
</table>
|
|
<!-- Phase 2 -->
|
|
<table id="acc-angle-table" style="display:none">
|
|
<tr><td>Winkel (Sensor)</td><td id="acc-angle">—</td><td>°</td></tr>
|
|
<tr><td>Winkel (Driver b)</td><td id="driver-b">—</td><td>°</td></tr>
|
|
<tr><td>Differenz</td><td id="acc-diff">—</td><td>°</td></tr>
|
|
</table>
|
|
</div>
|
|
```
|
|
|
|
---
|
|
|
|
### G — `public/app.js`
|
|
|
|
- [ ] Neue Funktion `updateAcceleration()`:
|
|
- `fetch('/api/acceleration')` → JSON
|
|
- Felder `#acc-x`, `#acc-y`, `#acc-z` setzen (auf 3 Dezimalstellen)
|
|
- Fehlerfall: Felder auf `—` setzen
|
|
- [ ] `updateAcceleration()` in den 1-Sekunden-Poll-Zyklus aufnehmen
|
|
(neben `updateStatus()`, `updatePosition()`)
|
|
- [ ] (Phase 2) Winkel-Zeilen einblenden und befüllen, sobald Achse bekannt
|
|
|
|
---
|
|
|
|
## Offene Fragen / nächste Schritte
|
|
|
|
1. **WebSocket-Nachrichtenformat**: Verifizieren, dass FluidNC Port 81 `log_info`-Nachrichten
|
|
(`I2CRESP …`) als plain-text WebSocket-Frames sendet (und nicht JSON-gewrappt).
|
|
→ Testbefehl: `M260` (Scan) manuell über FluidNC WebUI senden und WS-Traffic beobachten.
|
|
|
|
2. **Achsenorientierung**: Welche LIS3DH-Achse (x/y/z) entspricht dem Motor-`b`-Winkel?
|
|
→ Arm langsam kippen, Sensorwerte in der Info-Page beobachten.
|
|
|
|
3. **Polling-Konflikt**: I²C-Befehle dürfen nicht während eines Moves gesendet werden.
|
|
Prüfen ob `grblState === 'Idle'` ausreicht, oder ob ein Queue-Mechanismus nötig ist.
|
|
|
|
4. **`ok`-Synchronisation**: Für `initLIS3DH()` und den Zeiger-Befehl vor dem Lesen wird
|
|
ein `ok`-Wait benötigt. Prüfen, ob ein einfaches `await delay(50)` reicht oder ob
|
|
ein echter `ok`-Promise nötig ist (dann Abschnitt A3 erweitern).
|
|
|
|
5. **Kalibrierung**: Nullpunkt und Skalierung des Sensors ggf. gegen bekannte Positionen
|
|
abgleichen (Arm senkrecht = b=0, Arm waagrecht = b=π/2 usw.).
|