352 lines
15 KiB
JavaScript
Executable File
352 lines
15 KiB
JavaScript
Executable File
const MotorPosition = require('./RobotMotorPosition.js')
|
|
|
|
class Robot{
|
|
|
|
|
|
constructor(l1, l2, l3) {
|
|
// Umgebungsvariablen-Logik
|
|
const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ?
|
|
Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000;
|
|
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 */
|
|
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;
|
|
|
|
/** @type {number} Länge des Oberarms in mm */
|
|
this.l1 = l1;
|
|
/** @type {number} Länge des Unterarms in mm */
|
|
this.l2 = l2;
|
|
/** @type {number} Länge der Hand (Endeffector) in mm */
|
|
this.l3 = l3;
|
|
|
|
// 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|null} Python-Sender-Instanz für GCode-Kommunikation */
|
|
this.pythonSender = null;
|
|
/** @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;
|
|
}
|
|
|
|
// Berechnet aus XYZ die Motor-Winkel für den GCode
|
|
calculateAngles3D(verbose){
|
|
while(this.phi > Math.PI){this.phi -= 2*Math.PI}
|
|
while(this.phi < -Math.PI){this.phi += 2*Math.PI}
|
|
while(this.theta > Math.PI){this.theta -= 2*Math.PI}
|
|
while(this.theta < -Math.PI){this.theta += 2*Math.PI}
|
|
|
|
// Handgelenk-Punkt ausrechnen:
|
|
this.pX = this.x + this.l3*Math.sin(this.theta)*Math.cos(this.phi);
|
|
this.pY = this.y + this.l3*Math.sin(this.theta)*Math.sin(this.phi);
|
|
this.pZ = this.z + this.l3*Math.cos(this.theta);
|
|
|
|
var pX = this.pX;
|
|
var pY = this.pY;
|
|
var pZ = this.pZ;
|
|
|
|
this.xMotor = pX;
|
|
// Ziel-Punkt ausrechnen ==> 2D Rechnung Arm
|
|
var r = Math.sqrt(pY * pY + pZ * pZ);
|
|
|
|
if (r > (this.l1 + this.l2)) { return; }
|
|
if (r == 0) { return; }
|
|
|
|
var gamma = Math.asin(pZ / r);
|
|
var delta = Math.acos((this.l1 * this.l1 + this.l2 * this.l2 - r * r) / (2 * this.l1 * this.l2));
|
|
this.alpha = Math.acos((this.l1 * this.l1 + r * r - this.l2 * this.l2) / (2 * r * this.l1)) + gamma;
|
|
this.beta = -Math.PI + (this.alpha + delta);
|
|
// Ende <== 2D Rechnung Arm
|
|
|
|
// Richtung der Hand ausgerechnet
|
|
// Arm = (0, cos(beta), sin(beta)) Punkt = (sin(theta)cos(phi), sin(theta)sin(phi), cos(theta))
|
|
//
|
|
// Unterarm muss gedreht werden. Aus der Y-Z-Ebene raus. Hin in die Ebene n x r
|
|
// wobei n = Unterarm x (P-O) ist
|
|
var nX = Math.cos(this.beta)*Math.cos(this.theta) - Math.sin(this.theta)*Math.sin(this.phi)*Math.sin(this.beta);
|
|
var nY = Math.sin(this.beta)*Math.sin(this.theta)*Math.cos(this.phi);
|
|
var nZ = -1.0*Math.sin(this.theta)*Math.cos(this.phi)*Math.cos(this.beta);
|
|
var nBetrag = Math.sqrt(nX*nX + nY*nY + nZ*nZ);
|
|
|
|
if(verbose) console.log("Richtung: > ", nX/nBetrag, nY/nBetrag, nZ/nBetrag);
|
|
|
|
var cosA = (nX)/nBetrag;
|
|
this.a = Math.acos(cosA)
|
|
if(Math.cos(this.phi) > 0){this.a = -this.a}
|
|
if(Math.sin(this.theta) < 0) {this.a = -this.a}
|
|
|
|
|
|
// Handgelenk-Knick-Winkel ist zwischen Arm und Punkt-O
|
|
var cosB = Math.cos(this.beta)*Math.sin(this.theta)*Math.sin(this.phi) + Math.sin(this.beta)*Math.cos(this.theta);
|
|
this.b = Math.acos(cosB);
|
|
|
|
|
|
// Winkel zwischen n und z muss rumgedreht werden.
|
|
var cosC = - nZ / nBetrag;
|
|
this.c = Math.acos(cosC);
|
|
this.c += this.psi;
|
|
|
|
// a um 180° drehen
|
|
this.a += Math.PI;
|
|
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;
|
|
}
|
|
|
|
// 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;
|
|
|
|
// 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.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;
|
|
}
|
|
|
|
// 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 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.
|
|
}
|
|
|
|
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)
|
|
};
|
|
}
|
|
|
|
calculatePositionFromMotorAngles(verbose = false) {
|
|
|
|
const vecBizeps = {x: this.xMotor, y: this.l1 * Math.cos(this.alpha), z: this.l1 * Math.sin(this.alpha)}
|
|
const vecUnterarm = {x: 0, y: Math.cos(this.beta), z: Math.sin(this.beta)}
|
|
|
|
// der Handgelenk-Punkt
|
|
this.pX = vecBizeps.x + this.l2 * vecUnterarm.x;
|
|
this.pY = vecBizeps.y + this.l2 * vecUnterarm.y;
|
|
this.pZ = vecBizeps.z + this.l2 * vecUnterarm.z;
|
|
|
|
// n: Die Handgelenk-Unterarm-Knick-Achse. X-Achse wird um den Unterarm gedreht.
|
|
const n = { x: -Math.cos(this.a), y: vecUnterarm.z * Math.sin(this.a), z: -vecUnterarm.y * Math.sin(this.a) };
|
|
|
|
if(verbose) console.log("n inverse:", n.x, n.y, n.z);
|
|
|
|
const vHand = this.rotateAroundAxis(vecUnterarm, n, this.b);
|
|
|
|
this.x = this.pX - this.l3 * vHand.x;
|
|
this.y = this.pY - this.l3 * vHand.y;
|
|
this.z = this.pZ - this.l3 * vHand.z;
|
|
|
|
this.theta = Math.atan2(Math.sqrt(vHand.x*vHand.x + vHand.y*vHand.y),vHand.z);
|
|
this.phi = Math.atan2(vHand.y, vHand.x);
|
|
this.psi = this.c - Math.acos(-n.z);
|
|
|
|
while(this.phi > Math.PI){this.phi -= 2*Math.PI}
|
|
while(this.phi < -Math.PI){this.phi += 2*Math.PI}
|
|
while(this.theta > Math.PI){this.theta -= 2*Math.PI}
|
|
while(this.theta < -Math.PI){this.theta += 2*Math.PI}
|
|
}
|
|
|
|
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};
|
|
|
|
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 => {
|
|
receiver.execCommand(cmd,this.motorPositionOld, this.motorPosition);
|
|
});
|
|
}
|
|
}
|
|
|
|
|
|
|
|
module.exports = Robot // Export class
|