112 lines
3.7 KiB
JavaScript
Executable File
112 lines
3.7 KiB
JavaScript
Executable File
const MotorPosition = require('./RobotMotorPosition.js')
|
|
|
|
class Robot{
|
|
|
|
constructor(l1, l2, l3) {
|
|
this.speedX = 100; // mm/min
|
|
this.speedY = 100;
|
|
this.speedZ = 100;
|
|
|
|
this.lastCommandSend = 0;
|
|
|
|
if(this.lastCommandSend == 0){ this.lastCommandSend = Date.now() };
|
|
this.doAnimate = false;
|
|
|
|
this.l1 = l1; // Oberarm
|
|
this.l2 = l2; // Unterarm
|
|
this.l3 = l3; // Hand-Länge
|
|
|
|
// Plan-Koordinaten - XYZ FingerSpitze
|
|
this.x = 0;
|
|
this.y = 0;
|
|
this.z = 0;
|
|
|
|
// Plan-Koordinaten - HandRichtung
|
|
this.theta = -Math.PI/2; // Euler-Winkel zwischen Z-Achse und P - Breitengrad
|
|
this.psi = 0.0; // Euler-Winkel: Drehung des Handgelenks abweichend vom Breitengrad
|
|
|
|
this.e = 0.0 // Finger-Distance
|
|
|
|
// Zwischen-Ergebnisse: Handgelenk Punkt (nur für Tests sind die Public)
|
|
this.Hz = 0.0;
|
|
this.Hr = 0.0;
|
|
|
|
this.OH = 0.0;
|
|
this.epsilon = 0.0;
|
|
|
|
this.r = 0.0; // Abstand in der XY-Ebene
|
|
this.alpha = 0.0;
|
|
this.beta = 0.0;
|
|
|
|
// Motor-Koordinaten in Grad
|
|
this.aMotor = 0; // Dreher
|
|
this.bMotor = 0; // Schulter
|
|
this.cMotor = 0; // Ellebogen
|
|
this.dMotor = 0; // Handgelenk Knick
|
|
this.eMotor = 0; // handgelenk Drehung
|
|
this.fMotor = 0; // eMotor Finger-Distanz
|
|
|
|
|
|
this.oldCommandTime = Date.now();
|
|
this.showFunctions = [];
|
|
|
|
this.savedPoints = [];
|
|
this.atPointNr = 0;
|
|
this.t = 0; // TimeStamp of this Point
|
|
|
|
this.moveRelative = true;
|
|
|
|
this.pythonSender = null;
|
|
this.cmdReceivers = [];
|
|
}
|
|
|
|
createMotorPosition(){
|
|
this.motorPosition = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);
|
|
}
|
|
|
|
// Berechnet aus XYZ die Motor-Winkel für den GCode
|
|
calculateAngles3D(){
|
|
|
|
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.phi = Math.atan2(this.y, this.x);
|
|
this.r = Math.sqrt(this.x*this.x + this.y*this.y);
|
|
|
|
this.Hr = this.r - this.l3*Math.cos(this.theta);
|
|
this.Hz = this.z + this.l3*Math.sin(this.theta);
|
|
|
|
this.epsilon = Math.atan2(this.Hz, this.Hr);
|
|
this.OH = Math.sqrt(this.Hr*this.Hr + this.Hz*this.Hz);
|
|
|
|
// Winkel für die Oberarm und Unterarm Dreiecksberechnung
|
|
this.alpha = Math.acos((this.l1*this.l1 + this.OH*this.OH - this.l2*this.l2) / (2*this.l1*this.OH));
|
|
this.beta = Math.acos((this.l1*this.l1 + this.l2*this.l2 - this.OH*this.OH) / (2*this.l1*this.l2));
|
|
|
|
this.eMotor = this.e;
|
|
}
|
|
|
|
calculatePositionFromMotorAngles(){
|
|
|
|
}
|
|
|
|
sendCommand(){
|
|
if(this.motorPosition == null){this.createMotorPosition() }
|
|
this.motorPositionOld = this.motorPosition;
|
|
this.createMotorPosition()
|
|
|
|
console.log("Robot.sendCommand: neue RobotMotorPosition: 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.moveTo(this.motorPositionOld, this.motorPosition);
|
|
});
|
|
}
|
|
}
|
|
|
|
|
|
|
|
module.exports = Robot // Export class
|