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

73
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);
const time = (dist === 0) ? 1/200 : dist / this.feedrate; // Zeit in Minuten
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;
}
// 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;
// 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);
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())
}