throttle_motor_mapping/can.js

const can = require("socketcan");
const csv = require("fast-csv");
const fs = require("fs");
const net = require('net');

const can_conf = require("./app/config/can.conf");
const map_conf = require("./app/config/map.conf");
const path = require("path");

const SOCKET_PATH = path.join(__dirname, map_conf.map_storage_path, can_conf.socket_filename);

var map_rpm = [];
var map_throttle = [];

var loaded_map_name = null;
var loaded_map_data = [];

var throttle_output = 0;
var ERPM = 0;

var enabled = false;

var channel = can.createRawChannel(can_conf.network_id, true);

// Some test messages...
// 404#0000245E00710186
// 019#02
// 050#9702

const current_id = (0x01 << 5) | can_conf.controller.id;
const brake_current_id = (0x02 << 5) | can_conf.controller.id;
const erpm_id = (0x20 << 5) | can_conf.controller.id;

function parseMap(filename) {
    let name = "";
    let map = [];

    return new Promise((resolve, reject) => {
        csv.parseFile((filename), { comment: "#" })
            .on('error', error => reject(error))
            .on('data', row => map.push(row))
            .on('end', () => { name = filename; resolve([name, map]) });
    });
}

function getMapIndex(array, input) {
    if (array.length === 0) {
        return -1;
    }

    let left = 0;
    let right = array.length - 1;

    while (left <= right) {
        const mid = Math.floor((left + right) / 2);

        if (array[mid] === input) {
            return mid;
        } else if (array[mid] < input) {
            left = mid + 1;
        } else {
            right = mid - 1;
        }
    }

    if (left >= array.length) {
        return array.length - 1;
    }
    if (right < 0) {
        return 0;
    }
    return Math.abs(array[left] - input) < Math.abs(array[right] - input) ? left : right;
}

function connect() {
    console.log("Connecting...");
    if (fs.existsSync(SOCKET_PATH)) {
        const client = net.createConnection(SOCKET_PATH, () => {
            console.log("Connected");
        });

        client.on("data", async (data) => {
            let enabled_map_name = JSON.parse(data)["map"];
            console.log(`Received: ${enabled_map_name}`);
            if (enabled_map_name === null) {
                console.log("Purging memory");
                loaded_map_name = null;
                loaded_map_data = [];
            } else if (enabled_map_name != loaded_map_name) {
                console.log("Parsing map");
                [loaded_map_name, loaded_map_data] = await parseMap(path.join(__dirname, map_conf.map_storage_path, enabled_map_name));
            }
        });

        client.on("end", () => {
            console.log("Disconnected");
            setTimeout(connect, can_conf.socket_connect_wait);
        });

        client.on("error", (err) => {
            console.log(`Error: ${err.message}`);
            setTimeout(connect, can_conf.socket_connect_wait);
        });

    } else {
        setTimeout(connect, can_conf.socket_connect_wait);
    }
}

(async () => {
    for (let i = 0; i <= map_conf.steps; i++) {
        map_rpm[i] = i * (map_conf.max_rpm / map_conf.steps);
        map_throttle[i] = i * (map_conf.max_throttle / map_conf.steps);
    }

    connect();
})();

channel.addListener("onMessage", async function (message) {
    switch (message.id) {
        case can_conf.selector.id:
            if (Buffer.compare(message.data, can_conf.selector.command) == 0) {
                console.log("Enable")
                enabled = true;
            } else {
                console.log("Disable");
                enabled = false;
            }

            break;
        case can_conf.apps_id:
            const apps_percentage = ((message.data[1] << 8) | message.data[0]) / 1000;
            var max_current = 0;
            throttle_output = 0;

            if (loaded_map_name !== null) {
                throttle_output = loaded_map_data[getMapIndex(map_throttle, apps_percentage)][2];
                max_current = loaded_map_data[getMapIndex(map_rpm, ERPM / can_conf.motor_poles)][(throttle_output > 0) ? 0 : 1];
            }

            let current = throttle_output * max_current * 10;
            channel.send({
                id: (throttle_output > 0) ? current_id : brake_current_id,
                ext: false,
                data: Buffer.from([(current >> 8) & 0xFF, current & 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF])
            });

            break;
        case erpm_id:
            if ((enabled == true) && (loaded_map_name !== null)) {
                ERPM = message.data[0] << 3 * 8 | message.data[1] << 2 * 8 | message.data[2] << 8 | message.data[3];

                let max_current = loaded_map_data[getMapIndex(map_rpm, ERPM / can_conf.motor_poles)][(throttle_output > 0) ? 0 : 1];
                let current = throttle_output * max_current * 10;
                channel.send({
                    id: (throttle_output > 0) ? current_id : brake_current_id,
                    ext: false,
                    data: Buffer.from([(current >> 8) & 0xFF, current & 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF])
                });
            } else if ((enabled == true) && (loaded_map_name === null)) {
                channel.send({
                    id: brake_current_id,
                    ext: false,
                    data: Buffer.from([0, 0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF])
                });
            }
            break;
        default: {
            break;
        }
    }
});

channel.start();