From e9e50acf5fcec252ca7935ff8e31ef5bbb72b69d Mon Sep 17 00:00:00 2001 From: Kendel Christoph Date: Sat, 17 Jan 2026 15:45:43 +0100 Subject: [PATCH] Add Robot_JoyIt driver --- robot/Robot_JoyIt.js | 152 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 152 insertions(+) create mode 100755 robot/Robot_JoyIt.js diff --git a/robot/Robot_JoyIt.js b/robot/Robot_JoyIt.js new file mode 100755 index 0000000..b9ec9e2 --- /dev/null +++ b/robot/Robot_JoyIt.js @@ -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 \ No newline at end of file