336 lines
14 KiB
JavaScript
336 lines
14 KiB
JavaScript
const MotorPosition = require('./RobotMotorPosition.js')
|
|
|
|
/**
|
|
* RobotBase — abstrakte Basisklasse und zugleich der Interface-Vertrag des
|
|
* Frameworks. Enthält die roboter-unabhängige Infrastruktur (Zustand,
|
|
* `sendCommand`, `createMotorPosition`, `calculateSpeeds`, `rotateAroundAxis`).
|
|
*
|
|
* Der arm-spezifische Teil sind ausschließlich die beiden Kinematik-Methoden
|
|
* {@link RobotBase#calculateAngles3D} und
|
|
* {@link RobotBase#calculatePositionFromMotorAngles}. Sie sind hier abstrakt
|
|
* (werfen) und MÜSSEN von jeder konkreten Kinematik-Klasse überschrieben werden.
|
|
*
|
|
* Verträge (siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md):
|
|
* - Workspace-Koordinaten: `x, y, z, phi, theta, psi` + `e` (Greifer) — 6-DOF.
|
|
* - Motor-Zustand: 7 feste Slots `xMotor, alpha, beta, a, b, c, eMotor`.
|
|
*
|
|
* Konkrete Implementierungen leben in `robot/kinematics/`.
|
|
*
|
|
* @abstract
|
|
*/
|
|
class RobotBase{
|
|
|
|
|
|
constructor(config = {}) {
|
|
// config-Werte haben Vorrang, Env-Variablen als Fallback (Kompatibilität).
|
|
const DEFAULT_FEEDRATE = config.defaultFeedrate
|
|
?? (process.env.ROBOT_DEFAULT_FEEDRATE ? Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000);
|
|
// Speed-Regelung-Schalter: 'legacy' (Default — exakt wie bisher) oder 'correct'.
|
|
// Siehe doc/ToDo_6a_Speed.md.
|
|
this.speedMode = (config.speedMode || process.env.ROBOT_SPEED_MODE || 'legacy').toLowerCase();
|
|
// ROBOT_USE_SPEED_CALC bleibt der interne Schalter für calculateSpeeds();
|
|
// der Korrekt-Modus aktiviert die Berechnung automatisch.
|
|
this.useSpeedCalc = config.useSpeedCalc
|
|
?? (this.speedMode === 'correct' ||
|
|
process.env.ROBOT_USE_SPEED_CALC === 'true' ||
|
|
process.env.ROBOT_USE_SPEED_CALC === '1');
|
|
/** @type {number} Bewegungszeit des letzten Schritts in Minuten (für koordinierte Feedrate) */
|
|
this.lastMoveTime = 0;
|
|
|
|
/** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */
|
|
this.speedX = 200;
|
|
/** @type {number} Bewegungsgeschwindigkeit Y-Achse in mm/min */
|
|
this.speedY = 200;
|
|
/** @type {number} Bewegungsgeschwindigkeit Z-Achse in mm/min */
|
|
this.speedZ = 200;
|
|
|
|
/** @type {number} Zeitstempel des zuletzt gesendeten Kommandos */
|
|
this.lastCommandSend = 0;
|
|
|
|
if(this.lastCommandSend == 0){ this.lastCommandSend = Date.now() };
|
|
/** @type {boolean} Animation aktiviert */
|
|
this.doAnimate = false;
|
|
|
|
// Plan-Koordinaten - XYZ FingerSpitze
|
|
/** @type {number} X-Position der Fingerspitze in mm */
|
|
this.x = 0;
|
|
/** @type {number} Y-Position der Fingerspitze in mm */
|
|
this.y = 0;
|
|
/** @type {number} Z-Position der Fingerspitze in mm */
|
|
this.z = 0;
|
|
|
|
// Plan-Koordinaten - HandRichtung (Euler-Winkel)
|
|
/** @type {number} Phi - Euler-Winkel (Längengrad): Rotation um Z-Achse in rad */
|
|
this.phi = 0.0;
|
|
/** @type {number} Theta - Euler-Winkel (Breitengrad): Neigungswinkel der Handachse in rad */
|
|
this.theta = -Math.PI/2;
|
|
/** @type {number} Psi - Euler-Winkel: Zusätzliche Drehung des Handgelenks in rad */
|
|
this.psi = 0.0;
|
|
|
|
/** @type {number} Finger-Abstands-Einstellung (Öffnungsweite) */
|
|
this.e = 0.0;
|
|
|
|
/** @type {number} Feedrate für Bewegungen in mm/min */
|
|
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};
|
|
|
|
// 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;
|
|
/** @type {number} Handgelenk-Position Y in mm (berechneter Zwischenwert) */
|
|
this.pY = 0.0;
|
|
/** @type {number} Handgelenk-Position Z in mm (berechneter Zwischenwert) */
|
|
this.pZ = 0.0;
|
|
|
|
// Motor-Koordinaten - Schulter, Ellebogen, Hand-Dreher
|
|
/** @type {number} X-Motor-Position (Schulterposition auf X-Schiene) in mm */
|
|
this.xMotor = 0;
|
|
/** @type {number} Alpha - Y-Motor-Winkel (Schulterposition) in rad */
|
|
this.alpha = 0;
|
|
/** @type {number} Beta - Z-Motor-Winkel (Unterarm-Neigung unter Y-Achse) in rad */
|
|
this.beta = 0;
|
|
|
|
this.xMotorChanged = false;
|
|
this.yMotorChanged = false;
|
|
this.zMotorChanged = false;
|
|
|
|
// Motor-Winkel für's Handgelenk
|
|
/** @type {number} a-Motor-Winkel: Rotation am Ellbogen in rad */
|
|
this.a = 0;
|
|
/** @type {number} b-Motor-Winkel: Handgelenk-Knicker-Winkel in rad */
|
|
this.b = 0;
|
|
/** @type {number} c-Motor-Winkel: Hand-Dreher-Rotation in rad */
|
|
this.c = 0;
|
|
|
|
this.aMotorChanged = false;
|
|
this.bMotorChanged = false;
|
|
this.cMotorChanged = false;
|
|
this.eMotorChanged = false;
|
|
|
|
/** @type {number} e-Motor-Wert: Finger-Abstands-Motor-Position */
|
|
this.eMotor = 0;
|
|
|
|
/** @type {number} Zeitstempel des letzten verarbeiteten Kommandos */
|
|
this.oldCommandTime = Date.now();
|
|
/** @type {Function[]} Array von Visualisierungs-Funktionen */
|
|
this.showFunctions = [];
|
|
|
|
/** @type {Object[]} Gespeicherte Roboterpositionen/Punkte */
|
|
this.savedPoints = [];
|
|
/** @type {number} Index des aktuell angesteuerten Punktes */
|
|
this.atPointNr = 0;
|
|
/** @type {number} Zeitstempel des aktuellen Punktes in ms */
|
|
this.t = 0;
|
|
|
|
/** @type {boolean} Relative oder absolute Bewegung (true = relativ) */
|
|
this.moveRelative = true;
|
|
|
|
/** @type {Object[]} Array von Kommando-Empfängern */
|
|
this.cmdReceivers = [];
|
|
}
|
|
|
|
createMotorPosition(){
|
|
this.motorPosition = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);
|
|
|
|
// Setze Changed-Flags basierend auf Änderungen seit der letzten Position
|
|
this.motorPosition.xMotorChanged = this.motorPositionOld ? this.xMotor !== this.motorPositionOld.x : true;
|
|
this.motorPosition.yMotorChanged = this.motorPositionOld ? this.alpha !== this.motorPositionOld.y : true;
|
|
this.motorPosition.zMotorChanged = this.motorPositionOld ? this.beta !== this.motorPositionOld.z : true;
|
|
this.motorPosition.aMotorChanged = this.motorPositionOld ? this.a !== this.motorPositionOld.a : true;
|
|
this.motorPosition.bMotorChanged = this.motorPositionOld ? this.b !== this.motorPositionOld.b : true;
|
|
this.motorPosition.cMotorChanged = this.motorPositionOld ? this.c !== this.motorPositionOld.c : true;
|
|
this.motorPosition.eMotorChanged = this.motorPositionOld ? this.eMotor !== this.motorPositionOld.e : true;
|
|
|
|
// 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;
|
|
|
|
// Speed-Regelung: Modus und (vorläufige) Bewegungszeit für die Sender
|
|
this.motorPosition.speedMode = this.speedMode;
|
|
this.motorPosition.moveTime = this.lastMoveTime || 0;
|
|
}
|
|
|
|
/**
|
|
* Vorwärts-Kinematik: Workspace-Koordinaten → Motorwinkel.
|
|
*
|
|
* Liest `this.x/y/z/phi/theta/psi/e` und schreibt das Ergebnis auf die
|
|
* Motor-Slots `this.xMotor/alpha/beta/a/b/c/eMotor` (sowie die Zwischenwerte
|
|
* `this.pX/pY/pZ` des Handgelenk-Punkts).
|
|
*
|
|
* ABSTRAKT — muss von der konkreten Kinematik-Klasse überschrieben werden.
|
|
*
|
|
* @abstract
|
|
* @param {boolean} [verbose] Debug-Ausgaben aktivieren
|
|
*/
|
|
calculateAngles3D(verbose){
|
|
throw new Error('calculateAngles3D() not implemented — RobotBase ist abstrakt; eine konkrete Kinematik-Klasse muss diese Methode überschreiben.');
|
|
}
|
|
|
|
// 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;
|
|
|
|
this.lastMoveTime = 0; // wird unten gesetzt, sobald eine Bewegung erkannt wird
|
|
|
|
// 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 xyz_dist = Math.sqrt(dx*dx + dy*dy + dz*dz);
|
|
|
|
if (xyz_dist > 0.001) {
|
|
const time = xyz_dist / this.feedrate;
|
|
this.lastMoveTime = time;
|
|
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
|
|
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
|
|
this.motorSpeeds.z = (this.beta - oldPos.z) / 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;
|
|
}
|
|
|
|
// 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.lastMoveTime = time;
|
|
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
|
|
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
|
|
this.motorSpeeds.z = (this.beta - oldPos.z) / 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.lastMoveTime = time;
|
|
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
|
|
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
|
|
this.motorSpeeds.z = (this.beta - oldPos.z) / 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 AUF DEFAULT
|
|
|
|
|
|
// ToDo: Aus motorSpeed mit einzelnenen Werten muss noch die feedrate berechnet werden.
|
|
// hier bin ich unsicher, ob das nicht in den Sender rein sollte, da es eventuell
|
|
// abhngig vom FluidNC und dessen speed interpretation ist.
|
|
}
|
|
|
|
/**
|
|
* Geschützte Hilfsmethode (Rodrigues-Rotation): dreht den Vektor `v` um die
|
|
* (normierte) Achse `n` um den Winkel `angle`. Für Kinematik-Implementierungen.
|
|
*/
|
|
rotateAroundAxis(v, n, angle) {
|
|
const cos = Math.cos(angle);
|
|
const sin = Math.sin(angle);
|
|
|
|
const dot = v.x*n.x + v.y*n.y + v.z*n.z;
|
|
|
|
const cross = {
|
|
x: n.y*v.z - n.z*v.y,
|
|
y: n.z*v.x - n.x*v.z,
|
|
z: n.x*v.y - n.y*v.x
|
|
};
|
|
|
|
return {
|
|
x: v.x*cos + cross.x*sin + n.x*dot*(1 - cos),
|
|
y: v.y*cos + cross.y*sin + n.y*dot*(1 - cos),
|
|
z: v.z*cos + cross.z*sin + n.z*dot*(1 - cos)
|
|
};
|
|
}
|
|
|
|
/**
|
|
* 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.
|
|
*
|
|
* Liest die Motor-Slots `this.xMotor/alpha/beta/a/b/c` und schreibt das
|
|
* Ergebnis auf `this.x/y/z/phi/theta/psi` (sowie `this.pX/pY/pZ`).
|
|
*
|
|
* ABSTRAKT — muss von der konkreten Kinematik-Klasse überschrieben werden.
|
|
*
|
|
* @abstract
|
|
* @param {boolean} [verbose] Debug-Ausgaben aktivieren
|
|
*/
|
|
calculatePositionFromMotorAngles(verbose = false) {
|
|
throw new Error('calculatePositionFromMotorAngles() not implemented — RobotBase ist abstrakt; eine konkrete Kinematik-Klasse muss diese Methode überschreiben.');
|
|
}
|
|
|
|
sendCommand(cmd="G1"){
|
|
const isFirstCall = !this.motorPosition;
|
|
|
|
if (isFirstCall) {
|
|
this.motorPositionOld = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);
|
|
} else {
|
|
this.motorPositionOld = this.motorPosition;
|
|
}
|
|
|
|
this.createMotorPosition()
|
|
|
|
// Für den ersten Aufruf setze alle Changed-Flags auf true
|
|
if (isFirstCall) {
|
|
this.motorPosition.xMotorChanged = true;
|
|
this.motorPosition.yMotorChanged = true;
|
|
this.motorPosition.zMotorChanged = true;
|
|
this.motorPosition.aMotorChanged = true;
|
|
this.motorPosition.bMotorChanged = true;
|
|
this.motorPosition.cMotorChanged = true;
|
|
this.motorPosition.eMotorChanged = true;
|
|
}
|
|
|
|
// Berechne Geschwindigkeiten
|
|
this.calculateSpeeds(this.motorPositionOld, this.motorPosition);
|
|
this.motorPosition.speeds = {...this.motorSpeeds};
|
|
// moveTime nach der Berechnung aktualisieren (createMotorPosition lief davor)
|
|
this.motorPosition.moveTime = this.lastMoveTime || 0;
|
|
|
|
console.log("Robot.sendCommand: ", cmd.toString(), " 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 => {
|
|
receiver.execCommand(cmd,this.motorPositionOld, this.motorPosition);
|
|
});
|
|
}
|
|
}
|
|
|
|
|
|
|
|
module.exports = RobotBase // Export abstrakte Basisklasse / Interface
|