127 lines
6.0 KiB
JavaScript
127 lines
6.0 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');
|
|
|
|
class RobotController {
|
|
|
|
/**
|
|
* Parst eine rohe Nachricht und wendet alle enthaltenen Befehle der Reihe nach an.
|
|
* @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;
|
|
commands.forEach(parsed => this.applyCommand(robot, parsed));
|
|
}
|
|
|
|
/**
|
|
* 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') {
|
|
robot.x = 0;
|
|
robot.y = robot.l1 + robot.l2 + robot.l3;
|
|
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 === 'M92' || cmd === 'G92') {
|
|
robot.createMotorPosition();
|
|
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('G92');
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
module.exports = RobotController;
|