ChK/appRobotDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Test vom GCode

Browse Source
This commit is contained in:
ChK
2026-04-08 08:35:42 +02:00
parent ff7cde40ac
commit 249f7447bb
6 changed files with 202 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
robot/GCode.js
View File

@@ -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){

39
robot/Robot.js
View File

@@ -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 => {

6
robot/RobotMotorPosition.js
View File

@@ -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;
}
}

5
robot/TelnetSenderGRBL.js
View File

@@ -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"))}

59
test/GCode.receiveList.test.js Normal file
View File

@@ -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);
});
});
});
});

93
test/GCode.speed.test.js Normal file
View File

@@ -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');
}
});
});