163 lines
6.5 KiB
JavaScript
Executable File
163 lines
6.5 KiB
JavaScript
Executable File
// __tests__/Robot.inverseKinematics.test.js
|
||
const Robot = require('../robot/kinematics/Arm3SegmentLinearX');
|
||
const GCode = require('../robot/GCode.js');
|
||
var TenetSender = require('../robot/TelnetSenderGRBL.js')
|
||
|
||
describe("Robot G92", () => {
|
||
|
||
beforeAll(() => {
|
||
jest.spyOn(console, 'log').mockImplementation(() => {})
|
||
})
|
||
|
||
afterAll(() => {
|
||
jest.restoreAllMocks()
|
||
})
|
||
|
||
test("ReadPosition -> calculatePositionFromMotorAngles()", () => {
|
||
|
||
// === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
|
||
const L1 = 300;
|
||
const L2 = 200;
|
||
const L3 = 10;
|
||
|
||
|
||
const A = new Robot(L1, L2, L3)
|
||
|
||
// Beispiel-Eingabe
|
||
A.x = 0;
|
||
A.y = 310;
|
||
A.z = 0;
|
||
|
||
A.phi = -Math.PI/2;
|
||
A.theta = Math.PI/2;
|
||
A.psi = 0;
|
||
A.e = 0;
|
||
|
||
A.calculateAngles3D();
|
||
|
||
var strGCode = `M92 X${A.xMotor} Y${A.alpha} Z${A.beta} A${A.a} B${A.b} C${A.c}`
|
||
|
||
|
||
const T = new Robot(L1, L2, L3)
|
||
console.log("GCode: " + strGCode);
|
||
GCode.receiveGCode(T, strGCode);
|
||
|
||
|
||
const EPS = 0.01; // 1/1000 mm Genauigkeit
|
||
expect(T.xMotor).toBeCloseTo(A.xMotor, EPS);
|
||
expect(T.alpha).toBeCloseTo(A.alpha, EPS);
|
||
expect(T.beta).toBeCloseTo(A.beta, EPS);
|
||
expect(T.x).toBeCloseTo(A.x, EPS);
|
||
expect(T.y).toBeCloseTo(A.y, EPS);
|
||
expect(T.z).toBeCloseTo(A.z, EPS);
|
||
|
||
|
||
});
|
||
|
||
// G92 y2.8 z131.0
|
||
test("ReadPosition -> G92() test ob nur bestimmte Achsen gesendet werden", () => {
|
||
|
||
// === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
|
||
const L1 = 300;
|
||
const L2 = 300;
|
||
const L3 = 20;
|
||
|
||
const robot = new Robot(L1, L2, L3)
|
||
|
||
var telnetSender1 = new TenetSender(urlGRBL = "test.test", maxSpeedF = 2300, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z");
|
||
var telnetSender2 = new TenetSender(urlGRBL = "test.test", maxSpeedF = 5000, xAxisGrbl = "a", yAxisGrbl = null, zAxisGrbl = null);
|
||
var telnetSender3 = new TenetSender(urlGRBL = "test.test", maxSpeedF = 5000, xAxisGrbl = "c", yAxisGrbl = "e", zAxisGrbl = "b");
|
||
|
||
robot.cmdReceivers.push(telnetSender1);
|
||
robot.cmdReceivers.push(telnetSender2);
|
||
robot.cmdReceivers.push(telnetSender3);
|
||
|
||
|
||
GCode.receiveGCode(robot,"M92 y0.049 z2.286");
|
||
|
||
//const strExpect1 = 'G92 x0.00 y2.81 z128.17\r\n';
|
||
const strExpect1 = 'G92 y2.81 z128.17\r\n'; // x soll eben nicht gesendet werden.
|
||
const strExpect2 = 'G92 y2.81 z128.17\r\n';
|
||
|
||
allowedValues = [strExpect1, strExpect2];
|
||
expect(allowedValues).toContain(telnetSender1.tSocket.written);
|
||
expect(telnetSender2.tSocket.written.length).toBe(0); // kein GCode für Ellbow
|
||
|
||
// ("Wenn nur G92 x3 gegeben wird, dann wird trotzdem auch y und z gesendet. schlecht." );
|
||
});
|
||
// G92 y2.8 z131.0
|
||
test("ReadPosition -> G92() test ob nur bestimmte Achsen gesendet werden", () => {
|
||
|
||
// === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
|
||
const L1 = 300;
|
||
const L2 = 300;
|
||
const L3 = 20;
|
||
|
||
const robot = new Robot(L1, L2, L3)
|
||
|
||
var telnetSender1 = new TenetSender(urlGRBL = "test.test", maxSpeedF = 2300, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z");
|
||
var telnetSender2 = new TenetSender(urlGRBL = "test.test", maxSpeedF = 5000, xAxisGrbl = "a", yAxisGrbl = null, zAxisGrbl = null);
|
||
var telnetSender3 = new TenetSender(urlGRBL = "test.test", maxSpeedF = 5000, xAxisGrbl = "c", yAxisGrbl = "e", zAxisGrbl = "b");
|
||
|
||
robot.cmdReceivers.push(telnetSender1);
|
||
robot.cmdReceivers.push(telnetSender2);
|
||
robot.cmdReceivers.push(telnetSender3);
|
||
|
||
|
||
GCode.receiveGCode(robot,"M92 a0.20");
|
||
|
||
|
||
expect(telnetSender1.tSocket.written.length).toBe(0); // kein GCode für XYZ
|
||
expect(telnetSender2.tSocket.written).toBe('G92 x11.46\r\n');
|
||
expect(telnetSender3.tSocket.written.length).toBe(0); // kein GCode für XYZ
|
||
|
||
// ("Wenn nur G92 x3 gegeben wird, dann wird trotzdem auch y und z gesendet. schlecht." );
|
||
});
|
||
|
||
test("G92 E: Greifer-Öffnung (mm) → eMotor über Kopplung e - b - c", () => {
|
||
const robot = new Robot(300, 300, 20);
|
||
const D = 180 / Math.PI;
|
||
|
||
// B/C in Grad rein (→ intern Radiant), E in mm. eMotor muss aus e, b, c abgeleitet
|
||
// werden — die Greifer-Sehne läuft durchs Handgelenk (Arm3SegmentLinearX-Kopplung).
|
||
GCode.receiveGCode(robot, "G92 B30 C-45 E10");
|
||
|
||
expect(robot.b).toBeCloseTo(30 / D, 6); // 30° → rad
|
||
expect(robot.c).toBeCloseTo(-45 / D, 6); // -45° → rad
|
||
expect(robot.e).toBe(10); // mm, unverändert
|
||
expect(robot.eMotor).toBeCloseTo(10 - robot.b - robot.c, 6); // = e - b - c
|
||
|
||
// Konsistenz: identische Kopplung wie der reguläre Bewegungspfad (calculateAngles3D).
|
||
expect(robot.eMotor).toBeCloseTo(robot.gripperMotorFromOpening(robot.e), 12);
|
||
});
|
||
|
||
test("G92 E: Greifer-Port (y) geht real an den Hand-Controller raus (nicht nur ins Modell)", () => {
|
||
// Regression: G92 mit E muss nicht nur robot.eMotor füllen, sondern den Greifer-Port
|
||
// auch tatsächlich an FluidNC senden (Kalibrierung nach dem Foto-Homing). Hand-
|
||
// Verkabelung x=c, y=e, z=b → Greifer liegt auf dem y-Port, gesendet wird eMotor·(180/π).
|
||
// Schützt davor, dass das E-Senden bei G92 still deaktiviert wird (war zeitweise vermutet).
|
||
const robot = new Robot(300, 300, 20);
|
||
const D = 180 / Math.PI;
|
||
|
||
const base = new TenetSender("test.test", 2300, "x", "y", "z");
|
||
const elbow = new TenetSender("test.test", 5000, "a", null, null);
|
||
const hand = new TenetSender("test.test", 5000, "c", "e", "b");
|
||
robot.cmdReceivers.push(base, elbow, hand);
|
||
|
||
GCode.receiveGCode(robot, "G92 B30 C-45 E10");
|
||
|
||
const handSent = hand.tSocket.written;
|
||
|
||
// 1) Greifer-Port muss überhaupt rausgehen (sonst wäre das E-Senden deaktiviert).
|
||
const yMatch = handSent.match(/ y(-?\d+\.\d+)/);
|
||
expect(yMatch).not.toBeNull();
|
||
|
||
// 2) Wert = eMotor·D (= (e − b − c)·D), konsistent mit dem G1/Gamepad-Pfad.
|
||
expect(parseFloat(yMatch[1])).toBeCloseTo(robot.eMotor * D, 1);
|
||
|
||
// 3) Es ist ein Setz-Befehl (G92), keine Bewegung (G1).
|
||
expect(handSent.startsWith("G92")).toBe(true);
|
||
});
|
||
});
|
||
|