G92 ohne Senden

robot/GCode.js

@@ -39,6 +39,7 @@ class GCode{
         if(s.indexOf('G') !== 0){return false;}
         if(s.indexOf('G90') ==  0){return true;}
         if(s.indexOf('G91') ==  0){return true;}
+        if(s.indexOf('G92') ==  0){return true;}    // G92 - Set Position
         if(s.indexOf('G28') !== -1){return true;}
         if(s.indexOf('G1 ') !== -1){return true;}
         return false;
@@ -89,7 +90,7 @@ class GCode{
 
         if(g == undefined) return;
 
-        console.log("GCode: Empfange GCode: " + g);
+        //console.log("GCode: Empfange GCode: " + g);
 
         var multipleCommands = g.split(" G");
         var doProcessRest = false;
@@ -202,7 +203,32 @@ class GCode{
                 if(s.includes("e")){ robot.e = Number(s.substring(1, s.length));}
             });
         }
+        
+        if(g[0] == "G92"){  // G92 - Set Position
+            g.forEach((s) => { 
+                if(s.includes("X")){ robot.xMotor = Number(s.substring(1, s.length));}
+                if(s.includes("x")){ robot.xMotor = Number(s.substring(1, s.length));}
+                if(s.includes("Y")){ robot.alpha = Number(s.substring(1, s.length));}
+                if(s.includes("y")){ robot.alpha = Number(s.substring(1, s.length));}
+                if(s.includes("Z")){ robot.beta = Number(s.substring(1, s.length));}
+                if(s.includes("z")){ robot.beta = Number(s.substring(1, s.length));}
 
+                if(s.includes("A")){ robot.a = Number(s.substring(1, s.length));}
+                if(s.includes("a")){ robot.a = Number(s.substring(1, s.length));}
+                if(s.includes("B")){ robot.b = Number(s.substring(1, s.length));}
+                if(s.includes("b")){ robot.b = Number(s.substring(1, s.length));}
+                if(s.includes("C")){ robot.c = Number(s.substring(1, s.length));}
+                if(s.includes("c")){ robot.c = Number(s.substring(1, s.length));}
+
+                if(s.includes("E")){ robot.e = Number(s.substring(1, s.length));}
+                if(s.includes("e")){ robot.e = Number(s.substring(1, s.length));}
+            });
+            robot.calculatePositionFromMotorAngles();
+            calculateNew = false;
+
+            // ToDo: Send Command to update Position of Robot, because G92 should 
+            // set the current Position to the given Coordinates without moving the Robot.
+        }
 
         if(calculateNew && !calculateFromMotorCoordinates){  
             robot.calculateAngles3D();  

robot/TelnetSenderGRBL.js

@@ -12,7 +12,6 @@ module.exports = class TelnetSenderGRBL{
      *  zAxisGrbl: ... 
      */
     constructor(urlGRBL = "grblesp.local", maxSpeedF = 5000, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z", aAxisGrbl = null, bAxisGrbl = null, cAxisGrbl = null, eAxisGrbl = null){
-
         var socket = null;
         this.tSocket = null;
         this.receiver = null;
@@ -26,6 +25,12 @@ module.exports = class TelnetSenderGRBL{
         this.cAxisGrbl = cAxisGrbl;
         this.eAxisGrbl = eAxisGrbl;
         
+        
+        if (urlGRBL === "test.test") {
+            this.tSocket = { written: "", write(txt){ this.written = txt; } };
+            this.isTestMode = true;
+            return;
+        }
 
 
         new Promise((resolve, reject) => {
@@ -53,10 +58,10 @@ module.exports = class TelnetSenderGRBL{
     }
     
     moveTo(mOld, mNew){
-        this.translateAxisNames(mOld, mNew, "G1")
+        this.execCommand("G1", mOld, mNew)
     }
 
-    translateAxisNames(mOld, mNew, strCommand = "G1"){
+    execCommand(strCommand = "G1", mOld, mNew ){
 
         // The Hand-Turn is not 1:1 to the Hand-Lift °     
         var factorTurnLift = 1.2;
@@ -67,7 +72,7 @@ module.exports = class TelnetSenderGRBL{
         // Hand-Open in mm
         var handOpenInMM = 1.0
 
-        var data = strCommand; 
+        var data = strCommand.toString("utf-8"); 
 
         if(this.xAxisGrbl == "x"){
             data += " x" + (mNew.x).toFixed(2).toString();
@@ -200,10 +205,9 @@ module.exports = class TelnetSenderGRBL{
 
         data += " f"+(maxSpeedF.toFixed(2).toString())
 
-
-        console.log("" + this.urlGRBLstr + " receives: " + data.toString("utf-8"))
     
         if(this.tSocket && data.toString("utf-8").length > 3){
+            if(!this.isTestMode){ console.log("" + this.urlGRBLstr + " gets the message: " + data.toString("utf-8"))}
             this.tSocket.write( data.toString("utf-8") + "\r\n"); 
         }
     } 

robot/WSSenderGrbl.js

@@ -26,6 +26,12 @@ module.exports = class TelnetSenderGRBL{
         this.bAxisGrbl = bAxisGrbl;
         this.cAxisGrbl = cAxisGrbl;
         this.eAxisGrbl = eAxisGrbl;
+
+        if (urlGRBL === "test.test") {
+            this.tSocket = { written: "", write(txt){ this.written = txt; } };
+            this.isTestMode = true;
+            return;
+        }
         
         var fluidConfig = { host: urlGRBL, port: 80, reconnectDelay: 30000}
 
@@ -36,6 +42,12 @@ module.exports = class TelnetSenderGRBL{
     
 
     moveTo(mOld, mNew){
+        this.execCommand("G1", mOld, mNew)
+    }
+
+    execCommand(strCommand = "G1", mOld, mNew){
+
+        var data = strCommand.toString("utf-8");
 
         // The Hand-Turn is not 1:1 to the Hand-Lift °     
         var factorTurnLift = 1.2;
@@ -46,7 +58,7 @@ module.exports = class TelnetSenderGRBL{
         // Hand-Open in mm
         var handOpenInMM = 1.0
 
-        var data = "G1" 
+
 
         if(this.xAxisGrbl == "x"){
             data += " x" + (mNew.x).toFixed(2).toString();
@@ -179,10 +191,9 @@ module.exports = class TelnetSenderGRBL{
 
         data += " f"+(maxSpeedF.toFixed(2).toString())
 
-
-        console.log("" + this.urlGRBLstr + " receives: " + data.toString("utf-8"))
     
         if(this.tSocket && data.toString("utf-8").length > 3){
+            if(!this.isTestMode){ console.log("" + this.urlGRBLstr + " gets the message: " + data.toString("utf-8"))}
             this.tSocket.write( data.toString("utf-8") + "\r\n"); 
         }
     } 

test/GCode_G92.test.js

@@ -0,0 +1,48 @@
+// __tests__/Robot.inverseKinematics.test.js
+const Robot = require('../robot/Robot.js');
+const GCode = require('../robot/GCode.js');
+
+describe("Robot G92", () => {
+
+    test("ReadPosition -> calculatePositionFromMotorAngles()", () => {
+
+        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
+        const L1 = 300;
+        const L2 = 200;
+        const L3 = 10;
+
+
+        const A = new Robot(L1, L2, L3)
+
+        // Beispiel-Eingabe
+        A.x = 0;
+        A.y = 310;
+        A.z = 0;
+
+        A.phi   = -Math.PI/2;
+        A.theta = Math.PI/2;
+        A.psi   = 0;
+        A.e     = 0;
+
+        A.calculateAngles3D();
+
+        var strGCode = `G92 X${A.xMotor} Y${A.alpha} Z${A.beta} A${A.a} B${A.b} C${A.c}`
+
+
+        const T = new Robot(L1, L2, L3)
+        console.log("GCode: " + strGCode);
+        GCode.receiveGCode(T, strGCode);
+
+        
+        const EPS = 0.01;  // 1/1000 mm Genauigkeit
+        expect(T.xMotor).toBeCloseTo(A.xMotor, EPS);
+        expect(T.alpha).toBeCloseTo(A.alpha, EPS);
+        expect(T.beta).toBeCloseTo(A.beta, EPS);
+        expect(T.x).toBeCloseTo(A.x, EPS);
+        expect(T.y).toBeCloseTo(A.y, EPS);
+        expect(T.z).toBeCloseTo(A.z, EPS);
+
+
+    });
+});
+

test/Robot.Kinematics.RoundTrip.test.js

@@ -40,6 +40,9 @@ describe("Robot Kinematics Roundtrip", () => {
         B.xMotor = motor.xMotor;
         B.alpha  = motor.alpha;
         B.beta   = motor.beta;
+        B.a      = motor.a;
+        B.b      = motor.b;
+        B.c      = motor.c;
 
         // Diese Funktion rekonstruiert nur x, y, z!
         B.calculatePositionFromMotorAngles();
@@ -49,9 +52,9 @@ describe("Robot Kinematics Roundtrip", () => {
 
         expect(B.pY).toBeCloseTo(A.pY, EPS);
         expect(B.pZ).toBeCloseTo(A.pZ, EPS);
-        //expect(B.x).toBeCloseTo(A.x, EPS);
+        expect(B.x).toBeCloseTo(A.x, EPS);
-        //expect(B.y).toBeCloseTo(A.y, EPS);
+        expect(B.y).toBeCloseTo(A.y, EPS);
-        //expect(B.z).toBeCloseTo(A.z, EPS);
+        expect(B.z).toBeCloseTo(A.z, EPS);
     });
 
     test("calculateAngles3D() <-> calculatePositionFromMotorAngles()  handgelenk 2", () => {

test/Robot.Kinematics.RoundTripList.test.js

@@ -65,7 +65,7 @@ describe("Robot Kinematics Roundtrip (parametrisiert)", () => {
 
                 B.calculatePositionFromMotorAngles();
 
-                const EPS = 2; // 10^-2 mm → realistisch bei trig
+                const EPS = 0.02; // 10^-2 mm → realistisch bei trig
 
                 try {
                     expect(B.pY).toBeCloseTo(A.pY, EPS);

test/Sender.Telnet.test.js

@@ -0,0 +1,53 @@
+var TenetSender = require('../robot/TelnetSenderGRBL.js')
+
+
+describe("TelnetSenderGRBL.execCommand", () => {
+
+    test("writes correct G-code to mocked tSocket", () => {
+
+        // Create instance (will try real connection, but we override tSocket immediately)
+        const sender = new TenetSender(urlGRBL = "test.test",   maxSpeedF = 2300, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z");
+
+        // Mock tSocket.write
+        sender.tSocket = {
+            written: "",
+            write: function(txt) {
+                this.written = txt;   // store what was written
+            }
+        };
+
+        // Provide some sample motion data
+        const mOld = { x: 0, y: 0, z: 0, a:0, b:0, c:0, e:0 };   // not used in your code
+        const mNew = { x: 12.34, y: Math.PI/2, z: 0, a:0, b:0, c:0, e:0 };
+
+        sender.execCommand("G1", mOld, mNew);
+
+        // ✅ verify output
+        expect(sender.tSocket.written).toBe("G1 x12.34 y90.00 z-90.00 f2300.00\r\n");
+    });
+
+
+     test("writes correct G-code G92 to mocked tSocket", () => {
+
+        // Create instance (will try real connection, but we override tSocket immediately)
+        const sender = new TenetSender(urlGRBL = "test.test",   maxSpeedF = 2300, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z");
+
+        // Mock tSocket.write
+        sender.tSocket = {
+            written: "",
+            write: function(txt) {
+                this.written = txt;   // store what was written
+            }
+        };
+
+        // Provide some sample motion data
+        const mOld = { x: 0, y: 0, z: 0, a:0, b:0, c:0, e:0 };   // not used in your code
+        const mNew = { x: 12.34, y: 1.0, z: 2.0, a:0, b:0, c:0, e:0 };
+
+        sender.execCommand("G92", mOld, mNew);
+
+        // ✅ verify output
+        expect(sender.tSocket.written).toBe("G92 x12.34 y57.30 z57.30 f2300.00\r\n");
+    });
+
+});

test/Sender.WS.test.js

@@ -0,0 +1,53 @@
+var Sender = require('../robot/WSSenderGrbl.js')
+
+
+describe("WS-SenderGRBL.execCommand", () => {
+
+    test("writes correct G-code to mocked WS tSocket", () => {
+
+        // Create instance (will try real connection, but we override tSocket immediately)
+        const sender = new Sender(urlGRBL = "test.test",   maxSpeedF = 2300, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z");
+
+        // Mock tSocket.write
+        sender.tSocket = {
+            written: "",
+            write: function(txt) {
+                this.written = txt;   // store what was written
+            }
+        };
+
+        // Provide some sample motion data
+        const mOld = { x: 0, y: 0, z: 0, a:0, b:0, c:0, e:0 };   // not used in your code
+        const mNew = { x: 12.34, y: 1.0, z: 2.0, a:0, b:0, c:0, e:0 };
+
+        sender.execCommand("G1", mOld, mNew);
+
+        // ✅ verify output
+        expect(sender.tSocket.written).toBe("G1 x12.34 y57.30 z57.30 f2300.00\r\n");
+    });
+
+
+     test("writes correct G-code G92 to mocked WS tSocket", () => {
+
+        // Create instance (will try real connection, but we override tSocket immediately)
+        const sender = new Sender(urlGRBL = "test.test",   maxSpeedF = 2300, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z");
+
+        // Mock tSocket.write
+        sender.tSocket = {
+            written: "",
+            write: function(txt) {
+                this.written = txt;   // store what was written
+            }
+        };
+
+        // Provide some sample motion data
+        const mOld = { x: 0, y: 0, z: 0, a:0, b:0, c:0, e:0 };   // not used in your code
+        const mNew = { x: 12.34, y: 1.0, z: 2.0, a:0, b:0, c:0, e:0 };
+
+        sender.execCommand("G92", mOld, mNew);
+
+        // ✅ verify output
+        expect(sender.tSocket.written).toBe("G92 x12.34 y57.30 z57.30 f2300.00\r\n");
+    });
+
+});