255 lines
12 KiB
JavaScript
255 lines
12 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;
|
|
robot.e = 0;
|
|
robot.eMotor = robot.gripperMotorFromOpening(robot.e);
|
|
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;
|