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

G92-Grad + E-Korrektur

Browse Source
This commit is contained in:
chk
2026-06-25 18:58:55 +02:00
parent 8deb7bb8a6
commit b96a538b89
18 changed files with 1369 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
robot/GCode.js
View File

@@ -40,20 +40,25 @@ class GCode{
static getM114(robot){
// position = Workspace (x/y/z in mm, a/b/c als Euler-Winkel phi/theta/psi in rad,
// e = Greifer-Öffnung in mm).
// motorCounts = die 7 Motor-Slots, inkl. e = eMotor (abgeleiteter Greifer-Motorwert,
// NICHT die mm-Öffnung — die steht in position.e).
let text = '{"position":{ "x":'+robot.x+
', "y":'+robot.y+
', "z":'+robot.z+
', "a":' +robot.phi +
', "b":' +robot.theta +
', "c":' +robot.psi + '},' +
', "a":' +robot.phi +
', "b":' +robot.theta +
', "c":' +robot.psi +
', "e":' +(robot.e ?? 0) + '},' +
'"motorCounts":{ "x":'+ robot.xMotor +
', "y":'+ robot.alpha +
', "y":'+ robot.alpha +
', "z":'+ robot.beta +
', "a":'+ robot.a +
', "b":'+ robot.b +
', "a":'+ robot.a +
', "b":'+ robot.b +
', "c":'+ robot.c +
', "e":'+ (robot.e ?? 0) +
'}}';
', "e":'+ (robot.eMotor ?? 0) +
'}}';
return text;
}

15
robot/RobotBase.js
View File

@@ -264,6 +264,21 @@ class RobotBase{
};
}
/**
* Greifer-Öffnung (Workspace, `e` in mm) → Greifer-Motorwert (`eMotor`).
*
* Default: keine Kopplung (`eMotor = e`). Kinematiken, bei denen die Greifer-Sehne
* mechanisch durchs Handgelenk läuft, überschreiben dies, um die Handgelenk-Winkel
* herauszurechnen (siehe {@link Arm3SegmentLinearX}). Wird sowohl von
* `calculateAngles3D()` als auch beim Setzen per G92/M92 genutzt — eine Quelle.
*
* @param {number} e Finger-Öffnung in mm (ab Null-Position)
* @returns {number} zugehöriger Greifer-Motorwert
*/
gripperMotorFromOpening(e) {
return e;
}
/**
* Rückwärts-Kinematik: Motorwinkel → Workspace-Koordinaten.
*

33
robot/RobotController.js
View File

@@ -8,7 +8,7 @@
* der Controller kennt nur strukturierte Befehle, keine rohen Textstrings.
*/
const GCodeParser = require('./GCodeParser');
const { motorStateFromPorts } = require('./portInverse');
const { motorStateFromPorts, D } = require('./portInverse');
class RobotController {
@@ -121,14 +121,31 @@ class RobotController {
}
if (cmd === 'M92' || cmd === 'G92') {
// Beide setzen die Motorposition ohne Bewegung, unterscheiden sich aber in den
// Winkel-EINHEITEN:
// G92 → GRAD (G-Code-Konvention für Rotationsachsen, wie FluidNC und die
// "Position Motoren"-Anzeige in public/app.js). Intern sind die
// Winkel-Slots in Radiant → Grad/D umrechnen (D = 180/π).
// M92 → RADIANT, roh in die internen Slots (interne/Test-Variante).
// X ist die lineare mm-Schiene, E die Greifer-Öffnung in mm (ab Null-Position
// eines Fingers) — beide ohne Winkel-Umrechnung.
const angScale = (cmd === 'G92') ? 1 / D : 1;
robot.createMotorPosition();
if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; }
if (Number.isFinite(params.Y)) { robot.alpha = params.Y; robot.yMotorChanged = true; }
if (Number.isFinite(params.Z)) { robot.beta = params.Z; robot.zMotorChanged = true; }
if (Number.isFinite(params.A)) { robot.a = params.A; robot.aMotorChanged = true; }
if (Number.isFinite(params.B)) { robot.b = params.B; robot.bMotorChanged = true; }
if (Number.isFinite(params.C)) { robot.c = params.C; robot.cMotorChanged = true; }
if (Number.isFinite(params.E)) { robot.e = params.E; robot.eMotorChanged = true; }
if (Number.isFinite(params.X)) { robot.xMotor = params.X; robot.xMotorChanged = true; }
if (Number.isFinite(params.Y)) { robot.alpha = params.Y * angScale; robot.yMotorChanged = true; }
if (Number.isFinite(params.Z)) { robot.beta = params.Z * angScale; robot.zMotorChanged = true; }
if (Number.isFinite(params.A)) { robot.a = params.A * angScale; robot.aMotorChanged = true; }
if (Number.isFinite(params.B)) { robot.b = params.B * angScale; robot.bMotorChanged = true; }
if (Number.isFinite(params.C)) { robot.c = params.C * angScale; robot.cMotorChanged = true; }
// E nach B/C setzen: der Greifer-Motorwert hängt über die Kinematik-Kopplung
// von b und c ab. robot.e = Finger-Öffnung (mm), eMotor = abgeleiteter Motorwert.
// Ohne diese eMotor-Ableitung bliebe der Greiferwert stale (alte E-Inkonsistenz):
// sendCommand() verschickt eMotor, nicht e.
if (Number.isFinite(params.E)) {
robot.e = params.E;
robot.eMotor = robot.gripperMotorFromOpening(robot.e);
robot.eMotorChanged = true;
}
robot.calculatePositionFromMotorAngles();
robot.sendCommand('G92');

12
robot/kinematics/Arm3SegmentLinearX.js
View File

@@ -95,7 +95,17 @@ class Arm3SegmentLinearX extends RobotBase {
while(this.a > Math.PI){this.a -= 2*Math.PI}
while(this.a < -Math.PI){this.a += 2*Math.PI}
this.eMotor = this.e - this.b - this.c;
this.eMotor = this.gripperMotorFromOpening(this.e);
}
/**
* Greifer-Kopplung dieses Arms: die Finger-Sehne läuft durchs Handgelenk, daher
* ziehen Knick (`b`) und Dreh (`c`) am Greifer mit. `eMotor` kompensiert das, damit
* die Finger-Öffnung `e` (mm, ab Null-Position eines Fingers) unabhängig von der
* Handstellung bleibt. Einzige Quelle für diese Kopplung (auch via G92/M92 genutzt).
*/
gripperMotorFromOpening(e) {
return e - this.b - this.c;
}
calculatePositionFromMotorAngles(verbose = false) {