appRobotDriver/robot/WSSenderGrbl.js

const WebSocket = require('ws');
const SenderInterface = require('./SenderInterface');

module.exports = class WSSenderGrbl extends SenderInterface {

    constructor(urlGRBL = "grblesp.local", maxSpeedF = 5000, xAxisGrbl = "x", yAxisGrbl = "y", zAxisGrbl = "z", aAxisGrbl = null, bAxisGrbl = null, cAxisGrbl = null, eAxisGrbl = null, options = {}) {
        super();

        this.ws = null;
        this.state = 'disconnected';
        this.error = null;
        this.isTestMode = false;
        this.shouldReconnect = true;
        this.reconnectTimer = null;
        this.connectPromise = null;
        this.connectResolver = null;
        this.connectRejecter = null;
        this.reconnectAttempt = 0;

        this.urlGRBLstr = urlGRBL;
        this.maxSpeedF = maxSpeedF;
        this.xAxisGrbl = xAxisGrbl;
        this.yAxisGrbl = yAxisGrbl;
        this.zAxisGrbl = zAxisGrbl;
        this.aAxisGrbl = aAxisGrbl;
        this.bAxisGrbl = bAxisGrbl;
        this.cAxisGrbl = cAxisGrbl;
        this.eAxisGrbl = eAxisGrbl;

        this.WebSocketClass = options.WebSocketClass || WebSocket;
        this.setTimeoutFn = options.setTimeoutFn || setTimeout;
        this.clearTimeoutFn = options.clearTimeoutFn || clearTimeout;
        this.reconnectDelay = Number.isFinite(options.reconnectDelay) ? options.reconnectDelay : 2000;
        this.maxReconnectDelay = Number.isFinite(options.maxReconnectDelay) ? options.maxReconnectDelay : 30000;
        this.wsPort = options.wsPort || 81;
        this.autoConnect = options.autoConnect !== false;

        if (urlGRBL === "test.test") {
            this.isTestMode = true;
            this.state = 'connected';
            this.shouldReconnect = false;
            this.ws = { readyState: 1, written: "", send(txt) { this.written = txt; }, close() {} };
            return;
        }

        if (this.autoConnect) {
            this.connect();
            console.log("🤖  WSSenderGrbl initialized: " + urlGRBL);
        }
    }

    async connect() {
        if (this.isTestMode) {
            this.state = 'connected';
            return Promise.resolve(this);
        }

        if (this.state === 'connected' && this.ws && this.ws.readyState === 1) {
            return Promise.resolve(this);
        }

        if (this.connectPromise) {
            return this.connectPromise;
        }

        if (this.reconnectTimer) {
            this.clearTimeoutFn(this.reconnectTimer);
            this.reconnectTimer = null;
        }

        this.state = 'connecting';
        this.error = null;
        this.shouldReconnect = true;

        this.connectPromise = new Promise((resolve, reject) => {
            this.connectResolver = resolve;
            this.connectRejecter = reject;
            this._tryConnect();
        });

        return this.connectPromise;
    }

    _tryConnect() {
        if (!this.shouldReconnect) return;

        this.state = 'connecting';
        this.error = null;

        const url = `ws://${this.urlGRBLstr}:${this.wsPort}`;
        const ws = new this.WebSocketClass(url);

        ws.on('open', () => {
            if (!this.shouldReconnect) {
                ws.close();
                return;
            }
            this.ws = ws;
            this.state = 'connected';
            this.error = null;
            this.reconnectAttempt = 0;
            if (this.connectResolver) {
                this.connectResolver(this);
                this.connectResolver = null;
                this.connectRejecter = null;
            }
            this.connectPromise = null;
        });

        ws.on('close', () => {
            console.log("WS Closed " + this.urlGRBLstr);
            this.ws = null;
            if (this.shouldReconnect) {
                this.state = 'reconnecting';
                this._scheduleReconnect();
            } else {
                this.state = 'disconnected';
            }
        });

        ws.on('error', (err) => {
            console.log("WS Connection Error on " + this.urlGRBLstr + ": " + err.message);
            this.error = err.message || String(err);
            if (!this.shouldReconnect) {
                this.state = 'disconnected';
                if (this.connectRejecter) {
                    this.connectRejecter(err);
                    this.connectResolver = null;
                    this.connectRejecter = null;
                    this.connectPromise = null;
                }
            }
        });
    }

    _scheduleReconnect() {
        if (!this.shouldReconnect || this.reconnectTimer) return;

        const delay = Math.min(this.reconnectDelay * 2 ** this.reconnectAttempt, this.maxReconnectDelay);
        this.reconnectAttempt += 1;
        console.log(`WS reconnect attempt ${this.reconnectAttempt} in ${delay}ms for ${this.urlGRBLstr}`);

        this.reconnectTimer = this.setTimeoutFn(() => {
            this.reconnectTimer = null;
            this._tryConnect();
        }, delay);
    }

    send(command) {
        if (!this.ws || this.ws.readyState !== 1) return false;
        const payload = typeof command === 'string' ? command : String(command);
        if (!payload) return false;
        this.ws.send(payload + "\n");
        return true;
    }

    getStatus() {
        return {
            state: this.state,
            url: this.urlGRBLstr,
            error: this.error,
            isTestMode: !!this.isTestMode,
            reconnectAttempt: this.reconnectAttempt,
            reconnectTimer: !!this.reconnectTimer
        };
    }

    disconnect() {
        this.shouldReconnect = false;

        if (this.reconnectTimer) {
            this.clearTimeoutFn(this.reconnectTimer);
            this.reconnectTimer = null;
        }

        if (this.connectPromise && this.connectRejecter) {
            this.connectRejecter(new Error('disconnect'));
            this.connectPromise = null;
            this.connectResolver = null;
            this.connectRejecter = null;
        }

        if (this.ws && this.ws.readyState !== 3) {
            this.ws.close();
        }

        this.ws = null;
        this.state = 'disconnected';
        this.error = null;
    }

    moveTo(mOld, mNew) {
        this.execCommand("G1", mOld, mNew);
    }

    execCommand(strCommand = "G1", mOld, mNew) {

        var factorTurnLift = 1.2;
        var factorOpenTurn = 1.92;
        var handOpenInMM = 1.0;

        var data = strCommand.toString("utf-8");

        if (this.xAxisGrbl == "x") {
            if (Number.isFinite(mNew.x)) {
                data += " x" + (mNew.x).toFixed(2).toString();
            }
        }
        if (this.xAxisGrbl == "y") {
            if (Number.isFinite(mNew.y)) {
                data += " x" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.xAxisGrbl == "z") {
            if (Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                data += " x" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.xAxisGrbl == "a") {
            if (Number.isFinite(mNew.a)) {
                data += " x" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.xAxisGrbl == "b") {
            if (Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                data += " x" + (mNew.b * 180 / Math.PI + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.xAxisGrbl == "c") {
            if (Number.isFinite(mNew.b) && Number.isFinite(mNew.c)) {
                data += " x" + ((-1) * mNew.b * 180 / Math.PI + (mNew.c * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.xAxisGrbl == "e") {
            if (Number.isFinite(mNew.b) && Number.isFinite(mNew.c) && Number.isFinite(mNew.e)) {
                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") {
            if (Number.isFinite(mNew.x)) {
                data += " y" + (mNew.x).toFixed(2).toString();
            }
        }
        if (this.yAxisGrbl == "y") {
            if (Number.isFinite(mNew.y)) {
                data += " y" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.yAxisGrbl == "z") {
            if (Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                data += " y" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.yAxisGrbl == "a") {
            if (Number.isFinite(mNew.a)) {
                data += " y" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.yAxisGrbl == "b") {
            if (Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                data += " y" + (mNew.b * 180 / Math.PI + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.yAxisGrbl == "c") {
            if (Number.isFinite(mNew.b) && Number.isFinite(mNew.c)) {
                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") {
            if (Number.isFinite(mNew.e)) {
                data += " y" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
            }
        }

        if (this.zAxisGrbl == "x") {
            if (Number.isFinite(mNew.x)) {
                data += " z" + (mNew.x).toFixed(2).toString();
            }
        }
        if (this.zAxisGrbl == "y") {
            if (Number.isFinite(mNew.y)) {
                data += " z" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.zAxisGrbl == "z") {
            if (Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                data += " z" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.zAxisGrbl == "a") {
            if (Number.isFinite(mNew.a)) {
                data += " z" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.zAxisGrbl == "b") {
            if (Number.isFinite(mNew.b)) {
                data += " z" + (mNew.b * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.zAxisGrbl == "c") {
            if (Number.isFinite(mNew.c) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                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") {
            if (Number.isFinite(mNew.e)) {
                data += " z" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
            }
        }

        if (this.aAxisGrbl == "x") {
            if (Number.isFinite(mNew.y)) {
                data += " a" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.aAxisGrbl == "y") {
            if (Number.isFinite(mNew.y)) {
                data += " a" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.aAxisGrbl == "z") {
            if (Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                data += " a" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.aAxisGrbl == "a") {
            if (Number.isFinite(mNew.a)) {
                data += " a" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.aAxisGrbl == "b") {
            if (Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                data += " a" + (mNew.b * 180 / Math.PI + (mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.aAxisGrbl == "c") {
            if (Number.isFinite(mNew.c) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                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") {
            if (Number.isFinite(mNew.e)) {
                data += " a" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
            }
        }

        if (this.bAxisGrbl == "x") {
            if (Number.isFinite(mNew.x)) {
                data += " b" + (mNew.x).toFixed(2).toString();
            }
        }
        if (this.bAxisGrbl == "y") {
            if (Number.isFinite(mNew.y)) {
                data += " b" + (mNew.y * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.bAxisGrbl == "z") {
            if (Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                data += " b" + ((mNew.z * 180 / Math.PI) - (mNew.y * 180 / Math.PI)).toFixed(2).toString();
            }
        }
        if (this.bAxisGrbl == "a") {
            if (Number.isFinite(mNew.a)) {
                data += " b" + (mNew.a * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.bAxisGrbl == "b") {
            if (Number.isFinite(mNew.b)) {
                data += " b" + (mNew.b * 180 / Math.PI).toFixed(2).toString();
            }
        }
        if (this.bAxisGrbl == "c") {
            if (Number.isFinite(mNew.c) && Number.isFinite(mNew.b) && Number.isFinite(mNew.z) && Number.isFinite(mNew.y)) {
                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") {
            if (Number.isFinite(mNew.e)) {
                data += " b" + (mNew.e * 180 / Math.PI).toFixed(2).toString();
            }
        }

        data += " f" + (this.maxSpeedF.toFixed(2).toString());

        if (data.length > 3) {
            if (data.indexOf("G90") == -1) {
                data = "G90 " + data;
            }
            console.log("Driver send to 🤖  " + this.urlGRBLstr + " the message: " + data);
            this.send(data);
        }
    }
};