Speed testen (anfang)

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);
 
+        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;
+        }
 
-        const time = (dist === 0) ? 1/200 : dist / this.feedrate; // Zeit in Minuten
+        // 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);
 
-        // Geschwindigkeiten in Motor-Einheiten pro Minute
+        if (handgelenk_dist > 0.001) {
-        // Annahme: xMotor in mm/min, alpha/beta/a/b/c in rad/min, e in mm/min
+            const time = handgelenk_dist / this.feedrate;
-        this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time;
+            this.motorSpeeds.x = (this.xMotor - oldPos.xMotor) / time;
-        this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time;
+            this.motorSpeeds.y = (this.alpha - oldPos.alpha) / time;
-        this.motorSpeeds.z = (this.beta - oldPos.beta) / time;
+            this.motorSpeeds.z = (this.beta - oldPos.beta) / time;
-        this.motorSpeeds.a = (this.a - oldPos.a) / time;
+            this.motorSpeeds.a = (this.a - oldPos.a) / time;
-        this.motorSpeeds.b = (this.b - oldPos.b) / time;
+            this.motorSpeeds.b = (this.b - oldPos.b) / time;
-        this.motorSpeeds.c = (this.c - oldPos.c) / time;
+            this.motorSpeeds.c = (this.c - oldPos.c) / time;
-        this.motorSpeeds.e = (this.eMotor - oldPos.e) / 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) {

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;

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())
             }
             

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;

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')