ChK/xOld_appRoboticsDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add Robot_JoyIt driver

Browse Source
This commit is contained in:
Kendel Christoph
2026-01-17 16:50:07 +01:00
parent e9e50acf5f
commit 0cfb4d5a95
15848 changed files with 570836 additions and 268976 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
robot/ComSender.js
View File

@@ -1,27 +1,39 @@
const fs = require('fs');
module.exports = class ComSender{
// comSender.js
const { SerialPort } = require('serialport');
constructor(){
module.exports = class ComSender {
constructor(comPort = '/dev/ttyS0', baudRate = 115200) {
this.comPort = comPort;
this.baudRate = baudRate;
this.port = null;
}
async init() {
return new Promise((resolve, reject) => {
this.port = new SerialPort(
{
path: this.comPort,
baudRate: this.baudRate,
dataBits: 8,
stopBits: 1,
parity: 'none',
autoOpen: true,
},
(err) => (err ? reject(err) : resolve())
);
});
}
moveTo(mOld, mNew) {
if (!this.port || !this.port.isOpen) {
console.error('Serial port not open. Call init() first.');
return;
}
moveTo(mOld, mNew){
try {
const fd = fs.openSync('/dev/tty0', 'w'); // may require root
fs.writeSync(fd, 'Hello console from Node.js!\n');
fs.closeSync(fd);
console.log('Wrote to /dev/tty0');
} catch (err) {
console.error('Failed to write to /dev/tty0:', err);
}
}
}
const msg = 'Hello from Node at ' + this.baudRate + ' baud\r\n';
this.port.write(msg, (err) => {
if (err) return console.error('Write error:', err);
this.port.drain(() => console.log('Wrote to ' + this.comPort));
});
}
}

104
robot/Robot_JoyIt.js
View File

@@ -1,13 +1,11 @@
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.speedX = 100; // mm/min
this.speedY = 100;
this.speedZ = 100;
this.lastCommandSend = 0;
@@ -24,28 +22,29 @@ class Robot{
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;
// Zwischen-Ergebnisse: Handgelenk Punkt (nur für Tests sind die Public)
this.Hz = 0.0;
this.Hr = 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.
this.OH = 0.0;
this.epsilon = 0.0;
// Motor-Winkel fuer's Handgelenk
this.a = 0; // aMotor am Ellebogen
this.b = 0; // bMotor Handgelenk-Knicker
this.c = 0; // cMotor Hand-Dreher
this.r = 0.0; // Abstand in der XY-Ebene
this.alpha = 0.0;
this.beta = 0.0;
this.eMotor = 0; // eMotor Finger-Distanz
// 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();
@@ -74,60 +73,21 @@ class Robot{
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);
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));
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;
this.eMotor = this.e;
}
calculatePositionFromMotorAngles(){