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

Speed testen (anfang)

Browse Source
This commit is contained in:
ChK
2026-04-10 07:20:23 +02:00
parent 753952d4ac
commit 9328747fe6
5 changed files with 121 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
robot/Robot.js
View File

@@ -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
// 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;
if (handgelenk_dist > 0.001) {
const time = handgelenk_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;
}
// 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) {

5
robot/RobotMotorPosition.js
View File

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

6
robot/TelnetSenderGRBL.js
View File

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

39
test/GCode.speed.test.js
View File

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

13
test/Robot.sendCommand.test.js
View File

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