diff --git a/can.js b/can.js new file mode 100644 index 0000000..eeaad5c --- /dev/null +++ b/can.js @@ -0,0 +1,118 @@ +const can = require("socketcan"); +const csv = require("fast-csv"); +const fs = require("fs"); + +const can_conf = require("./app/config/can.conf"); +const map_conf = require("./app/config/map.conf"); + +var map_rpm = []; +var map_throttle = []; + +var loaded_map_name = ""; +var loaded_map_data = []; + +var throttle_output = 0; + +var channel = can.createRawChannel(can_conf.network_id, true); + +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); +} + +const current_id = (0x01 << 5) | can_conf.controller.id; +const brake_current_id = (0x02 << 5) | can_conf.controller.id; + +function parseMap(filename) { + loaded_map_data = []; + + return new Promise((resolve, reject) => { + csv.parseFile(`app/storage/${filename}`, { comment: "#" }) + .on('error', error => reject(error)) + .on('data', row => loaded_map_data.push(row)) + .on('end', () => { loaded_map_name = filename; resolve() }); + }); +} + +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; +} + +channel.addListener("onMessage", async function (message) { + if (fs.existsSync(map_conf.selected_map_path)) { + let enabled_map_name = fs.readFileSync(map_conf.selected_map_path); + + if (enabled_map_name != "") { + if (loaded_map_name != enabled_map_name) { + await parseMap(enabled_map_name); + } + + if (message.id == can_conf.apps_id) { + const apps_percentage = ((message.data[1] << 8) | message.data[0]) / 1000; + let pos = getMapIndex(map_throttle, apps_percentage); + + throttle_output = loaded_map_data[pos][2]; + } else { + let packet_id = message.id >> 5; + let node_id = message.id & 0x1F; + + if ((node_id == can_conf.controller.id) && (packet_id == 0x20)) { + let ERPM = message.data[0] << 3 * 8 | message.data[1] << 2 * 8 | message.data[2] << 8 | message.data[3]; + + if (throttle_output > 0) { + let max_current = loaded_map_data[getMapIndex(map_rpm, ERPM / can_conf.motor_poles)][0]; + let current = throttle_output * max_current * 10; + + channel.send({ + id: current_id, + ext: false, + data: Buffer.from([(current >> 8) & 0xFF, current & 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF]) + }); + } else if (throttle_output < 0) { + let max_brake_current = loaded_map_data[getMapIndex(map_rpm, ERPM / can_conf.motor_poles)][1]; + let brake_current = throttle_output * max_brake_current * 10; + + channel.send({ + id: brake_current_id, + ext: false, + data: Buffer.from([(brake_current >> 8) & 0xFF, brake_current & 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF]) + }); + } else { + channel.send({ + id: current_id, + ext: false, + data: Buffer.from([0, 0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF]) + }); + } + } + } + } + } +}); + +channel.start(); \ No newline at end of file