Claude: Umbau Robot
This commit is contained in:
chk
108
robot/GCode.js
108
robot/GCode.js
@@ -2,7 +2,7 @@
|
||||
* Receives GCode, processes it and moves the Data to the Roboter-Class
|
||||
*/
|
||||
const fs = require('fs');
|
||||
const GCodeParser = require('./GCodeParser');
|
||||
const RobotController = require('./RobotController');
|
||||
|
||||
|
||||
class GCode{
|
||||
@@ -81,106 +81,14 @@ class GCode{
|
||||
return newGString.trim();
|
||||
};
|
||||
|
||||
/**
|
||||
* Verarbeitet eine rohe G-Code-Nachricht. Parsing + Steuerlogik liegen jetzt im
|
||||
* GCodeParser bzw. RobotController (ToDo_6); diese Methode bleibt als Fassade
|
||||
* erhalten, damit bestehende Aufrufer (InputWS, Datei-Befehle) unverändert
|
||||
* funktionieren.
|
||||
*/
|
||||
static receiveGCode(robot, g){
|
||||
const commands = GCodeParser.parse(g);
|
||||
if (!commands.length) return;
|
||||
|
||||
commands.forEach(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') {
|
||||
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;
|
||||
}
|
||||
});
|
||||
RobotController.receive(robot, g);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////77
|
||||
|
||||
@@ -7,8 +7,16 @@ class Robot{
|
||||
// Umgebungsvariablen-Logik
|
||||
const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ?
|
||||
Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000;
|
||||
this.useSpeedCalc = process.env.ROBOT_USE_SPEED_CALC === 'true' ||
|
||||
// Speed-Regelung-Schalter: 'legacy' (Default — exakt wie bisher) oder 'correct'.
|
||||
// Siehe doc/ToDo_6a_Speed.md.
|
||||
this.speedMode = (process.env.ROBOT_SPEED_MODE || 'legacy').toLowerCase();
|
||||
// ROBOT_USE_SPEED_CALC bleibt der interne Schalter für calculateSpeeds();
|
||||
// der Korrekt-Modus aktiviert die Berechnung automatisch.
|
||||
this.useSpeedCalc = this.speedMode === 'correct' ||
|
||||
process.env.ROBOT_USE_SPEED_CALC === 'true' ||
|
||||
process.env.ROBOT_USE_SPEED_CALC === '1';
|
||||
/** @type {number} Bewegungszeit des letzten Schritts in Minuten (für koordinierte Feedrate) */
|
||||
this.lastMoveTime = 0;
|
||||
|
||||
/** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */
|
||||
this.speedX = 200;
|
||||
@@ -131,6 +139,10 @@ class Robot{
|
||||
// Setze Geschwindigkeiten
|
||||
this.motorPosition.speeds = {...this.motorSpeeds};
|
||||
this.motorPosition.feedrate = this.feedrate || 200;
|
||||
|
||||
// Speed-Regelung: Modus und (vorläufige) Bewegungszeit für die Sender
|
||||
this.motorPosition.speedMode = this.speedMode;
|
||||
this.motorPosition.moveTime = this.lastMoveTime || 0;
|
||||
}
|
||||
|
||||
// Berechnet aus XYZ die Motor-Winkel für den GCode
|
||||
@@ -203,6 +215,8 @@ class Robot{
|
||||
if (!this.useSpeedCalc) return; // Neue Logik nur aktivieren, wenn Flag gesetzt
|
||||
if (!oldPos || !newPos || this.feedrate <= 0) return;
|
||||
|
||||
this.lastMoveTime = 0; // wird unten gesetzt, sobald eine Bewegung erkannt wird
|
||||
|
||||
// 1. Berechne xyz-Distanz (primär)
|
||||
const dx = newPos.x - oldPos.x;
|
||||
const dy = newPos.y - oldPos.y;
|
||||
@@ -211,9 +225,10 @@ class Robot{
|
||||
|
||||
if (xyz_dist > 0.001) {
|
||||
const time = xyz_dist / this.feedrate;
|
||||
this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time;
|
||||
this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time;
|
||||
this.motorSpeeds.z = (this.beta - oldPos.beta) / time;
|
||||
this.lastMoveTime = time;
|
||||
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
|
||||
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
|
||||
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
|
||||
this.motorSpeeds.a = (this.a - oldPos.a) / time;
|
||||
this.motorSpeeds.b = (this.b - oldPos.b) / time;
|
||||
this.motorSpeeds.c = (this.c - oldPos.c) / time;
|
||||
@@ -229,9 +244,10 @@ class Robot{
|
||||
|
||||
if (handgelenk_dist > 0.001) {
|
||||
const time = handgelenk_dist / this.feedrate;
|
||||
this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time;
|
||||
this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time;
|
||||
this.motorSpeeds.z = (this.beta - oldPos.beta) / time;
|
||||
this.lastMoveTime = time;
|
||||
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
|
||||
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
|
||||
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
|
||||
this.motorSpeeds.a = (this.a - oldPos.a) / time;
|
||||
this.motorSpeeds.b = (this.b - oldPos.b) / time;
|
||||
this.motorSpeeds.c = (this.c - oldPos.c) / time;
|
||||
@@ -243,9 +259,10 @@ class Robot{
|
||||
const de = Math.abs(this.eMotor - oldPos.e);
|
||||
if (de > 0.001) {
|
||||
const time = de / this.feedrate;
|
||||
this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time;
|
||||
this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time;
|
||||
this.motorSpeeds.z = (this.beta - oldPos.beta) / time;
|
||||
this.lastMoveTime = time;
|
||||
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
|
||||
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
|
||||
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
|
||||
this.motorSpeeds.a = (this.a - oldPos.a) / time;
|
||||
this.motorSpeeds.b = (this.b - oldPos.b) / time;
|
||||
this.motorSpeeds.c = (this.c - oldPos.c) / time;
|
||||
@@ -336,6 +353,8 @@ class Robot{
|
||||
// Berechne Geschwindigkeiten
|
||||
this.calculateSpeeds(this.motorPositionOld, this.motorPosition);
|
||||
this.motorPosition.speeds = {...this.motorSpeeds};
|
||||
// moveTime nach der Berechnung aktualisieren (createMotorPosition lief davor)
|
||||
this.motorPosition.moveTime = this.lastMoveTime || 0;
|
||||
|
||||
console.log("Robot.sendCommand: ", cmd.toString(), " Motor-Pos: x=", this.motorPosition.x.toFixed(3), "yMotor=",this.motorPosition.y.toFixed(3), "zMotor=",this.motorPosition.z.toFixed(3), "aM=", this.motorPosition.a.toFixed(3), "bM=", this.motorPosition.b.toFixed(3), "cM=", this.motorPosition.c.toFixed(3), " e=", this.motorPosition.e.toFixed(3));
|
||||
|
||||
|
||||
126
robot/RobotController.js
Normal file
126
robot/RobotController.js
Normal file
@@ -0,0 +1,126 @@
|
||||
/***
|
||||
* 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') {
|
||||
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;
|
||||
@@ -32,6 +32,12 @@ module.exports = class RobotMotorPosition{
|
||||
|
||||
// Feedrate in mm/min
|
||||
this.feedrate = 0;
|
||||
|
||||
// Bewegungszeit des Schritts in Minuten (für koordinierte Feedrate, ToDo_6a)
|
||||
this.moveTime = 0;
|
||||
|
||||
// Speed-Regelung-Modus, vom Robot gesetzt: 'legacy' | 'correct'
|
||||
this.speedMode = 'legacy';
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -235,6 +235,116 @@ module.exports = class TelnetSenderGRBL extends SenderInterface {
|
||||
this.execCommand("G1", mOld, mNew)
|
||||
}
|
||||
|
||||
/**
|
||||
* Liefert den Wert, der an einen GRBL-Port gesendet wird, wenn dieser auf eine
|
||||
* Roboter-Achse abgebildet ist — als reine Funktion der Positions-/Geschwindigkeits-
|
||||
* Quelle `pos` ({x,y,z,a,b,c,e}). Dieselben Formeln wie der Sende-Pfad, aber isoliert
|
||||
* und testbar. Wird für die koordinierte Feedrate (ToDo_6a) verwendet.
|
||||
* @returns {number|null}
|
||||
*/
|
||||
portValue(grblPort, robotAxis, pos) {
|
||||
if (!robotAxis || !pos) return null;
|
||||
const D = 180 / Math.PI;
|
||||
const factorTurnLift = 1.2;
|
||||
const handOpenInMM = 1.0;
|
||||
const { x, y, z, a, b, c, e } = pos;
|
||||
|
||||
switch (grblPort) {
|
||||
case 'x':
|
||||
switch (robotAxis) {
|
||||
case 'x': return x;
|
||||
case 'y': return y * D;
|
||||
case 'z': return (z - y) * D;
|
||||
case 'a': return a * D;
|
||||
case 'b': return (b + z - y) * D;
|
||||
case 'c': return (-b + c) * D;
|
||||
case 'e': return -1.0 * (-1.0 * (b * D * factorTurnLift) + c * D) + e * handOpenInMM;
|
||||
}
|
||||
break;
|
||||
case 'y':
|
||||
switch (robotAxis) {
|
||||
case 'x': return x;
|
||||
case 'y': return y * D;
|
||||
case 'z': return (z - y) * D;
|
||||
case 'a': return a * D;
|
||||
case 'b': return (b + z - y) * D;
|
||||
case 'c': return -1.0 * (b * D * factorTurnLift) + c * D;
|
||||
case 'e': return e * D;
|
||||
}
|
||||
break;
|
||||
case 'z':
|
||||
switch (robotAxis) {
|
||||
case 'x': return x;
|
||||
case 'y': return y * D;
|
||||
case 'z': return (z - y) * D;
|
||||
case 'a': return a * D;
|
||||
case 'b': return b * D;
|
||||
case 'c': return (c + b + z - y) * D;
|
||||
case 'e': return e * D;
|
||||
}
|
||||
break;
|
||||
case 'a':
|
||||
switch (robotAxis) {
|
||||
case 'x': return y * D; // Quirk: a-Port auf robotAxis 'x' nutzt y (wie im Sende-Pfad)
|
||||
case 'y': return y * D;
|
||||
case 'z': return (z - y) * D;
|
||||
case 'a': return a * D;
|
||||
case 'b': return (b + z - y) * D;
|
||||
case 'c': return (c + b + z - y) * D;
|
||||
case 'e': return e * D;
|
||||
}
|
||||
break;
|
||||
case 'b':
|
||||
switch (robotAxis) {
|
||||
case 'x': return x;
|
||||
case 'y': return y * D;
|
||||
case 'z': return (z - y) * D;
|
||||
case 'a': return a * D;
|
||||
case 'b': return b * D;
|
||||
case 'c': return (c + b + z - y) * D;
|
||||
case 'e': return e * D;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
/**
|
||||
* Koordinierte Feedrate für diesen Sender (Korrekt-Modus): die euklidische Strecke,
|
||||
* die dieser Sender in diesem Schritt zurücklegt, geteilt durch die Bewegungszeit.
|
||||
* So beenden alle Controller den Schritt gleichzeitig.
|
||||
* @returns {number|null} Feedrate oder null, wenn nicht bestimmbar (→ Legacy-Fallback)
|
||||
*/
|
||||
computeCoordinatedFeedrate(mOld, mNew) {
|
||||
const moveTime = mNew && Number.isFinite(mNew.moveTime) ? mNew.moveTime : 0;
|
||||
if (!(moveTime > 0) || !mOld) return null;
|
||||
|
||||
const ports = [
|
||||
['x', this.xAxisGrbl],
|
||||
['y', this.yAxisGrbl],
|
||||
['z', this.zAxisGrbl],
|
||||
['a', this.aAxisGrbl],
|
||||
['b', this.bAxisGrbl],
|
||||
];
|
||||
|
||||
let sumSq = 0;
|
||||
let any = false;
|
||||
for (const [port, robotAxis] of ports) {
|
||||
if (!robotAxis) continue;
|
||||
const vNew = this.portValue(port, robotAxis, mNew);
|
||||
const vOld = this.portValue(port, robotAxis, mOld);
|
||||
if (!Number.isFinite(vNew) || !Number.isFinite(vOld)) continue;
|
||||
const d = vNew - vOld;
|
||||
sumSq += d * d;
|
||||
any = true;
|
||||
}
|
||||
if (!any) return null;
|
||||
|
||||
const dist = Math.sqrt(sumSq);
|
||||
if (!(dist > 0)) return null;
|
||||
return dist / moveTime;
|
||||
}
|
||||
|
||||
execCommand(strCommand = "G1", mOld, mNew ){
|
||||
|
||||
// The Hand-Turn is not 1:1 to the Hand-Lift °
|
||||
@@ -377,14 +487,22 @@ module.exports = class TelnetSenderGRBL extends SenderInterface {
|
||||
if(this.tSocket && data.length > 3){
|
||||
|
||||
if(strCommand == "G1" && mNew){
|
||||
const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ?
|
||||
const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ?
|
||||
Number(process.env.ROBOT_DEFAULT_FEEDRATE) : this.maxSpeedF;
|
||||
const feedrate = mNew.feedrate !== undefined ? mNew.feedrate : DEFAULT_FEEDRATE;
|
||||
data += " f"+(feedrate.toFixed(2).toString())
|
||||
|
||||
// ToDo: Aus motorSpeed mit einzelnenen Werten muss noch die feedrate berechnet werden.
|
||||
// hier bin ich unsicher, ob das nicht in den Sender rein sollte, da es eventuell
|
||||
// abhngig vom FluidNC und dessen speed interpretation ist.
|
||||
let feedrate;
|
||||
if (mNew.speedMode === 'correct') {
|
||||
// Korrekt-Modus: koordinierte Feedrate; Fallback auf Legacy-Wert,
|
||||
// wenn (noch) nicht bestimmbar (z. B. erster Schritt ohne moveTime).
|
||||
const coordinated = this.computeCoordinatedFeedrate(mOld, mNew);
|
||||
feedrate = (coordinated != null && Number.isFinite(coordinated) && coordinated > 0)
|
||||
? coordinated
|
||||
: (mNew.feedrate !== undefined ? mNew.feedrate : DEFAULT_FEEDRATE);
|
||||
} else {
|
||||
// Legacy-Pfad — unverändert: kartesische Feedrate an alle Achsen
|
||||
feedrate = mNew.feedrate !== undefined ? mNew.feedrate : DEFAULT_FEEDRATE;
|
||||
}
|
||||
data += " f"+(feedrate.toFixed(2).toString())
|
||||
}
|
||||
|
||||
if(data.indexOf("G90") == -1 && data.indexOf("G1 ") !== -1){
|
||||
|
||||
Reference in New Issue
Block a user