diff --git a/robot/GCode.js b/robot/GCode.js index 1edbec9..b2fc64f 100755 --- a/robot/GCode.js +++ b/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(); diff --git a/robot/TelnetSenderGRBL.js b/robot/TelnetSenderGRBL.js index 453e4aa..15001e7 100755 --- a/robot/TelnetSenderGRBL.js +++ b/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"); } } diff --git a/robot/WSSenderGrbl.js b/robot/WSSenderGrbl.js index 18a2298..931270c 100755 --- a/robot/WSSenderGrbl.js +++ b/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"); } } diff --git a/test/GCode_G92.test.js b/test/GCode_G92.test.js new file mode 100755 index 0000000..f581a41 --- /dev/null +++ b/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); + + + }); +}); + diff --git a/test/Robot.Kinematics.RoundTrip.test.js b/test/Robot.Kinematics.RoundTrip.test.js index 33850f4..e3c2715 100755 --- a/test/Robot.Kinematics.RoundTrip.test.js +++ b/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.y).toBeCloseTo(A.y, EPS); - //expect(B.z).toBeCloseTo(A.z, EPS); + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); }); test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 2", () => { diff --git a/test/Robot.Kinematics.RoundTripList.test.js b/test/Robot.Kinematics.RoundTripList.test.js index e53533b..8dddb46 100755 --- a/test/Robot.Kinematics.RoundTripList.test.js +++ b/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); diff --git a/test/Sender.Telnet.test.js b/test/Sender.Telnet.test.js new file mode 100755 index 0000000..3a0e3d3 --- /dev/null +++ b/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"); + }); + +}); \ No newline at end of file diff --git a/test/Sender.WS.test.js b/test/Sender.WS.test.js new file mode 100755 index 0000000..018a1dd --- /dev/null +++ b/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"); + }); + +}); \ No newline at end of file