Files

2026-06-26 23:23:54 +02:00

10 KiB

Raw Blame History

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)

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):

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"):

<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

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.
Achsenorientierung: Welche LIS3DH-Achse (x/y/z) entspricht dem Motor-b-Winkel? → Arm langsam kippen, Sensorwerte in der Info-Page beobachten.
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.
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).
Kalibrierung: Nullpunkt und Skalierung des Sensors ggf. gegen bekannte Positionen abgleichen (Arm senkrecht = b=0, Arm waagrecht = b=π/2 usw.).

10 KiB Raw Blame History