f6a752cf58
Die Phase-2-Umstellung (Commits549d10b,2197a89) war falsch: sie machte gerade Hand = b=0. Hardware-Daten zeigen aber gerade = B~180 (z.B. G92 ... B179.20). Folge: G28 fuhr nach b=0 -> ~179 Grad Einklappen -> Hand schlug in den Arm (Crash). Zurueck auf den verifizierten Stand933a017: - IK: this.b = Math.acos(cosB) - FK: rotateAroundAxis(vecUnterarm, n, this.b) - G28: robot.b = Math.PI Verifiziert mit echten Daten: G92 B179.20 -> b=179.20; G28 -> b=180 (Weg 0.8 Grad, kein Slam); IK-Round-Trip exakt; 452/452 Tests gruen. Logs nicht enthalten. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
257 lines
13 KiB
JavaScript
257 lines
13 KiB
JavaScript
/***
|
||
* RobotController — Steuerlogik (ToDo_6).
|
||
*
|
||
* Verarbeitet bereits geparste G-Code-/Befehlsobjekte (aus GCodeParser) und wendet
|
||
* sie auf das Robotermodell an: Bewegungsmodus, Kinematik, sendCommand().
|
||
*
|
||
* Bewusst getrennt vom Parsing (GCodeParser) und vom Datei-Handling (GCode):
|
||
* der Controller kennt nur strukturierte Befehle, keine rohen Textstrings.
|
||
*/
|
||
const GCodeParser = require('./GCodeParser');
|
||
const { motorStateFromPorts, D } = require('./portInverse');
|
||
|
||
class RobotController {
|
||
|
||
/**
|
||
* Parst eine rohe Nachricht und wendet alle enthaltenen Befehle der Reihe nach an.
|
||
*
|
||
* Rückgabe: `undefined` (synchron, wie bisher) für alle gewöhnlichen Befehle.
|
||
* Nur wenn ein asynchroner Befehl enthalten war (Hardware-Sync, ToDo_9 Paket 4)
|
||
* wird ein Promise zurückgegeben, das auf dessen Abschluss wartet.
|
||
* @param {object} robot Robotermodell
|
||
* @param {string|Buffer} message rohe G-Code-Nachricht
|
||
*/
|
||
static receive(robot, message) {
|
||
const commands = GCodeParser.parse(message);
|
||
if (!commands.length) return;
|
||
const results = commands.map(parsed => this.applyCommand(robot, parsed));
|
||
const pending = results.filter(r => r && typeof r.then === 'function');
|
||
if (pending.length === 0) return; // synchroner Pfad unverändert
|
||
return Promise.all(pending).then(arr => arr[arr.length - 1]);
|
||
}
|
||
|
||
/**
|
||
* Wendet einen einzelnen, bereits geparsten Befehl auf das Robotermodell an.
|
||
* @param {object} robot
|
||
* @param {{command: string, params: object}} parsed
|
||
*/
|
||
static applyCommand(robot, parsed) {
|
||
if (!parsed || !parsed.command) return;
|
||
|
||
const cmd = parsed.command;
|
||
const params = parsed.params || {};
|
||
|
||
if (cmd === 'G90') {
|
||
robot.moveRelative = false;
|
||
return;
|
||
}
|
||
|
||
if (cmd === 'G91') {
|
||
robot.moveRelative = true;
|
||
return;
|
||
}
|
||
|
||
if (cmd === 'G28') {
|
||
// Home = Grundstellung: Arm + Hand gestreckt entlang -y (siehe
|
||
// doc/Info_Koordinaten.md). Ziel-Fingerspitze: (0, -(l1+l2+l3), 0).
|
||
const reach = robot.l1 + robot.l2 + robot.l3;
|
||
const homeY = -reach;
|
||
|
||
if (Math.abs(Math.abs(homeY) - reach) < 1e-6) {
|
||
// Sonderfall voll ausgestreckt: |y| = l1+l2+l3 ist eine Handgelenk-
|
||
// Singularität — die IK kann den Unterarm-Dreher a (und damit c) dort
|
||
// nicht bestimmen und liefert Müll (z.B. a=135°, c=45°), wodurch der
|
||
// Finger schräg/nach unten zeigt. Daher die Motorwerte DIREKT in die
|
||
// Grundstellung setzen und die Workspace-Pose per FK füllen.
|
||
robot.xMotor = 0;
|
||
robot.alpha = 0;
|
||
robot.beta = 0;
|
||
robot.a = 0;
|
||
robot.b = Math.PI; // gerade Hand (Phase-1-Konvention; Phase 2: 0)
|
||
robot.c = 0;
|
||
// Greifer (e/eMotor) bewusst UNANGETASTET lassen: G28 fährt nur den Arm.
|
||
// Würde man e=0 setzen, ergäbe die Kopplung eMotor = e − b − c bei b=π den
|
||
// Wert −π → −180° am Finger-Motor → Anschlag-Slam (siehe
|
||
// doc/Info_Koordinaten.md, Phase 2). Phase 2 (b=0 = gerade) löst das sauber.
|
||
robot.calculatePositionFromMotorAngles(); // FK -> x=0, y=-(l1+l2+l3), z=0
|
||
} else {
|
||
// Allgemeiner (nicht-singulärer) Home-Punkt: regulär über die IK.
|
||
robot.x = 0;
|
||
robot.y = homeY;
|
||
robot.z = 0;
|
||
robot.phi = Math.PI / 2;
|
||
robot.theta = Math.PI / 2;
|
||
robot.psi = 0;
|
||
robot.e = 0;
|
||
robot.calculateAngles3D();
|
||
}
|
||
robot.sendCommand();
|
||
return;
|
||
}
|
||
|
||
if (cmd === 'G1') {
|
||
if (robot.moveRelative) {
|
||
if (Number.isFinite(params.X)) { robot.x += params.X; robot.xMotorChanged = true; }
|
||
if (Number.isFinite(params.Y)) { robot.y += params.Y; robot.yMotorChanged = true; }
|
||
if (Number.isFinite(params.Z)) { robot.z += params.Z; robot.zMotorChanged = true; }
|
||
if (Number.isFinite(params.A)) { robot.phi += params.A; robot.aMotorChanged = true; }
|
||
if (Number.isFinite(params.B)) { robot.theta += params.B; robot.bMotorChanged = true; }
|
||
if (Number.isFinite(params.C)) { robot.psi += params.C; robot.cMotorChanged = true; }
|
||
if (Number.isFinite(params.E)) { robot.e += params.E; robot.eMotorChanged = true; }
|
||
if (Number.isFinite(params.F)) { robot.feedrate = params.F; }
|
||
} else {
|
||
if (Number.isFinite(params.X)) { robot.x = params.X; robot.xMotorChanged = true; }
|
||
if (Number.isFinite(params.Y)) { robot.y = params.Y; robot.yMotorChanged = true; }
|
||
if (Number.isFinite(params.Z)) { robot.z = params.Z; robot.zMotorChanged = true; }
|
||
if (Number.isFinite(params.A)) { robot.phi = params.A; robot.aMotorChanged = true; }
|
||
if (Number.isFinite(params.B)) { robot.theta = params.B; robot.bMotorChanged = true; }
|
||
if (Number.isFinite(params.C)) { robot.psi = params.C; robot.cMotorChanged = true; }
|
||
if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; }
|
||
if (Number.isFinite(params.F)) { robot.feedrate = params.F; }
|
||
}
|
||
|
||
robot.calculateAngles3D();
|
||
robot.sendCommand();
|
||
return;
|
||
}
|
||
|
||
if (cmd === 'M1') {
|
||
if (robot.moveRelative) {
|
||
if (Number.isFinite(params.X)) { robot.xMotor += params.X; robot.xMotorChanged = true; }
|
||
if (Number.isFinite(params.Y)) { robot.alpha += params.Y; robot.yMotorChanged = true; }
|
||
if (Number.isFinite(params.Z)) { robot.beta += params.Z; robot.zMotorChanged = true; }
|
||
if (Number.isFinite(params.A)) { robot.a += params.A; robot.aMotorChanged = true; }
|
||
if (Number.isFinite(params.B)) { robot.b += params.B; robot.bMotorChanged = true; }
|
||
if (Number.isFinite(params.C)) { robot.c += params.C; robot.cMotorChanged = true; }
|
||
if (Number.isFinite(params.E)) { robot.e += params.E; robot.eMotorChanged = true; }
|
||
} else {
|
||
if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; }
|
||
if (Number.isFinite(params.Y)) { robot.alpha = params.Y; robot.yMotorChanged = true; }
|
||
if (Number.isFinite(params.Z)) { robot.beta = params.Z; robot.zMotorChanged = true; }
|
||
if (Number.isFinite(params.A)) { robot.a = params.A; robot.aMotorChanged = true; }
|
||
if (Number.isFinite(params.B)) { robot.b = params.B; robot.bMotorChanged = true; }
|
||
if (Number.isFinite(params.C)) { robot.c = params.C; robot.cMotorChanged = true; }
|
||
if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; }
|
||
}
|
||
|
||
robot.calculatePositionFromMotorAngles();
|
||
robot.sendCommand();
|
||
return;
|
||
}
|
||
|
||
if (cmd === 'M114' && params.R === true) {
|
||
// Hardware-Sync (ToDo_9 Paket 4): liest die echten MPos aller Controller
|
||
// und übernimmt sie als Soll-Zustand. Asynchron → gibt ein Promise zurück.
|
||
return this.syncFromHardware(robot);
|
||
}
|
||
|
||
if (cmd === 'M92' || cmd === 'G92') {
|
||
// Beide setzen die Motorposition ohne Bewegung, unterscheiden sich aber in den
|
||
// Winkel-EINHEITEN:
|
||
// G92 → GRAD (G-Code-Konvention für Rotationsachsen, wie FluidNC und die
|
||
// "Position Motoren"-Anzeige in public/app.js). Intern sind die
|
||
// Winkel-Slots in Radiant → Grad/D umrechnen (D = 180/π).
|
||
// M92 → RADIANT, roh in die internen Slots (interne/Test-Variante).
|
||
// X ist die lineare mm-Schiene, E die Greifer-Öffnung in mm (ab Null-Position
|
||
// eines Fingers) — beide ohne Winkel-Umrechnung.
|
||
const angScale = (cmd === 'G92') ? 1 / D : 1;
|
||
robot.createMotorPosition();
|
||
if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; }
|
||
if (Number.isFinite(params.Y)) { robot.alpha = params.Y * angScale; robot.yMotorChanged = true; }
|
||
if (Number.isFinite(params.Z)) { robot.beta = params.Z * angScale; robot.zMotorChanged = true; }
|
||
if (Number.isFinite(params.A)) { robot.a = params.A * angScale; robot.aMotorChanged = true; }
|
||
if (Number.isFinite(params.B)) { robot.b = params.B * angScale; robot.bMotorChanged = true; }
|
||
if (Number.isFinite(params.C)) { robot.c = params.C * angScale; robot.cMotorChanged = true; }
|
||
// E nach B/C setzen: der Greifer-Motorwert hängt über die Kinematik-Kopplung
|
||
// von b und c ab. robot.e = Finger-Öffnung (mm), eMotor = abgeleiteter Motorwert.
|
||
// Ohne diese eMotor-Ableitung bliebe der Greiferwert stale (alte E-Inkonsistenz):
|
||
// sendCommand() verschickt eMotor, nicht e.
|
||
if (Number.isFinite(params.E)) {
|
||
robot.e = params.E;
|
||
robot.eMotor = robot.gripperMotorFromOpening(robot.e);
|
||
robot.eMotorChanged = true;
|
||
}
|
||
|
||
robot.calculatePositionFromMotorAngles();
|
||
robot.sendCommand('G92');
|
||
return;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Hardware-Sync (ToDo_9 Paket 4): liest die echten Achs-Positionen (`MPos`) der
|
||
* drei Controller, rekonstruiert daraus die sieben Motorwerte und übernimmt sie
|
||
* als neuen Soll-Zustand. Danach Vorwärtskinematik → Pose.
|
||
*
|
||
* Bewegt den Roboter NICHT — es wird kein `sendCommand()`/Move ausgelöst, nur der
|
||
* interne Zustand an die Realität angeglichen (nach Homing/Jog/Stall/Reconnect).
|
||
*
|
||
* @param {object} robot
|
||
* @param {{timeoutMs?: number}} [options]
|
||
* @returns {Promise<{x,y,z,phi,theta,psi}>} die übernommene Pose
|
||
*/
|
||
static async syncFromHardware(robot, options = {}) {
|
||
const timeoutMs = Number.isFinite(options.timeoutMs) ? options.timeoutMs : 1000;
|
||
const receivers = (robot && robot.cmdReceivers) || [];
|
||
|
||
// Sender nach Rolle zuordnen (controllerRole wird in startRobot.js gesetzt).
|
||
const byRole = {};
|
||
for (const s of receivers) {
|
||
if (s && s.controllerRole) byRole[s.controllerRole] = s;
|
||
}
|
||
for (const role of ['base', 'elbow', 'hand']) {
|
||
if (!byRole[role]) {
|
||
throw new Error(`Sync: Controller '${role}' fehlt (kein Sender mit controllerRole='${role}')`);
|
||
}
|
||
}
|
||
|
||
// Frische Reports von allen drei Controllern anfordern (aktiv '?', mit await).
|
||
const [baseSnap, elbowSnap, handSnap] = await Promise.all([
|
||
byRole.base.requestStatusReport(timeoutMs),
|
||
byRole.elbow.requestStatusReport(timeoutMs),
|
||
byRole.hand.requestStatusReport(timeoutMs),
|
||
]);
|
||
|
||
// MPos-Arrays validieren (FluidNC meldet ggf. mehr Achsen; nur die nötigen lesen).
|
||
const need = (snap, role, n) => {
|
||
const mp = snap && snap.machinePosition;
|
||
if (!Array.isArray(mp) || mp.length < n || !mp.slice(0, n).every(Number.isFinite)) {
|
||
throw new Error(`Sync: ${role} lieferte keine gültige MPos (${JSON.stringify(mp)})`);
|
||
}
|
||
return mp;
|
||
};
|
||
const b = need(baseSnap, 'base', 3);
|
||
const e = need(elbowSnap, 'elbow', 1);
|
||
const h = need(handSnap, 'hand', 3);
|
||
|
||
// Port → Motorwerte (linear/eindeutig, ToDo_9a) → auf den Roboter schreiben.
|
||
const m = motorStateFromPorts({
|
||
base: { x: b[0], y: b[1], z: b[2] },
|
||
elbow: { x: e[0] },
|
||
hand: { x: h[0], y: h[1], z: h[2] },
|
||
});
|
||
robot.xMotor = m.xMotor;
|
||
robot.alpha = m.alpha;
|
||
robot.beta = m.beta;
|
||
robot.a = m.a;
|
||
robot.b = m.b;
|
||
robot.c = m.c;
|
||
robot.eMotor = m.eMotor;
|
||
|
||
// Vorwärtskinematik: füllt x/y/z + phi/theta/psi aus den Hardwarewerten.
|
||
robot.calculatePositionFromMotorAngles();
|
||
|
||
// motorPosition zurücksetzen, damit der nächste Move den Speed-Delta von der
|
||
// echten Position aus rechnet (sonst falscher Feedrate im Korrekt-Modus).
|
||
robot.motorPosition = null;
|
||
robot.motorPositionOld = null;
|
||
|
||
return {
|
||
x: robot.x, y: robot.y, z: robot.z,
|
||
phi: robot.phi, theta: robot.theta, psi: robot.psi,
|
||
};
|
||
}
|
||
}
|
||
|
||
module.exports = RobotController;
|