From 9328747fe69cb9804175f5c1ca077704638f271f Mon Sep 17 00:00:00 2001 From: ChK Date: Fri, 10 Apr 2026 07:20:23 +0200 Subject: [PATCH] Speed testen (anfang) --- robot/Robot.js | 73 +++++++++++++++++++++++++++------- robot/RobotMotorPosition.js | 5 +++ robot/TelnetSenderGRBL.js | 6 ++- test/GCode.speed.test.js | 39 ++++++++++++++++++ test/Robot.sendCommand.test.js | 13 ++++++ 5 files changed, 121 insertions(+), 15 deletions(-) diff --git a/robot/Robot.js b/robot/Robot.js index e423dcc..4da65d8 100755 --- a/robot/Robot.js +++ b/robot/Robot.js @@ -4,6 +4,12 @@ class Robot{ constructor(l1, l2, l3) { + // Umgebungsvariablen-Logik + const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ? + Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 200; + this.useSpeedCalc = process.env.ROBOT_USE_SPEED_CALC === 'true' || + process.env.ROBOT_USE_SPEED_CALC === '1'; + /** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */ this.speedX = 200; /** @type {number} Bewegungsgeschwindigkeit Y-Achse in mm/min */ @@ -45,7 +51,7 @@ class Robot{ this.e = 0.0; /** @type {number} Feedrate für Bewegungen in mm/min */ - this.feedrate = 200; // Standardwert + this.feedrate = DEFAULT_FEEDRATE; /** @type {Object} Motor-Geschwindigkeiten in Einheiten pro Minute */ this.motorSpeeds = {x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0}; @@ -118,6 +124,11 @@ class Robot{ this.motorPosition.cMotorChanged = this.cMotorChanged; this.motorPosition.eMotorChanged = this.eMotorChanged; + // Setze Handgelenk-Koordinaten (für Speed-Berechnung) + this.motorPosition.pX = this.pX; + this.motorPosition.pY = this.pY; + this.motorPosition.pZ = this.pZ; + // Setze Geschwindigkeiten this.motorPosition.speeds = {...this.motorSpeeds}; this.motorPosition.feedrate = this.feedrate || 200; @@ -190,26 +201,60 @@ class Robot{ // Berechnet die Motor-Geschwindigkeiten basierend auf Feedrate und Positionsänderung calculateSpeeds(oldPos, newPos) { + if (!this.useSpeedCalc) return; // Neue Logik nur aktivieren, wenn Flag gesetzt if (!oldPos || !newPos || this.feedrate <= 0) return; - // Berechne Distanz im Raum (nur linear, Orientierung ignoriert für Einfachheit) + // 1. Berechne xyz-Distanz (primär) const dx = newPos.x - oldPos.x; const dy = newPos.y - oldPos.y; const dz = newPos.z - oldPos.z; - const dist = Math.sqrt(dx*dx + dy*dy + dz*dz); - + const xyz_dist = Math.sqrt(dx*dx + dy*dy + dz*dz); - const time = (dist === 0) ? 1/200 : dist / this.feedrate; // Zeit in Minuten + 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.motorSpeeds.a = (this.a - oldPos.a) / time; + this.motorSpeeds.b = (this.b - oldPos.b) / time; + this.motorSpeeds.c = (this.c - oldPos.c) / time; + this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; + return; + } - // Geschwindigkeiten in Motor-Einheiten pro Minute - // Annahme: xMotor in mm/min, alpha/beta/a/b/c in rad/min, e in mm/min - 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.motorSpeeds.a = (this.a - oldPos.a) / time; - this.motorSpeeds.b = (this.b - oldPos.b) / time; - this.motorSpeeds.c = (this.c - oldPos.c) / time; - this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; + // 2. Berechne Handgelenk-Punkt-Distanz (falls xyz = 0) + const dpx = newPos.pX - oldPos.pX; + const dpy = newPos.pY - oldPos.pY; + const dpz = newPos.pZ - oldPos.pZ; + const handgelenk_dist = Math.sqrt(dpx*dpx + dpy*dpy + dpz*dpz); + + 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.motorSpeeds.a = (this.a - oldPos.a) / time; + this.motorSpeeds.b = (this.b - oldPos.b) / time; + this.motorSpeeds.c = (this.c - oldPos.c) / time; + this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; + return; + } + + // 3. Berechne Finger-Distanz (falls Handgelenk = 0) + 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.motorSpeeds.a = (this.a - oldPos.a) / time; + this.motorSpeeds.b = (this.b - oldPos.b) / time; + this.motorSpeeds.c = (this.c - oldPos.c) / time; + this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; + return; + } + + // 4. Keine Bewegung erkannt → motorSpeeds bleiben 0 (sicherer Fallback) } rotateAroundAxis(v, n, angle) { diff --git a/robot/RobotMotorPosition.js b/robot/RobotMotorPosition.js index fb2ee07..e5b0dd2 100755 --- a/robot/RobotMotorPosition.js +++ b/robot/RobotMotorPosition.js @@ -12,6 +12,11 @@ module.exports = class RobotMotorPosition{ this.e = e; // Finger open + // Handgelenk-Punkt-Koordinaten (für Speed-Berechnung) + this.pX = 0; // Handgelenk X-Position in mm + this.pY = 0; // Handgelenk Y-Position in mm + this.pZ = 0; // Handgelenk Z-Position in mm + this.time = Date.now(); this.xMotorChanged = false; diff --git a/robot/TelnetSenderGRBL.js b/robot/TelnetSenderGRBL.js index 20280c5..8c28e0c 100755 --- a/robot/TelnetSenderGRBL.js +++ b/robot/TelnetSenderGRBL.js @@ -25,6 +25,8 @@ module.exports = class TelnetSenderGRBL{ this.cAxisGrbl = cAxisGrbl; this.eAxisGrbl = eAxisGrbl; + this.maxSpeedF = maxSpeedF; // Speichere für Backward-Kompatibilität + if (urlGRBL === "test.test") { this.tSocket = { written: "", write(txt){ this.written = txt; } }; @@ -204,7 +206,9 @@ module.exports = class TelnetSenderGRBL{ if(this.tSocket && data.toString("utf-8").length > 3){ if(strCommand == "G1" && mNew){ - const feedrate = mNew.feedrate !== undefined ? mNew.feedrate : 2300; + 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()) } diff --git a/test/GCode.speed.test.js b/test/GCode.speed.test.js index fcc933a..b1ccd23 100644 --- a/test/GCode.speed.test.js +++ b/test/GCode.speed.test.js @@ -38,6 +38,45 @@ describe('GCode Speed Tests', () => { // But since x,y,z changed, telnetSender1 should have the command }); + + test('G1 with Feedrate F300 - mit env', () => { + + + const previousValue = process.env.ROBOT_USE_SPEED_CALC; + process.env.ROBOT_USE_SPEED_CALC = 'true'; + + + const L1 = 300; + const L2 = 300; + const L3 = 20; + + const Robot2 = require('../robot/Robot'); // neu geladen + const robot = new Robot2(L1, L2, L3); + + const telnetSender1 = new TelnetSender("test.test", 5000, "x", "y", "z"); + + robot.cmdReceivers.push(telnetSender1); + + // G1 with F300 + robot.useSpeedCalc = true; // Aktiviere die neue Logik + GCode.receiveGCode(robot, "G90 G1 x0 f700"); // Absolut!! + GCode.receiveGCode(robot, "G91 G1 x120 f700"); // relativ!! + + // Check telnetSender1 (x,y,z axes) + expect(telnetSender1.tSocket.written).toContain('G1'); + expect(telnetSender1.tSocket.written).toContain('f700'); + expect(telnetSender1.tSocket.written).toMatch(/f700\.00/); // Exact format + + // telnetSender2 and telnetSender3 might not have output if no changes + // But since x,y,z changed, telnetSender1 should have the command + if (previousValue === undefined) { + delete process.env.ROBOT_USE_SPEED_CALC; + } else { + process.env.ROBOT_USE_SPEED_CALC = previousValue; + } + + }); + test('G1 without Feedrate - should use default f200', () => { const L1 = 300; const L2 = 300; diff --git a/test/Robot.sendCommand.test.js b/test/Robot.sendCommand.test.js index 0564b1c..569de3e 100644 --- a/test/Robot.sendCommand.test.js +++ b/test/Robot.sendCommand.test.js @@ -1,4 +1,5 @@ const Robot = require('../robot/Robot') +const GCode = require('../robot/GCode.js'); const MockCmdReceiver = require('./helpers/mockCmdReceiver') describe('Robot.sendCommand & cmdReceivers', () => { @@ -84,6 +85,18 @@ describe('Robot.sendCommand & cmdReceivers', () => { expect(newPos.y).toBe(20) }) + test('newPos enthält aktuelle Motorwerte GCode', () => { + const r = new MockCmdReceiver() + robot.cmdReceivers.push(r) + + GCode.receiveGCode(robot, "G90 G1 x10 y200 z0 f300") + + robot.sendCommand() + + const { newPos } = r.lastCall() + expect(newPos.x).toBe(5) // Dummy Test, soll eventuell anders sein. + }) + test('mehrere Receiver werden alle benachrichtigt', () => { const r1 = new MockCmdReceiver('r1') const r2 = new MockCmdReceiver('r2')