Initial commit
This commit is contained in:
152
robot/Robot.js
Executable file
152
robot/Robot.js
Executable file
@@ -0,0 +1,152 @@
|
||||
const MotorPosition = require('./RobotMotorPosition.js')
|
||||
const telnetSender = require('./TelnetSenderGRBL.js')
|
||||
|
||||
class Robot{
|
||||
|
||||
|
||||
constructor(l1, l2, l3) {
|
||||
this.speedX = 200; // mm/min
|
||||
this.speedY = 200;
|
||||
this.speedZ = 200;
|
||||
|
||||
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.phi = 0.0; // Euler-Winkel zwischen X-Achse - Laengengrad
|
||||
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: Hand Punkt (nur für Tests sind die Public)
|
||||
this.pX = 0.0;
|
||||
this.pY = 0.0;
|
||||
this.pZ = 0.0;
|
||||
|
||||
// Motor-Koordinaten - Schulter, Ellebogen, Hand-Dreher
|
||||
this.xMotor = 0;
|
||||
this.alpha = 0; // =Y Motor
|
||||
this.beta = 0; // =Z Motor = Winkel die der Unterarm unter der Y-Achse ist.
|
||||
|
||||
// Motor-Winkel fuer's Handgelenk
|
||||
this.a = 0; // aMotor am Ellebogen
|
||||
this.b = 0; // bMotor Handgelenk-Knicker
|
||||
this.c = 0; // cMotor Hand-Dreher
|
||||
|
||||
this.eMotor = 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.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);
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
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
|
||||
Reference in New Issue
Block a user