mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-16 01:21:20 -04:00
might ditch the esp32, this is the best ive gotten with it.
This commit is contained in:
legop3
@@ -0,0 +1,7 @@
|
||||
export function checksum8(buffer, length = buffer.length) {
|
||||
let sum = 0;
|
||||
for (let i = 0; i < length; i += 1) {
|
||||
sum = (sum + buffer[i]) & 0xff;
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
export const CONTROL_STREAM_HZ = 50;
|
||||
export const CONTROL_BIND_PORT = parseInt(process.env.CONTROL_BIND_PORT || '62000', 10);
|
||||
export const TELEMETRY_BIND_PORT = parseInt(process.env.TELEMETRY_BIND_PORT || '62001', 10);
|
||||
export const DEFAULT_DEVICE_CONTROL_PORT = parseInt(
|
||||
process.env.DEVICE_CONTROL_PORT || '50010',
|
||||
10,
|
||||
);
|
||||
|
||||
export const CONTROL_CONSTANTS = {
|
||||
MAGIC: 0xAA,
|
||||
VERSION: 1,
|
||||
ACTIONS: {
|
||||
SEEK_DOCK: 0x01,
|
||||
PLAY_SONG: 0x02,
|
||||
LOAD_SONG: 0x04,
|
||||
ENABLE_OI: 0x08,
|
||||
},
|
||||
MODES: {
|
||||
NO_CHANGE: 0,
|
||||
PASSIVE: 1,
|
||||
SAFE: 2,
|
||||
FULL: 3,
|
||||
},
|
||||
MAX_SPEED_MMPS: 500,
|
||||
};
|
||||
|
||||
export const TELEMETRY_CONSTANTS = {
|
||||
MAGIC: 0x55,
|
||||
VERSION: 1,
|
||||
HEADER_SIZE: 32,
|
||||
TRAILER_SIZE: 9,
|
||||
SENSOR_BLOB_BYTES: 80,
|
||||
MAX_ROBOT_ID_LEN: 16,
|
||||
};
|
||||
@@ -0,0 +1,61 @@
|
||||
import fs from 'fs';
|
||||
import path from 'path';
|
||||
import { fileURLToPath } from 'url';
|
||||
|
||||
import { DEFAULT_DEVICE_CONTROL_PORT } from './constants.js';
|
||||
|
||||
const REQUIRED_FIELDS = ['id'];
|
||||
|
||||
const moduleDir = path.dirname(fileURLToPath(import.meta.url));
|
||||
const serverRoot = path.resolve(moduleDir, '..');
|
||||
|
||||
function readJson(filePath) {
|
||||
if (!fs.existsSync(filePath)) {
|
||||
return null;
|
||||
}
|
||||
const content = fs.readFileSync(filePath, 'utf8');
|
||||
return JSON.parse(content);
|
||||
}
|
||||
|
||||
function resolveConfig() {
|
||||
const candidates = [
|
||||
path.join(serverRoot, 'robots.json'),
|
||||
path.join(serverRoot, 'robots.example.json'),
|
||||
path.join(process.cwd(), 'server', 'robots.json'),
|
||||
path.join(process.cwd(), 'server', 'robots.example.json'),
|
||||
];
|
||||
|
||||
for (const candidate of candidates) {
|
||||
const data = readJson(candidate);
|
||||
if (data) {
|
||||
if (candidate.endsWith('robots.example.json')) {
|
||||
console.warn('[robots] robots.json missing, using example configuration');
|
||||
}
|
||||
return data;
|
||||
}
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
export function loadRobots() {
|
||||
const payload = resolveConfig();
|
||||
if (!payload) {
|
||||
throw new Error('robots configuration file not found');
|
||||
}
|
||||
if (!Array.isArray(payload)) {
|
||||
throw new Error('robots configuration must be an array');
|
||||
}
|
||||
return payload.map((entry) => {
|
||||
for (const field of REQUIRED_FIELDS) {
|
||||
if (!entry[field]) {
|
||||
throw new Error(`robot entry missing field ${field}`);
|
||||
}
|
||||
}
|
||||
return {
|
||||
id: entry.id,
|
||||
host: entry.deviceHost || entry.host || null,
|
||||
controlPort: Number(entry.deviceControlPort || entry.controlPort || DEFAULT_DEVICE_CONTROL_PORT),
|
||||
maxWheelSpeed: Number(entry.maxWheelSpeed || 500),
|
||||
};
|
||||
});
|
||||
}
|
||||
@@ -0,0 +1,197 @@
|
||||
import path from 'path';
|
||||
import http from 'http';
|
||||
import dgram from 'dgram';
|
||||
import express from 'express';
|
||||
import { Server as SocketIo } from 'socket.io';
|
||||
import { fileURLToPath } from 'url';
|
||||
|
||||
import {
|
||||
CONTROL_BIND_PORT,
|
||||
CONTROL_CONSTANTS,
|
||||
CONTROL_STREAM_HZ,
|
||||
TELEMETRY_BIND_PORT,
|
||||
} from './constants.js';
|
||||
import { loadRobots } from './robotRegistry.js';
|
||||
import { buildControlPacket } from './udpPackets.js';
|
||||
import { decodeTelemetry } from './telemetryDecoder.js';
|
||||
|
||||
const __filename = fileURLToPath(import.meta.url);
|
||||
const __dirname = path.dirname(__filename);
|
||||
|
||||
const app = express();
|
||||
const server = http.createServer(app);
|
||||
const io = new SocketIo(server, {
|
||||
cors: {
|
||||
origin: '*',
|
||||
},
|
||||
});
|
||||
|
||||
const robots = loadRobots();
|
||||
if (robots.length === 0) {
|
||||
throw new Error('No robots configured. Add at least one entry to server/robots.json');
|
||||
}
|
||||
|
||||
const robotState = new Map();
|
||||
const telemetryState = new Map();
|
||||
|
||||
robots.forEach((robot) => {
|
||||
robotState.set(robot.id, {
|
||||
config: robot,
|
||||
seq: 0,
|
||||
leftMmps: 0,
|
||||
rightMmps: 0,
|
||||
pendingMode: CONTROL_CONSTANTS.MODES.NO_CHANGE,
|
||||
pendingActions: 0,
|
||||
songSlot: 0,
|
||||
lastKnownHost: robot.host || null,
|
||||
lastKnownPort: robot.controlPort,
|
||||
});
|
||||
});
|
||||
|
||||
const controlSocket = dgram.createSocket('udp4');
|
||||
controlSocket.on('error', (err) => {
|
||||
console.error('[control] socket error', err);
|
||||
});
|
||||
controlSocket.bind(CONTROL_BIND_PORT, () => {
|
||||
console.log(`[control] bound on port ${CONTROL_BIND_PORT}`);
|
||||
});
|
||||
|
||||
const telemetrySocket = dgram.createSocket('udp4');
|
||||
telemetrySocket.on('message', (msg, rinfo) => {
|
||||
try {
|
||||
const telemetry = decodeTelemetry(msg);
|
||||
const robotId = telemetry.header.robotId || rinfo.address;
|
||||
telemetryState.set(robotId, telemetry);
|
||||
const state = robotState.get(robotId);
|
||||
if (state) {
|
||||
state.lastKnownHost = rinfo.address;
|
||||
state.lastKnownPort = state.config.controlPort;
|
||||
} else {
|
||||
console.warn(`[telemetry] received frame from unknown robot ${robotId} (${rinfo.address})`);
|
||||
}
|
||||
io.emit('telemetry', { robotId, telemetry });
|
||||
} catch (err) {
|
||||
console.warn('[telemetry] failed to decode packet', err.message);
|
||||
}
|
||||
});
|
||||
telemetrySocket.bind(TELEMETRY_BIND_PORT, () => {
|
||||
console.log(`[telemetry] listening on port ${TELEMETRY_BIND_PORT}`);
|
||||
});
|
||||
|
||||
app.use(express.static(path.join(__dirname, '..', 'public')));
|
||||
|
||||
function clamp(value, min, max) {
|
||||
return Math.min(max, Math.max(min, value));
|
||||
}
|
||||
|
||||
function updateDrive(robotId, left, right) {
|
||||
const state = robotState.get(robotId);
|
||||
if (!state) {
|
||||
return;
|
||||
}
|
||||
const limit = state.config.maxWheelSpeed || CONTROL_CONSTANTS.MAX_SPEED_MMPS;
|
||||
const parsedLeft = Number(left) || 0;
|
||||
const parsedRight = Number(right) || 0;
|
||||
state.leftMmps = clamp(parsedLeft, -limit, limit);
|
||||
state.rightMmps = clamp(parsedRight, -limit, limit);
|
||||
}
|
||||
|
||||
function requestMode(robotId, mode) {
|
||||
const state = robotState.get(robotId);
|
||||
if (!state) {
|
||||
return;
|
||||
}
|
||||
state.pendingMode = mode;
|
||||
}
|
||||
|
||||
function triggerAction(robotId, actionBit, songSlot = 0) {
|
||||
const state = robotState.get(robotId);
|
||||
if (!state) {
|
||||
return;
|
||||
}
|
||||
state.pendingActions |= actionBit;
|
||||
state.songSlot = songSlot;
|
||||
}
|
||||
|
||||
function sendControlFrame(robotId) {
|
||||
const state = robotState.get(robotId);
|
||||
if (!state) {
|
||||
return;
|
||||
}
|
||||
if (!state.lastKnownHost) {
|
||||
return; // have not yet received telemetry -> cannot address robot
|
||||
}
|
||||
const packet = buildControlPacket({
|
||||
seq: state.seq++,
|
||||
leftMmps: state.leftMmps,
|
||||
rightMmps: state.rightMmps,
|
||||
mode: state.pendingMode,
|
||||
actions: state.pendingActions,
|
||||
songSlot: state.songSlot,
|
||||
});
|
||||
controlSocket.send(
|
||||
packet,
|
||||
0,
|
||||
packet.length,
|
||||
state.lastKnownPort,
|
||||
state.lastKnownHost,
|
||||
(err) => {
|
||||
if (err) {
|
||||
console.warn(`[control] failed to send to ${state.lastKnownHost}`, err.message);
|
||||
}
|
||||
},
|
||||
);
|
||||
state.pendingMode = CONTROL_CONSTANTS.MODES.NO_CHANGE;
|
||||
state.pendingActions = 0;
|
||||
}
|
||||
|
||||
setInterval(() => {
|
||||
for (const robot of robots) {
|
||||
sendControlFrame(robot.id);
|
||||
}
|
||||
}, Math.round(1000 / CONTROL_STREAM_HZ));
|
||||
|
||||
io.on('connection', (socket) => {
|
||||
console.log('[socket] client connected');
|
||||
socket.emit('robots', robots);
|
||||
socket.emit(
|
||||
'telemetrySnapshot',
|
||||
Array.from(telemetryState.entries()).map(([robotId, telemetry]) => ({
|
||||
robotId,
|
||||
telemetry,
|
||||
})),
|
||||
);
|
||||
|
||||
socket.on('drive', ({ robotId, left = 0, right = 0 } = {}) => {
|
||||
updateDrive(robotId, left, right);
|
||||
});
|
||||
|
||||
socket.on('mode', ({ robotId, mode }) => {
|
||||
const modes = CONTROL_CONSTANTS.MODES;
|
||||
const requested = mode
|
||||
? modes[mode.toUpperCase()] ?? modes.NO_CHANGE
|
||||
: modes.NO_CHANGE;
|
||||
requestMode(robotId, requested);
|
||||
});
|
||||
|
||||
socket.on('seekDock', ({ robotId }) => {
|
||||
triggerAction(robotId, CONTROL_CONSTANTS.ACTIONS.SEEK_DOCK);
|
||||
});
|
||||
|
||||
socket.on('enableOi', ({ robotId }) => {
|
||||
triggerAction(robotId, CONTROL_CONSTANTS.ACTIONS.ENABLE_OI);
|
||||
});
|
||||
|
||||
socket.on('playSong', ({ robotId, slot = 0 }) => {
|
||||
triggerAction(robotId, CONTROL_CONSTANTS.ACTIONS.PLAY_SONG, slot);
|
||||
});
|
||||
|
||||
socket.on('disconnect', () => {
|
||||
console.log('[socket] client disconnected');
|
||||
});
|
||||
});
|
||||
|
||||
const PORT = process.env.PORT || 8080;
|
||||
server.listen(PORT, () => {
|
||||
console.log(`[server] listening on http://localhost:${PORT}`);
|
||||
});
|
||||
@@ -0,0 +1,179 @@
|
||||
import { TELEMETRY_CONSTANTS } from './constants.js';
|
||||
import { checksum8 } from './checksum.js';
|
||||
|
||||
const BUTTON_BITS = {
|
||||
clean: 0x01,
|
||||
spot: 0x02,
|
||||
dock: 0x04,
|
||||
minute: 0x08,
|
||||
hour: 0x10,
|
||||
day: 0x20,
|
||||
schedule: 0x40,
|
||||
clock: 0x80,
|
||||
};
|
||||
|
||||
const BUMP_BITS = {
|
||||
bumpRight: 0x01,
|
||||
bumpLeft: 0x02,
|
||||
wheelDropRight: 0x04,
|
||||
wheelDropLeft: 0x08,
|
||||
};
|
||||
|
||||
const WHEEL_OVERCURRENT_BITS = {
|
||||
sideBrush: 0x01,
|
||||
mainBrush: 0x02,
|
||||
rightWheel: 0x04,
|
||||
leftWheel: 0x08,
|
||||
};
|
||||
|
||||
const CHARGE_SOURCE_BITS = {
|
||||
internalCharger: 0x01,
|
||||
homeBase: 0x02,
|
||||
};
|
||||
|
||||
const LIGHT_BUMPER_BITS = {
|
||||
left: 0x01,
|
||||
frontLeft: 0x02,
|
||||
centerLeft: 0x04,
|
||||
centerRight: 0x08,
|
||||
frontRight: 0x10,
|
||||
right: 0x20,
|
||||
};
|
||||
|
||||
const STASIS_BITS = {
|
||||
toggling: 0x01,
|
||||
disabled: 0x02,
|
||||
};
|
||||
|
||||
const boolField = (value, bits) => {
|
||||
const result = {};
|
||||
for (const [name, mask] of Object.entries(bits)) {
|
||||
result[name] = Boolean(value & mask);
|
||||
}
|
||||
return result;
|
||||
};
|
||||
|
||||
const readUInt16BE = (buf, offset) => buf.readUInt16BE(offset);
|
||||
const readInt16BE = (buf, offset) => buf.readInt16BE(offset);
|
||||
|
||||
function decodeSensorGroup100(buf) {
|
||||
if (!buf || buf.length === 0) {
|
||||
return null;
|
||||
}
|
||||
return {
|
||||
bumps: boolField(buf.readUInt8(0), BUMP_BITS),
|
||||
wall: Boolean(buf.readUInt8(1)),
|
||||
cliffLeft: Boolean(buf.readUInt8(2)),
|
||||
cliffFrontLeft: Boolean(buf.readUInt8(3)),
|
||||
cliffFrontRight: Boolean(buf.readUInt8(4)),
|
||||
cliffRight: Boolean(buf.readUInt8(5)),
|
||||
virtualWall: Boolean(buf.readUInt8(6)),
|
||||
wheelOvercurrents: boolField(buf.readUInt8(7), WHEEL_OVERCURRENT_BITS),
|
||||
dirtDetect: buf.readUInt8(8),
|
||||
irOpcode: buf.readUInt8(10),
|
||||
buttons: boolField(buf.readUInt8(11), BUTTON_BITS),
|
||||
distance: readInt16BE(buf, 12),
|
||||
angle: readInt16BE(buf, 14),
|
||||
chargingState: buf.readUInt8(16),
|
||||
voltageMv: readUInt16BE(buf, 17),
|
||||
currentMa: readInt16BE(buf, 19),
|
||||
temperatureC: buf.readInt8(21),
|
||||
batteryChargeMah: readUInt16BE(buf, 22),
|
||||
batteryCapacityMah: readUInt16BE(buf, 24),
|
||||
wallSignal: readUInt16BE(buf, 26),
|
||||
cliffSignals: {
|
||||
left: readUInt16BE(buf, 28),
|
||||
frontLeft: readUInt16BE(buf, 30),
|
||||
frontRight: readUInt16BE(buf, 32),
|
||||
right: readUInt16BE(buf, 34),
|
||||
},
|
||||
chargingSources: boolField(buf.readUInt8(39), CHARGE_SOURCE_BITS),
|
||||
oiMode: buf.readUInt8(40),
|
||||
songNumber: buf.readUInt8(41),
|
||||
songPlaying: Boolean(buf.readUInt8(42)),
|
||||
oiStreamPackets: buf.readUInt8(43),
|
||||
velocity: readInt16BE(buf, 44),
|
||||
radius: readInt16BE(buf, 46),
|
||||
velocityRight: readInt16BE(buf, 48),
|
||||
velocityLeft: readInt16BE(buf, 50),
|
||||
encoderCounts: {
|
||||
left: readUInt16BE(buf, 52),
|
||||
right: readUInt16BE(buf, 54),
|
||||
},
|
||||
lightBumper: boolField(buf.readUInt8(56), LIGHT_BUMPER_BITS),
|
||||
lightBumpSignals: {
|
||||
left: readUInt16BE(buf, 57),
|
||||
frontLeft: readUInt16BE(buf, 59),
|
||||
centerLeft: readUInt16BE(buf, 61),
|
||||
centerRight: readUInt16BE(buf, 63),
|
||||
frontRight: readUInt16BE(buf, 65),
|
||||
right: readUInt16BE(buf, 67),
|
||||
},
|
||||
irLeft: buf.readUInt8(69),
|
||||
irRight: buf.readUInt8(70),
|
||||
motorCurrents: {
|
||||
left: readInt16BE(buf, 71),
|
||||
right: readInt16BE(buf, 73),
|
||||
mainBrush: readInt16BE(buf, 75),
|
||||
sideBrush: readInt16BE(buf, 77),
|
||||
},
|
||||
stasis: boolField(buf.readUInt8(79), STASIS_BITS),
|
||||
};
|
||||
}
|
||||
|
||||
export function decodeTelemetry(message) {
|
||||
if (message.length < TELEMETRY_CONSTANTS.HEADER_SIZE + TELEMETRY_CONSTANTS.TRAILER_SIZE) {
|
||||
throw new Error('telemetry frame too small');
|
||||
}
|
||||
const expected = checksum8(message, message.length - 1);
|
||||
if (expected !== message.readUInt8(message.length - 1)) {
|
||||
throw new Error('telemetry checksum mismatch');
|
||||
}
|
||||
|
||||
const robotIdLength = Math.min(
|
||||
message.readUInt8(15),
|
||||
TELEMETRY_CONSTANTS.MAX_ROBOT_ID_LEN,
|
||||
);
|
||||
const rawRobotId = message.toString(
|
||||
'utf8',
|
||||
16,
|
||||
16 + TELEMETRY_CONSTANTS.MAX_ROBOT_ID_LEN,
|
||||
);
|
||||
const header = {
|
||||
magic: message.readUInt8(0),
|
||||
version: message.readUInt8(1),
|
||||
seq: message.readUInt16LE(2),
|
||||
uptimeMs: message.readUInt32LE(4),
|
||||
lastControlAgeMs: message.readUInt32LE(8),
|
||||
wifiRssiDbm: message.readInt8(12),
|
||||
statusBits: message.readUInt8(13),
|
||||
sensorBytes: message.readUInt8(14),
|
||||
robotIdLength,
|
||||
robotId: rawRobotId.slice(0, robotIdLength),
|
||||
};
|
||||
|
||||
if (header.magic !== TELEMETRY_CONSTANTS.MAGIC) {
|
||||
throw new Error(`unexpected telemetry magic ${header.magic}`);
|
||||
}
|
||||
if (header.version !== TELEMETRY_CONSTANTS.VERSION) {
|
||||
throw new Error(`unexpected telemetry version ${header.version}`);
|
||||
}
|
||||
|
||||
const sensorOffset = TELEMETRY_CONSTANTS.HEADER_SIZE;
|
||||
const trailerOffset = message.length - TELEMETRY_CONSTANTS.TRAILER_SIZE;
|
||||
if (sensorOffset + header.sensorBytes > trailerOffset) {
|
||||
throw new Error('sensor payload overruns buffer');
|
||||
}
|
||||
const sensorBlob = message.slice(sensorOffset, sensorOffset + header.sensorBytes);
|
||||
|
||||
return {
|
||||
header,
|
||||
sensors: header.sensorBytes ? decodeSensorGroup100(sensorBlob) : null,
|
||||
trailer: {
|
||||
appliedLeftMmps: message.readInt16LE(trailerOffset),
|
||||
appliedRightMmps: message.readInt16LE(trailerOffset + 2),
|
||||
lastControlSeq: message.readUInt16LE(trailerOffset + 4),
|
||||
droppedControlPackets: message.readUInt16LE(trailerOffset + 6),
|
||||
},
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
import { CONTROL_CONSTANTS } from './constants.js';
|
||||
import { checksum8 } from './checksum.js';
|
||||
|
||||
const CONTROL_PACKET_SIZE = 12;
|
||||
|
||||
export function buildControlPacket(state) {
|
||||
const buffer = Buffer.allocUnsafe(CONTROL_PACKET_SIZE);
|
||||
buffer.writeUInt8(CONTROL_CONSTANTS.MAGIC, 0);
|
||||
buffer.writeUInt8(CONTROL_CONSTANTS.VERSION, 1);
|
||||
buffer.writeUInt16LE(state.seq & 0xffff, 2);
|
||||
buffer.writeInt16LE(state.leftMmps, 4);
|
||||
buffer.writeInt16LE(state.rightMmps, 6);
|
||||
buffer.writeUInt8(state.mode ?? CONTROL_CONSTANTS.MODES.NO_CHANGE, 8);
|
||||
buffer.writeUInt8(state.actions ?? 0, 9);
|
||||
buffer.writeUInt8(state.songSlot ?? 0, 10);
|
||||
buffer.writeUInt8(checksum8(buffer, CONTROL_PACKET_SIZE - 1), 11);
|
||||
return buffer;
|
||||
}
|
||||
Reference in New Issue
Block a user