Initial commit
This commit is contained in:
318
robot/GCode.js
Executable file
318
robot/GCode.js
Executable file
@@ -0,0 +1,318 @@
|
||||
/***
|
||||
* Receives GCode, processes it and moves the Data to the Roboter-Class
|
||||
*/
|
||||
const fs = require('fs');
|
||||
|
||||
|
||||
class GCode{
|
||||
|
||||
static fileName = "GCodeFiles/log.gcode";
|
||||
|
||||
static containsMCode(s){
|
||||
return s.indexOf('M1') == 0
|
||||
}
|
||||
|
||||
static receiveMCode(robot, m){
|
||||
|
||||
m = m.split(" ");
|
||||
if(m[0] == "M1"){
|
||||
m.forEach((s) => {
|
||||
if(s.includes("X")){ robot.xMotor += Number(s.substring(1, s.length));}
|
||||
if(s.includes("x")){ robot.xMotor += Number(s.substring(1, s.length));}
|
||||
|
||||
|
||||
if(s.includes("Y")){ robot.alpha += Number(s.substring(1, s.length));}
|
||||
if(s.includes("y")){ robot.alpha += Number(s.substring(1, s.length));}
|
||||
|
||||
|
||||
if(s.includes("Z")){ robot.beta += Number(s.substring(1, s.length));}
|
||||
if(s.includes("z")){ robot.beta += Number(s.substring(1, s.length));}
|
||||
})
|
||||
}
|
||||
|
||||
console.log("Empfangene Motor-Position (" + robot.xMotor.toPrecision(3)+ ", "+ robot.alpha.toPrecision(3)+ ", "+ robot.beta.toPrecision(3)+ ", "+ robot.a.toPrecision(5) + ", "+ robot.b.toPrecision(5)+ ", "+ robot.c.toPrecision(5)+ ") ");
|
||||
robot.sendCommand();
|
||||
}
|
||||
|
||||
static containsCommand(s){
|
||||
if(s.indexOf('M1 ') !== -1){return true;} // M1-Commands = G1-Command only for Motor-Coordinates
|
||||
if(s.indexOf('G') !== 0){return false;}
|
||||
if(s.indexOf('G90') == 0){return true;}
|
||||
if(s.indexOf('G91') == 0){return true;}
|
||||
if(s.indexOf('G28') !== -1){return true;}
|
||||
if(s.indexOf('G1 ') !== -1){return true;}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
static getM114(robot){
|
||||
let text = '{"position":{ "x":'+robot.x+
|
||||
', "y":'+robot.y+
|
||||
', "z":'+robot.z+
|
||||
', "a":' +robot.phi +
|
||||
', "b":' +robot.theta +
|
||||
', "c":' +robot.psi + '},' +
|
||||
'"motorCounts":{ "x":'+ robot.xMotor +
|
||||
', "y":'+ robot.alpha +
|
||||
', "z":'+ robot.beta +
|
||||
', "a":'+ robot.a +
|
||||
', "b":'+ robot.b +
|
||||
', "c":'+ robot.c +
|
||||
', "e":'+ 0.0 +
|
||||
'}}';
|
||||
return text;
|
||||
}
|
||||
|
||||
|
||||
static toPiMultiple(gCode){
|
||||
if(gCode == undefined){return gCode;}
|
||||
|
||||
var multipleParameters = gCode.split(" ");
|
||||
var newGString = "";
|
||||
multipleParameters.forEach((paramet) => {
|
||||
if(paramet == undefined || !paramet || paramet.length === 0 || paramet == " " || paramet ==""){
|
||||
|
||||
}
|
||||
else if(['A', 'a', 'B', 'b', 'C', 'c','E','e'].includes(paramet.charAt(0)) )
|
||||
{
|
||||
var numberParameter = Number(paramet.substring(1, paramet.length))
|
||||
newGString += " " + paramet[0] + String(numberParameter*Math.PI/180)
|
||||
}
|
||||
else{
|
||||
newGString += " " + paramet;
|
||||
}
|
||||
});
|
||||
return newGString.trim();
|
||||
};
|
||||
|
||||
static receiveGCode(robot, g){
|
||||
|
||||
if(g == undefined) return;
|
||||
|
||||
var multipleCommands = g.split(" G");
|
||||
var doProcessRest = false;
|
||||
if(multipleCommands.length > 1){
|
||||
doProcessRest = true;
|
||||
g = multipleCommands[0];
|
||||
}
|
||||
//console.log("Command: " + g);
|
||||
|
||||
g = g.split(" ");
|
||||
|
||||
var calculateNew = true;
|
||||
var calculateFromMotorCoordinates = false;
|
||||
|
||||
if(g[0] == "G90") {robot.moveRelative = false; calculateNew = false;}
|
||||
else if(g[0] == "G91") {robot.moveRelative = true; calculateNew = false;}
|
||||
else if(g[0] == "G28") {
|
||||
robot.x = 0;
|
||||
robot.y = robot.l1 + robot.l2 + robot.l3;
|
||||
robot.z = 0;
|
||||
robot.phi = -Math.PI/2;
|
||||
robot.theta = Math.PI/2;
|
||||
robot.psi = 0;
|
||||
robot.e = 0;
|
||||
}
|
||||
else if(g[0] == "G1" && robot.moveRelative){
|
||||
g.forEach((s) => {
|
||||
if(s.includes("X")){ robot.x += Number(s.substring(1, s.length));}
|
||||
if(s.includes("x")){ robot.x += Number(s.substring(1, s.length));}
|
||||
if(s.includes("Y")){ robot.y += Number(s.substring(1, s.length));}
|
||||
if(s.includes("y")){ robot.y += Number(s.substring(1, s.length));}
|
||||
if(s.includes("Z")){ robot.z += Number(s.substring(1, s.length));}
|
||||
if(s.includes("z")){ robot.z += Number(s.substring(1, s.length));}
|
||||
|
||||
|
||||
// abc in 2Pi-Angles
|
||||
if(s.includes("A")){ robot.phi += Number(s.substring(1, s.length));}
|
||||
if(s.includes("a")){ robot.phi += Number(s.substring(1, s.length));}
|
||||
if(s.includes("B")){ robot.theta += Number(s.substring(1, s.length));}
|
||||
if(s.includes("b")){ robot.theta += Number(s.substring(1, s.length));}
|
||||
if(s.includes("C")){ robot.psi += Number(s.substring(1, s.length));}
|
||||
if(s.includes("c")){ robot.psi += Number(s.substring(1, s.length));}
|
||||
|
||||
if(s.includes("E")){ robot.e += Number(s.substring(1, s.length));}
|
||||
if(s.includes("e")){ robot.e += Number(s.substring(1, s.length));}
|
||||
});
|
||||
}
|
||||
else if(g[0] == "M1" && robot.moveRelative){
|
||||
calculateFromMotorCoordinates = true;
|
||||
g.forEach((s) => {
|
||||
if(s.includes("X")){ robot.xMotor += Number(s.substring(1, s.length));}
|
||||
if(s.includes("x")){ robot.xMotor += Number(s.substring(1, s.length));}
|
||||
if(s.includes("Y")){ robot.alpha += Number(s.substring(1, s.length));}
|
||||
if(s.includes("y")){ robot.alpha += Number(s.substring(1, s.length));}
|
||||
if(s.includes("Z")){ robot.beta += Number(s.substring(1, s.length));}
|
||||
if(s.includes("z")){ robot.beta += Number(s.substring(1, s.length));}
|
||||
|
||||
|
||||
if(s.includes("A")){ robot.a += Number(s.substring(1, s.length));}
|
||||
if(s.includes("a")){ robot.a += Number(s.substring(1, s.length));}
|
||||
if(s.includes("B")){ robot.b += Number(s.substring(1, s.length));}
|
||||
if(s.includes("b")){ robot.b += Number(s.substring(1, s.length));}
|
||||
if(s.includes("C")){ robot.c += Number(s.substring(1, s.length));}
|
||||
if(s.includes("c")){ robot.c += Number(s.substring(1, s.length));}
|
||||
|
||||
if(s.includes("E")){ robot.e += Number(s.substring(1, s.length));}
|
||||
if(s.includes("e")){ robot.e += Number(s.substring(1, s.length));}
|
||||
});
|
||||
}
|
||||
// Absolute-Positioning
|
||||
else if(g[0] == "G1" && !robot.moveRelative){
|
||||
g.forEach((s) => {
|
||||
if(s.includes("X")){ robot.x = Number(s.substring(1, s.length));}
|
||||
if(s.includes("x")){ robot.x = Number(s.substring(1, s.length));}
|
||||
if(s.includes("Y")){ robot.y = Number(s.substring(1, s.length));}
|
||||
if(s.includes("y")){ robot.y = Number(s.substring(1, s.length));}
|
||||
if(s.includes("Z")){ robot.z = Number(s.substring(1, s.length));}
|
||||
if(s.includes("z")){ robot.z = Number(s.substring(1, s.length));}
|
||||
|
||||
if(s.includes("A")){ robot.phi = Number(s.substring(1, s.length));}
|
||||
if(s.includes("a")){ robot.phi = Number(s.substring(1, s.length));}
|
||||
if(s.includes("B")){ robot.theta = Number(s.substring(1, s.length));}
|
||||
if(s.includes("b")){ robot.theta = Number(s.substring(1, s.length));}
|
||||
if(s.includes("C")){ robot.psi = Number(s.substring(1, s.length));}
|
||||
if(s.includes("c")){ robot.psi = Number(s.substring(1, s.length));}
|
||||
|
||||
if(s.includes("E")){ robot.e = Number(s.substring(1, s.length));}
|
||||
if(s.includes("e")){ robot.e = Number(s.substring(1, s.length));}
|
||||
});
|
||||
}
|
||||
else if(g[0] == "M1" && !robot.moveRelative){
|
||||
calculateFromMotorCoordinates = true;
|
||||
g.forEach((s) => {
|
||||
if(s.includes("X")){ robot.xMotor = Number(s.substring(1, s.length));}
|
||||
if(s.includes("x")){ robot.xMotor = Number(s.substring(1, s.length));}
|
||||
if(s.includes("Y")){ robot.alpha = Number(s.substring(1, s.length));}
|
||||
if(s.includes("y")){ robot.alpha = Number(s.substring(1, s.length));}
|
||||
if(s.includes("Z")){ robot.beta = Number(s.substring(1, s.length));}
|
||||
if(s.includes("z")){ robot.beta = Number(s.substring(1, s.length));}
|
||||
|
||||
if(s.includes("A")){ robot.a = Number(s.substring(1, s.length));}
|
||||
if(s.includes("a")){ robot.a = Number(s.substring(1, s.length));}
|
||||
if(s.includes("B")){ robot.b = Number(s.substring(1, s.length));}
|
||||
if(s.includes("b")){ robot.b = Number(s.substring(1, s.length));}
|
||||
if(s.includes("C")){ robot.c = Number(s.substring(1, s.length));}
|
||||
if(s.includes("c")){ robot.c = Number(s.substring(1, s.length));}
|
||||
|
||||
if(s.includes("E")){ robot.e = Number(s.substring(1, s.length));}
|
||||
if(s.includes("e")){ robot.e = Number(s.substring(1, s.length));}
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
if(calculateNew && !calculateFromMotorCoordinates){
|
||||
robot.calculateAngles3D();
|
||||
robot.sendCommand();
|
||||
}
|
||||
|
||||
if(calculateFromMotorCoordinates){
|
||||
robot.calculatePositionFromMotorAngles();
|
||||
robot.sendCommand();
|
||||
}
|
||||
|
||||
|
||||
// verarbeitet alle weiteren Commands in dieser Zeile.
|
||||
if(doProcessRest){
|
||||
multipleCommands.forEach((cmd) => {
|
||||
if(cmd[0] != "G"){
|
||||
this.receiveGCode(robot, "G"+cmd);
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////////////////////77
|
||||
// Commands for Files
|
||||
|
||||
static removeStringFromFile(fileName, stringToRemove) {
|
||||
try {
|
||||
const data = fs.readFileSync(fileName, 'utf8');
|
||||
const modifiedData = data.replace(new RegExp(stringToRemove, 'g'), '');
|
||||
fs.writeFileSync(fileName, modifiedData, 'utf8');
|
||||
} catch (err) {
|
||||
console.error('Error:', err);
|
||||
}
|
||||
}
|
||||
|
||||
static ContainsFilesCommand(message){
|
||||
if(message.indexOf('FPoint') !== -1){return true;} // Writes new Point (at end of file)
|
||||
if(message.indexOf('FPlus') !== -1){return true;} // go to Next Position in Log File
|
||||
if(message.indexOf('FMinus') !== -1){return true;} // go to Previous Position
|
||||
if(message.indexOf('FFirst') !== -1){return true;} // set Cursour to First Position of Log File
|
||||
if(message.indexOf('FLast') !== -1){return true;} // set Cursour to Last Position of Log File
|
||||
|
||||
|
||||
if(message.indexOf('FShow') !== -1){return true;} // Shows/Sends GCode-File
|
||||
if(message.indexOf('FList') !== -1){return true;} // Lists GCode-Files
|
||||
if(message.indexOf('FLoad ') !== -1){return true;} // Loads File into Log
|
||||
if(message.indexOf('FSave ') !== -1){return true;} // Saves Log to GCode-File
|
||||
if(message.indexOf('FClear') !== -1){return true;} // Clears default Log File
|
||||
|
||||
if(message.indexOf('M20') !== -1){return true;} // M20 - List SD Card-Contents // https://marlinfw.org/docs/gcode/M020.html
|
||||
if(message.indexOf('M23') !== -1){return true;} // M23 - Select SD file
|
||||
if(message.indexOf('M28') !== -1){return true;} // M28 - Start SD write // M28 [B1] filename
|
||||
if(message.indexOf('M29') !== -1){return true;} // M29 - Stop SD write
|
||||
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static receiveFC(robot, message){
|
||||
|
||||
if(message.indexOf('FShow') !== -1){
|
||||
try {
|
||||
const data = fs.readFileSync(this.fileName, 'utf8');
|
||||
const reply = "XYZ__FShow__XYZ" + data; // prepend header
|
||||
return reply.replaceAll("G91 ","").replaceAll("G90 ","").replace(/\s?\bt\d+\b\s?/g, '').trim();
|
||||
|
||||
} catch (err) {
|
||||
console.error('Error:', err);
|
||||
}
|
||||
}
|
||||
|
||||
if(message.indexOf('FPoint') !== -1){
|
||||
|
||||
const secondsSinceEpoch = 10*Math.floor(Date.now() / 100);
|
||||
|
||||
var strGCode = String(`G90 G1 x${robot.x} y${robot.y} z${robot.z} a${(robot.phi*180/Math.PI).toFixed(2)} b${(robot.theta*180/Math.PI).toFixed(2)} c${(robot.psi*180/Math.PI).toFixed(2)} e${(robot.e*180/Math.PI).toFixed(2)} t${secondsSinceEpoch} f1000` )
|
||||
this.removeStringFromFile(this.fileName, ';!')
|
||||
fs.appendFileSync(this.fileName, strGCode+ ';!'+ '\r\n', 'utf8');
|
||||
}
|
||||
|
||||
if(message.indexOf('FMinus') !== -1 || message.indexOf('FPlus') !== -1){
|
||||
let lines = fs.readFileSync(this.fileName, 'utf8').split('\r\n');
|
||||
|
||||
let lineChange = -1;
|
||||
|
||||
// Process lines
|
||||
for (let i = 0; i < lines.length; i++) {
|
||||
if (lines[i].indexOf(';!') !== -1) {
|
||||
if(message.indexOf('FMinus') !== -1 && i > 0){
|
||||
lines[i - 1] += ';!';
|
||||
lines[i] = lines[i].split(';!')[0];
|
||||
var gCodePi = this.toPiMultiple(lines[i-1]);
|
||||
this.receiveGCode(robot, gCodePi);
|
||||
}
|
||||
if(message.indexOf('FPlus') !== -1 && i < (lines.length-2) && lineChange == -1){
|
||||
lines[i + 1] += ';!';
|
||||
lines[i] = lines[i].split(';!')[0];
|
||||
lineChange = i+1;
|
||||
var gCodePi = this.toPiMultiple(lines[i+1]);
|
||||
this.receiveGCode(robot, gCodePi);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Write the updated content back
|
||||
fs.writeFileSync(this.fileName, lines.join('\r\n'), 'utf8');
|
||||
|
||||
}
|
||||
|
||||
return GCode.getM114(robot);
|
||||
}
|
||||
}
|
||||
|
||||
module.exports = GCode
|
||||
|
||||
53
robot/PythonSender.js
Executable file
53
robot/PythonSender.js
Executable file
@@ -0,0 +1,53 @@
|
||||
|
||||
module.exports = class PythonSender {
|
||||
|
||||
|
||||
constructor(p = 9875){
|
||||
const WebSocket = require('ws');
|
||||
|
||||
|
||||
this.sockets = [];
|
||||
this.server = new WebSocket.Server({port: p});
|
||||
|
||||
this.server.on('connection', (socket) => this.connectThis(socket));
|
||||
}
|
||||
|
||||
msgThis(msg){
|
||||
console.log("message:" + msg.toString())
|
||||
this.sockets.forEach(s => s.send(msg));
|
||||
}
|
||||
|
||||
closeThis(socket){
|
||||
console.log("Python Receiver is closing the connection");
|
||||
this.sockets = this.sockets.filter(s => s !== socket);
|
||||
}
|
||||
connectThis(socket) {
|
||||
console.log("Python Receiver is connected")
|
||||
this.sockets.push(socket);
|
||||
|
||||
// When you receive a message, send that message to every socket.
|
||||
socket.on('message', (msg) => this.msgThis(msg));
|
||||
|
||||
|
||||
// When a socket closes, or disconnects, remove it from the array.
|
||||
socket.on('close', () => this.closeThis(socket));
|
||||
}
|
||||
|
||||
send(m){
|
||||
if((this.sockets.length) > 0){
|
||||
this.sockets.forEach( s => s.send(m));
|
||||
}
|
||||
else{ console.warn("No sockets connected. Can't send the Message.");}
|
||||
}
|
||||
/*
|
||||
if(this.pythonSender != null){
|
||||
var strJSON = CANCreator.cmdMovement(this.motorPositionOld, this.motorPosition);
|
||||
if(strJSON != '{"message":"empty"}'){
|
||||
this.pythonSender.send(strJSON);
|
||||
}
|
||||
}
|
||||
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
19
robot/RobotMotorPosition.js
Executable file
19
robot/RobotMotorPosition.js
Executable file
@@ -0,0 +1,19 @@
|
||||
module.exports = class RobotMotorPosition{
|
||||
|
||||
|
||||
constructor(x, y, z, a, b, c, e = 0.0){
|
||||
this.x = x; // X-Motor
|
||||
this.y = y; // Y-Motor
|
||||
this.z = z;
|
||||
|
||||
this.a = a; // A-Motor: Ellbow-LowerArm turner
|
||||
this.b = b; // B-Motor: Hand up-down
|
||||
this.c = c; // C-Motor: Hand twist
|
||||
|
||||
this.e = e; // Finger open
|
||||
|
||||
this.time = Date.now();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
213
robot/TelnetSenderGRBL.js
Executable file
213
robot/TelnetSenderGRBL.js
Executable file
@@ -0,0 +1,213 @@
|
||||
const net = require("net");
|
||||
const { resolve } = require("path");
|
||||
const { TelnetSocket } = require("telnet-stream");
|
||||
|
||||
|
||||
|
||||
module.exports = class TelnetSenderGRBL{
|
||||
|
||||
/* urlGRBL: URL für den GCode Empfänger (MicroController, GRBL)
|
||||
* xAxisGrbl: Welche Achse soll ausgelesen werden? Welche Achse ist die GRBL-X-Achse
|
||||
* yAxisGrbl: ...
|
||||
* zAxisGrbl: ...
|
||||
*/
|
||||
constructor(urlGRBL = "grblesp.local", maxSpeedF = 5000, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z", aAxisGrbl = null, bAxisGrbl = null, cAxisGrbl = null, eAxisGrbl = null){
|
||||
|
||||
var socket = null;
|
||||
this.tSocket = null;
|
||||
this.receiver = null;
|
||||
|
||||
this.urlGRBLstr = urlGRBL;
|
||||
this.xAxisGrbl = xAxisGrbl;
|
||||
this.yAxisGrbl = yAxisGrbl;
|
||||
this.zAxisGrbl = zAxisGrbl;
|
||||
this.aAxisGrbl = aAxisGrbl;
|
||||
this.bAxisGrbl = bAxisGrbl;
|
||||
this.cAxisGrbl = cAxisGrbl;
|
||||
this.eAxisGrbl = eAxisGrbl;
|
||||
|
||||
|
||||
|
||||
new Promise((resolve, reject) => {
|
||||
socket = net.createConnection({port: 23, host: urlGRBL},() => {
|
||||
resolve(socket);
|
||||
})
|
||||
.on('error', reject);
|
||||
}).then( connection => {
|
||||
connection.on('data', data => {
|
||||
//console.log("data from " + urlGRBL + " is: " + data);
|
||||
//if(this.receiver && this.receiver != null){
|
||||
// this.receiver.send(buffer);
|
||||
// }
|
||||
});
|
||||
}, error => {
|
||||
console.log("Telnet Connection Error on " + urlGRBL + ": " + error.toString());
|
||||
this.tSocket = null;
|
||||
});
|
||||
|
||||
|
||||
if(socket != null){
|
||||
this.tSocket = new TelnetSocket(socket);
|
||||
|
||||
this.tSocket.on("close", function () {
|
||||
console.log("Telnet Closed " + urlGRBL);
|
||||
this.tSocket = null;
|
||||
});
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
moveTo(mOld, mNew){
|
||||
|
||||
// The Hand-Turn is not 1:1 to the Hand-Lift °
|
||||
var factorTurnLift = 1.2;
|
||||
|
||||
// The Hand-Open is not 1:1 to the Hand-Turn
|
||||
var factorOpenTurn = 1.92;
|
||||
|
||||
// Hand-Open in mm
|
||||
var handOpenInMM = 1.0
|
||||
|
||||
var data = "G1"
|
||||
|
||||
if(this.xAxisGrbl == "x"){
|
||||
data += " x" + (mNew.x).toFixed(2).toString();
|
||||
}
|
||||
if(this.xAxisGrbl == "y"){
|
||||
data += " x" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.xAxisGrbl == "z"){
|
||||
data += " x" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
|
||||
}
|
||||
if(this.xAxisGrbl == "a"){
|
||||
// This is the case for the Ellbow, when the Motor is connected to the X-Port of the FluidNC
|
||||
data += " x" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.xAxisGrbl == "b"){
|
||||
data += " x" + (mNew.b * 180 / Math.PI+(mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
|
||||
}
|
||||
if(this.xAxisGrbl == "c"){
|
||||
// Runs correctly, substracts the "b" axis, uses the "c" axis to send to the x-Motor wich is in this case the hand-twist
|
||||
data += " x" + ((-1)*mNew.b * 180 / Math.PI + (mNew.c * 180 / Math.PI) ).toFixed(2).toString();
|
||||
}
|
||||
if(this.xAxisGrbl == "e"){
|
||||
//This is the case for the Hand, when the Open-Close Motor is connected to the X-Port of FluidNC
|
||||
var handUpDown = mNew.b * 180 * factorTurnLift / Math.PI ;
|
||||
var handTurn = mNew.c*180/Math.PI ;
|
||||
data += " x" + ((-1.0*(-1.0*(handUpDown) + handTurn) + mNew.e*handOpenInMM)).toFixed(2).toString();
|
||||
}
|
||||
|
||||
|
||||
|
||||
if(this.yAxisGrbl == "x"){
|
||||
data += " y" + (mNew.x ).toFixed(2).toString();
|
||||
}
|
||||
if(this.yAxisGrbl == "y"){
|
||||
data += " y" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.yAxisGrbl == "z"){
|
||||
data += " y" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
|
||||
}
|
||||
if(this.yAxisGrbl == "a"){
|
||||
data += " y" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.yAxisGrbl == "b"){
|
||||
data += " y" + (mNew.b * 180 / Math.PI+(mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
|
||||
}
|
||||
if(this.yAxisGrbl == "c"){
|
||||
// This is the case if the hand-rotation-turner is connected to the FluidNC-Y
|
||||
var handUpDown = (mNew.b * 180 / Math.PI ) * factorTurnLift;
|
||||
var handTurn = ( mNew.c*180/Math.PI ) ;
|
||||
data += " y" + (-1.0*(handUpDown) + handTurn).toFixed(2).toString();
|
||||
}
|
||||
if(this.yAxisGrbl == "e"){
|
||||
data += " y" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
|
||||
|
||||
if(this.zAxisGrbl == "x"){
|
||||
data += " z" + (mNew.x).toFixed(2).toString();
|
||||
}
|
||||
if(this.zAxisGrbl == "y"){
|
||||
data += " z" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.zAxisGrbl == "z"){
|
||||
data += " z" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
|
||||
}
|
||||
if(this.zAxisGrbl == "a"){
|
||||
data += " z" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.zAxisGrbl == "b"){
|
||||
// This is the case of the Hand, when the Up-Down-Motor is connected to the FluidNC-Z
|
||||
data += " z" + ( mNew.b * 180 / Math.PI ).toFixed(2).toString();
|
||||
}
|
||||
if(this.zAxisGrbl == "c"){
|
||||
data += " z" + (mNew.c * 180 / Math.PI + (mNew.b * 180 / Math.PI) + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
|
||||
}
|
||||
if(this.zAxisGrbl == "e"){
|
||||
|
||||
data += " z" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
|
||||
|
||||
|
||||
if(this.aAxisGrbl == "x"){
|
||||
data += " a" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.aAxisGrbl == "y"){
|
||||
data += " a" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.aAxisGrbl == "z"){
|
||||
data += " a" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
|
||||
}
|
||||
if(this.aAxisGrbl == "a"){
|
||||
data += " a" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.aAxisGrbl == "b"){
|
||||
data += " a" + (mNew.b * 180 / Math.PI+(mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
|
||||
}
|
||||
if(this.aAxisGrbl == "c"){
|
||||
data += " a" + (mNew.c * 180 / Math.PI + (mNew.b * 180 / Math.PI) + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
|
||||
}
|
||||
if(this.aAxisGrbl == "e"){
|
||||
// ToDo Mai 2024
|
||||
data += " a" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
if(this.bAxisGrbl == "x"){
|
||||
data += " b" + (mNew.x).toFixed(2).toString();
|
||||
}
|
||||
if(this.bAxisGrbl == "y"){
|
||||
data += " b" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.bAxisGrbl == "z"){
|
||||
data += " b" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI) ).toFixed(2).toString();
|
||||
}
|
||||
if(this.bAxisGrbl == "a"){
|
||||
data += " b" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.bAxisGrbl == "b"){
|
||||
data += " b" + (mNew.b * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
if(this.bAxisGrbl == "c"){
|
||||
data += " b" + (mNew.c * 180 / Math.PI + (mNew.b * 180 / Math.PI) + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
|
||||
}
|
||||
if(this.bAxisGrbl == "e"){
|
||||
data += " b" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
|
||||
}
|
||||
|
||||
data += " f"+(maxSpeedF.toFixed(2).toString())
|
||||
|
||||
|
||||
console.log("" + this.urlGRBLstr + " receives: " + data.toString("utf-8"))
|
||||
|
||||
if(this.tSocket && data.toString("utf-8").length > 3){
|
||||
this.tSocket.write( data.toString("utf-8") + "\r\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user