calculatePositionFromMotorAngles ist jetzt OK

robot/Robot.js

@@ -66,7 +66,7 @@ class Robot{
     }
 
     // Berechnet aus XYZ die Motor-Winkel für den GCode
-    calculateAngles3D(){
+    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}
@@ -104,6 +104,8 @@ class Robot{
         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}
@@ -126,19 +128,58 @@ class Robot{
         while(this.a < -Math.PI){this.a += 2*Math.PI}
 
         this.eMotor = this.e - this.b - this.c;
+    }
+    
+    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(){
+    calculatePositionFromMotorAngles(verbose = false) {
 
-        // Hier kommt generierter Code! Das ist höchst Fraglich !!
-        this.x = this.xMotor - this.l3*Math.sin(this.theta)*Math.cos(this.phi);
-        this.y = this.l1*Math.cos(this.alpha) + this.l2*Math.cos(this.beta) - this.l3*Math.sin(this.theta)*Math.sin(this.phi);
-        this.z = this.l1*Math.sin(this.alpha) + this.l2*Math.sin(this.beta) - this.l3*Math.cos(this.theta);
-         // Das war der generierte Code
+        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(){
         if(this.motorPosition == null){this.createMotorPosition() }
         this.motorPositionOld = this.motorPosition;

robot/TelnetSenderGRBL.js

@@ -52,9 +52,11 @@ module.exports = class TelnetSenderGRBL{
 
     }
     
-    
-
     moveTo(mOld, mNew){
+        this.translateAxisNames(mOld, mNew, "G1")
+    }
+
+    translateAxisNames(mOld, mNew, strCommand = "G1"){
 
         // The Hand-Turn is not 1:1 to the Hand-Lift °     
         var factorTurnLift = 1.2;
@@ -65,7 +67,7 @@ module.exports = class TelnetSenderGRBL{
         // Hand-Open in mm
         var handOpenInMM = 1.0
 
-        var data = "G1" 
+        var data = strCommand; 
 
         if(this.xAxisGrbl == "x"){
             data += " x" + (mNew.x).toFixed(2).toString();