diff --git a/robot/GCode.js b/robot/GCode.js index 5821698..169dcfd 100755 --- a/robot/GCode.js +++ b/robot/GCode.js @@ -120,6 +120,7 @@ class GCode{ if(s.toUpperCase().includes("B")){ robot.theta += Number(s.substring(1, s.length)); robot.bMotorChanged = true;} if(s.toUpperCase().includes("C")){ robot.psi += Number(s.substring(1, s.length)); robot.cMotorChanged = true;} if(s.toUpperCase().includes("E")){ robot.e += Number(s.substring(1, s.length)); robot.eMotorChanged = true;} + if(s.toUpperCase().includes("F")){ robot.feedrate = Number(s.substring(1, s.length)); } }); } else if(g[0] == "M1" && robot.moveRelative){ @@ -146,6 +147,7 @@ class GCode{ if(s.toUpperCase().includes("B")){ robot.theta = Number(s.substring(1, s.length)); robot.bMotorChanged = true;} if(s.toUpperCase().includes("C")){ robot.psi = Number(s.substring(1, s.length)); robot.cMotorChanged = true;} if(s.toUpperCase().includes("E")){ robot.e = Number(s.substring(1, s.length)); robot.eMotorChanged = true;} + if(s.toUpperCase().includes("F")){ robot.feedrate = Number(s.substring(1, s.length)); } }); } else if(g[0] == "M1" && !robot.moveRelative){ diff --git a/robot/Robot.js b/robot/Robot.js index be775e1..e11f675 100755 --- a/robot/Robot.js +++ b/robot/Robot.js @@ -44,6 +44,12 @@ class Robot{ /** @type {number} Finger-Abstands-Einstellung (Öffnungsweite) */ this.e = 0.0; + /** @type {number} Feedrate für Bewegungen in mm/min */ + this.feedrate = 200; // Standardwert + + /** @type {Object} Motor-Geschwindigkeiten in Einheiten pro Minute */ + this.motorSpeeds = {x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0}; + // Zwischen-Ergebnisse: Handgelenk-Punkt (Koordinaten des Handgelenks, nur für Tests public) /** @type {number} Handgelenk-Position X in mm (berechneter Zwischenwert) */ this.pX = 0.0; @@ -111,6 +117,10 @@ class Robot{ this.motorPosition.bMotorChanged = this.bMotorChanged; this.motorPosition.cMotorChanged = this.cMotorChanged; this.motorPosition.eMotorChanged = this.eMotorChanged; + + // Setze Geschwindigkeiten + this.motorPosition.speeds = {...this.motorSpeeds}; + this.motorPosition.feedrate = this.feedrate || 200; } // Berechnet aus XYZ die Motor-Winkel für den GCode @@ -178,6 +188,31 @@ class Robot{ this.eMotor = this.e - this.b - this.c; } + // Berechnet die Motor-Geschwindigkeiten basierend auf Feedrate und Positionsänderung + calculateSpeeds(oldPos, newPos) { + if (!oldPos || !newPos || this.feedrate <= 0) return; + + // Berechne Distanz im Raum (nur linear, Orientierung ignoriert für Einfachheit) + 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); + + if (dist === 0) return; // Keine Bewegung + + const time = dist / this.feedrate; // Zeit in Minuten + + // 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; + } + rotateAroundAxis(v, n, angle) { const cos = Math.cos(angle); const sin = Math.sin(angle); @@ -233,6 +268,10 @@ class Robot{ this.motorPositionOld = this.motorPosition; this.createMotorPosition() + // Berechne Geschwindigkeiten + this.calculateSpeeds(this.motorPositionOld, this.motorPosition); + this.motorPosition.speeds = {...this.motorSpeeds}; + console.log("Robot.sendCommand: 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)); this.cmdReceivers.forEach(receiver => { diff --git a/robot/RobotMotorPosition.js b/robot/RobotMotorPosition.js index 31a22e8..fb2ee07 100755 --- a/robot/RobotMotorPosition.js +++ b/robot/RobotMotorPosition.js @@ -21,6 +21,12 @@ module.exports = class RobotMotorPosition{ this.bMotorChanged = false; this.cMotorChanged = false; this.eMotorChanged = false; + + // Geschwindigkeiten in Einheiten pro Minute + this.speeds = {x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0}; + + // Feedrate in mm/min + this.feedrate = 0; } } diff --git a/robot/TelnetSenderGRBL.js b/robot/TelnetSenderGRBL.js index f6e42fa..20280c5 100755 --- a/robot/TelnetSenderGRBL.js +++ b/robot/TelnetSenderGRBL.js @@ -203,8 +203,9 @@ module.exports = class TelnetSenderGRBL{ if(this.tSocket && data.toString("utf-8").length > 3){ - if(strCommand == "G1"){ - data += " f"+(maxSpeedF.toFixed(2).toString()) + if(strCommand == "G1" && mNew){ + const feedrate = mNew.feedrate !== undefined ? mNew.feedrate : 2300; + data += " f"+(feedrate.toFixed(2).toString()) } if(!this.isTestMode){ console.log("" + this.urlGRBLstr + " gets the message: " + data.toString("utf-8"))} diff --git a/test/GCode.receiveList.test.js b/test/GCode.receiveList.test.js new file mode 100644 index 0000000..f301a62 --- /dev/null +++ b/test/GCode.receiveList.test.js @@ -0,0 +1,59 @@ +const Robot = require('../robot/Robot.js'); +const GCode = require('../robot/GCode.js'); + +describe("Robot Gcode receive", () => { + + beforeAll(() => { + jest.spyOn(console, 'log').mockImplementation(() => {}); + }); + + afterAll(() => { + jest.restoreAllMocks(); + }); + + const geometries = [ + { L1: 300, L2: 200, L3: 10 }, + { L1: 240, L2: 260, L3: 10 }, + { L1: 200, L2: 300, L3: 10 }, + { L1: 200, L2: 270, L3: 30 }, + { L1: 300, L2: 300, L3: 40 }, + { L1: 220, L2: 200, L3: 40 }, + ]; + + const poses = [ + // stabil mittig + { x: 100, y: 100, z: 0, phi: 0.3, theta: 1.0, psi: 0 }, + { x: 100, y: 200, z: 0, phi: 2, theta: 0.2, psi: 0 }, + { x: 100, y: 100, z: 30, phi: 0, theta: 0.2, psi: 0 }, + { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: 0 }, + { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: .3 }, + { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: .9 }, + { x: -100, y: 200, z: -40, phi: 0.1, theta: 1.2, psi: .9 }, + { x: -100, y: 200, z: -240, phi: 0.1, theta: 1.2, psi: .9 }, + { x: -100, y: 200, z: -240, phi: -0.5, theta: 1.2, psi: -0.9 }, + { x: -100, y: 200, z: -240, phi: -0.5, theta: Math.PI - 0.3, psi: .9 }, + ]; + + geometries.forEach(({ L1, L2, L3 }, gIndex) => { + + poses.forEach((pose, pIndex) => { + + test(`Roundtrip G${gIndex} P${pIndex}`, () => { + + const A = new Robot(L1, L2, L3); + + GCode.receiveGCode(A,`G90 G1 x${pose.x} y${pose.y} z${pose.z} a${pose.phi} b${pose.theta} c${pose.psi} f200`); + + var epsilon = 0.02 + + expect(A.x).toBeCloseTo(pose.x, epsilon); + expect(A.y).toBeCloseTo(pose.y, epsilon); + expect(A.z).toBeCloseTo(pose.z, epsilon); + expect(A.phi).toBeCloseTo(pose.phi, epsilon); + expect(A.theta).toBeCloseTo(pose.theta, epsilon); + expect(A.psi).toBeCloseTo(pose.psi, epsilon); + }); + }); + }); +}); + diff --git a/test/GCode.speed.test.js b/test/GCode.speed.test.js new file mode 100644 index 0000000..fcc933a --- /dev/null +++ b/test/GCode.speed.test.js @@ -0,0 +1,93 @@ +const GCode = require('../robot/GCode.js'); +const Robot = require('../robot/Robot.js'); +const TelnetSender = require('../robot/TelnetSenderGRBL.js'); + +describe('GCode Speed Tests', () => { + beforeAll(() => { + jest.spyOn(console, 'log').mockImplementation(() => {}); + }); + + afterAll(() => { + jest.restoreAllMocks(); + }); + + test('G1 with Feedrate F300 - TelnetSender1 should include f300', () => { + const L1 = 300; + const L2 = 300; + const L3 = 20; + + const robot = new Robot(L1, L2, L3); + + const telnetSender1 = new TelnetSender("test.test", 2300, "x", "y", "z"); + const telnetSender2 = new TelnetSender("test.test", 5000, "a", null, null); + const telnetSender3 = new TelnetSender("test.test", 5000, "c", "e", "b"); + + robot.cmdReceivers.push(telnetSender1); + robot.cmdReceivers.push(telnetSender2); + robot.cmdReceivers.push(telnetSender3); + + // G1 with F300 + GCode.receiveGCode(robot, "G1 x120 y100 z50 f300"); + + // Check telnetSender1 (x,y,z axes) + expect(telnetSender1.tSocket.written).toContain('G1'); + expect(telnetSender1.tSocket.written).toContain('f300'); + expect(telnetSender1.tSocket.written).toMatch(/f300\.00/); // Exact format + + // telnetSender2 and telnetSender3 might not have output if no changes + // But since x,y,z changed, telnetSender1 should have the command + }); + + test('G1 without Feedrate - should use default f200', () => { + const L1 = 300; + const L2 = 300; + const L3 = 20; + + const robot = new Robot(L1, L2, L3); + + const telnetSender1 = new TelnetSender("test.test", 2300, "x", "y", "z"); + + robot.cmdReceivers.push(telnetSender1); + + // G1 without F + GCode.receiveGCode(robot, "G1 x120 y100 z50"); + + // Check default feedrate + expect(telnetSender1.tSocket.written).toContain('G1'); + expect(telnetSender1.tSocket.written).toContain('f200'); + expect(telnetSender1.tSocket.written).toMatch(/f200\.00/); + }); + + test('G1 with different Feedrate F500 - multiple senders', () => { + const L1 = 300; + const L2 = 300; + const L3 = 20; + + const robot = new Robot(L1, L2, L3); + + const telnetSender1 = new TelnetSender("test.test", 2300, "x", "y", "z"); + const telnetSender2 = new TelnetSender("test.test", 5000, "a", null, null); + const telnetSender3 = new TelnetSender("test.test", 5000, "c", "e", "b"); + + robot.cmdReceivers.push(telnetSender1); + robot.cmdReceivers.push(telnetSender2); + robot.cmdReceivers.push(telnetSender3); + + // G1 with F500 + GCode.receiveGCode(robot, "G1 x120 y100 z50 a10 b20 c30 f500"); + + // telnetSender1 should have f500 + expect(telnetSender1.tSocket.written).toContain('f500'); + expect(telnetSender1.tSocket.written).toMatch(/f500\.00/); + + // telnetSender2 (a axis) should have f500 if a changed + if (telnetSender2.tSocket.written.length > 0) { + expect(telnetSender2.tSocket.written).toContain('f500'); + } + + // telnetSender3 (c,e,b axes) should have f500 if changed + if (telnetSender3.tSocket.written.length > 0) { + expect(telnetSender3.tSocket.written).toContain('f500'); + } + }); +}); \ No newline at end of file