mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-15 17:12:59 -04:00
Compare commits
1
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 legop3
|
6d001b5263 |
@@ -7,3 +7,5 @@ logs
|
||||
node_modules/
|
||||
.pio
|
||||
.vscode/
|
||||
include/config.h
|
||||
server/robots.json
|
||||
|
||||
@@ -2,7 +2,9 @@
|
||||
|
||||
a remake of my RoombaRover project with a decentralized and embedded approach
|
||||
|
||||
on each roomba:
|
||||
## Hardware stack
|
||||
|
||||
On each roomba:
|
||||
- an esp32
|
||||
- a level shifter
|
||||
- DONT FORGET THE BRC PIN PULSE
|
||||
@@ -13,6 +15,50 @@ on each roomba:
|
||||
- speaker
|
||||
- MAYBE a master relay which can be turned off programatically to save the roomba from discharging. based on battery voltage plus urgent battery #?
|
||||
|
||||
UDP:
|
||||
one control stream out to the esp32
|
||||
one raw data sensor stream in form the esp32
|
||||
## Current software layout
|
||||
|
||||
```
|
||||
.
|
||||
├── include/
|
||||
│ ├── config.example.h // copy to config.h with your Wi-Fi + server settings
|
||||
│ └── protocol.h // shared packet layout (control + telemetry)
|
||||
├── src/main.cpp // ESP32 firmware entrypoint (PlatformIO)
|
||||
└── server/
|
||||
├── package.json // Node.js server + Socket.IO web UI
|
||||
├── robots.example.json // copy/edit to robots.json for your fleet
|
||||
├── src/ // UDP relay + telemetry decoder
|
||||
└── public/ // barebones HTML/JS UI
|
||||
```
|
||||
|
||||
### Firmware quickstart
|
||||
|
||||
1. `cp include/config.example.h include/config.h` and fill in:
|
||||
- `WIFI_SSID` / `WIFI_PASSWORD`
|
||||
- `CONTROL_SERVER_IP` (Node server host)
|
||||
- `ROOMBA_ID` (unique per robot; must match the server entry)
|
||||
- tweak ports only if you have a reason.
|
||||
2. Flash with PlatformIO: `pio run -t upload` (env `esp32s3`).
|
||||
3. The firmware spawns three FreeRTOS tasks:
|
||||
- control loop (5 ms cadence) – consumes UDP control packets and drives the Create 2 via UART pins 16/17. Wheel commands decay to zero if no packets arrive for 250 ms.
|
||||
- telemetry loop (500 ms cadence) – polls sensor group 100, appends Wi-Fi/LRU stats, and streams UDP telemetry to the server.
|
||||
- BRC maintenance – pulses GPIO5 low for 1 s every minute to keep the robot awake.
|
||||
|
||||
### Server + web UI quickstart
|
||||
|
||||
1. `cd server`
|
||||
2. `cp robots.example.json robots.json` and add one entry per robot. Only the `id` is required (must match `ROOMBA_ID` in the firmware); override `controlPort`/`maxWheelSpeed` if you deviate from defaults.
|
||||
3. Install deps: `npm install`
|
||||
4. Run in dev mode: `npm run dev`
|
||||
- HTTP + Socket.IO on `http://localhost:8080`
|
||||
- UDP control bind port `62000`, telemetry bind port `62001` (override with env vars).
|
||||
5. Open the web UI:
|
||||
- select a robot
|
||||
- drive with WASD (left/right wheel mm/s shown in telemetry summary)
|
||||
- buttons issue Safe/Full/Enable-OI/Dock commands
|
||||
- sensor list renders the decoded Create 2 group-100 payload plus ESP stats
|
||||
|
||||
Each ESP32 announces itself as soon as it streams telemetry, so the server automatically learns the robot’s current IP address (no static DHCP entries required). If you do know a static IP, you can still set `deviceHost` in `robots.json` and the server will use it immediately.
|
||||
|
||||
UDP streams stay simple:
|
||||
- server -> ESP32: fixed 12-byte control packet blasted at 50 Hz per robot
|
||||
- ESP32 -> server: framed telemetry header + raw sensor group 100 + trailer (CRC-8)
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
|
||||
// Copy this file to include/config.h and fill in your network + server settings.
|
||||
|
||||
#define WIFI_SSID "YourNetworkName"
|
||||
#define WIFI_PASSWORD "YourNetworkPassword"
|
||||
|
||||
// UDP server that issues control packets and receives telemetry.
|
||||
#define CONTROL_SERVER_IP "192.168.1.50"
|
||||
#define CONTROL_SERVER_PORT 62000
|
||||
#define TELEMETRY_SERVER_PORT 62001
|
||||
|
||||
// Local ports on the ESP32. Keeping them distinct simplifies sniffing.
|
||||
#define ESP32_CONTROL_PORT 50010
|
||||
#define ESP32_TELEMETRY_PORT 50011
|
||||
|
||||
// Friendly name to embed in telemetry.
|
||||
#define ROOMBA_ID "roomba-alpha"
|
||||
@@ -0,0 +1,85 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <type_traits>
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
namespace mrr {
|
||||
|
||||
constexpr uint8_t kControlMagic = 0xAA;
|
||||
constexpr uint8_t kTelemetryMagic = 0x55;
|
||||
constexpr uint8_t kProtocolVersion = 1;
|
||||
constexpr size_t kSensorGroup100Length = 80;
|
||||
constexpr size_t kMaxRobotIdLength = 16;
|
||||
|
||||
enum class OiModeRequest : uint8_t {
|
||||
kNoChange = 0,
|
||||
kPassive = 1,
|
||||
kSafe = 2,
|
||||
kFull = 3,
|
||||
};
|
||||
|
||||
enum ActionBits : uint8_t {
|
||||
kActionSeekDock = 0x01,
|
||||
kActionPlaySong = 0x02,
|
||||
kActionLoadSong = 0x04,
|
||||
kActionEnableOi = 0x08,
|
||||
};
|
||||
|
||||
struct __attribute__((packed)) ControlPacket {
|
||||
uint8_t magic{kControlMagic};
|
||||
uint8_t version{kProtocolVersion};
|
||||
uint16_t seq{};
|
||||
int16_t left_mmps{};
|
||||
int16_t right_mmps{};
|
||||
uint8_t oi_mode{};
|
||||
uint8_t actions{};
|
||||
uint8_t song_slot{};
|
||||
uint8_t checksum{};
|
||||
};
|
||||
|
||||
static_assert(sizeof(ControlPacket) == 12, "ControlPacket must remain packed");
|
||||
|
||||
struct __attribute__((packed)) TelemetryPacketHeader {
|
||||
uint8_t magic{kTelemetryMagic};
|
||||
uint8_t version{kProtocolVersion};
|
||||
uint16_t seq{};
|
||||
uint32_t uptime_ms{};
|
||||
uint32_t last_control_age_ms{};
|
||||
int8_t wifi_rssi_dbm{};
|
||||
uint8_t status_bits{};
|
||||
uint8_t sensor_bytes{};
|
||||
uint8_t robot_id_length{};
|
||||
char robot_id[kMaxRobotIdLength]{};
|
||||
};
|
||||
|
||||
struct __attribute__((packed)) TelemetryPacketTrailer {
|
||||
int16_t applied_left_mmps{};
|
||||
int16_t applied_right_mmps{};
|
||||
uint16_t last_control_seq{};
|
||||
uint16_t dropped_control_packets{};
|
||||
uint8_t checksum{};
|
||||
};
|
||||
|
||||
inline uint8_t checksum8(const uint8_t* data, size_t len) {
|
||||
uint32_t sum = 0;
|
||||
for (size_t i = 0; i < len; ++i) {
|
||||
sum += data[i];
|
||||
}
|
||||
return static_cast<uint8_t>(sum & 0xFF);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline uint8_t checksumPayload(const T& pod) {
|
||||
static_assert(std::is_trivially_copyable<T>::value, "checksum payload must be POD");
|
||||
return checksum8(reinterpret_cast<const uint8_t*>(&pod), sizeof(T));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline uint8_t checksumExcludingLastByte(const T& pod) {
|
||||
static_assert(std::is_trivially_copyable<T>::value, "checksum payload must be POD");
|
||||
return checksum8(reinterpret_cast<const uint8_t*>(&pod), sizeof(T) - 1);
|
||||
}
|
||||
|
||||
} // namespace mrr
|
||||
+2
-1
@@ -1,4 +1,5 @@
|
||||
[env:esp32s3]
|
||||
platform = espressif32
|
||||
board = esp32-s3-devkitc-1
|
||||
monitor_speed = 115200
|
||||
monitor_speed = 115200
|
||||
framework = arduino
|
||||
Generated
+1462
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,18 @@
|
||||
{
|
||||
"name": "multi-roomba-rover-server",
|
||||
"version": "0.1.0",
|
||||
"type": "module",
|
||||
"description": "UDP relay and web UI for MultiRoombaRover",
|
||||
"main": "src/server.js",
|
||||
"scripts": {
|
||||
"start": "node src/server.js",
|
||||
"dev": "nodemon src/server.js"
|
||||
},
|
||||
"dependencies": {
|
||||
"express": "^4.19.2",
|
||||
"socket.io": "^4.7.5"
|
||||
},
|
||||
"devDependencies": {
|
||||
"nodemon": "^3.0.3"
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,193 @@
|
||||
const socket = io();
|
||||
|
||||
const DRIVE_SPEED = 250;
|
||||
const TURN_SPEED = 200;
|
||||
const STATUS_FLAGS = [
|
||||
{ bit: 0x01, label: 'wifi' },
|
||||
{ bit: 0x02, label: 'oi-ready' },
|
||||
{ bit: 0x04, label: 'sensors' },
|
||||
];
|
||||
|
||||
const state = {
|
||||
robots: [],
|
||||
selectedRobotId: null,
|
||||
telemetry: {},
|
||||
activeKeys: new Set(),
|
||||
};
|
||||
|
||||
const robotSelect = document.getElementById('robotSelect');
|
||||
const telemetrySummary = document.getElementById('telemetrySummary');
|
||||
const sensorList = document.getElementById('sensorList');
|
||||
const safeModeBtn = document.getElementById('safeModeBtn');
|
||||
const fullModeBtn = document.getElementById('fullModeBtn');
|
||||
const enableOiBtn = document.getElementById('enableOiBtn');
|
||||
const seekDockBtn = document.getElementById('seekDockBtn');
|
||||
const playSongBtn = document.getElementById('playSongBtn');
|
||||
const songSlotInput = document.getElementById('songSlot');
|
||||
|
||||
function flattenSensors(obj, prefix = '') {
|
||||
const result = {};
|
||||
Object.entries(obj || {}).forEach(([key, value]) => {
|
||||
const path = prefix ? `${prefix}.${key}` : key;
|
||||
if (value && typeof value === 'object' && !Array.isArray(value)) {
|
||||
Object.assign(result, flattenSensors(value, path));
|
||||
} else {
|
||||
result[path] = value;
|
||||
}
|
||||
});
|
||||
return result;
|
||||
}
|
||||
|
||||
function renderRobots() {
|
||||
robotSelect.innerHTML = '';
|
||||
state.robots.forEach((robot) => {
|
||||
const option = document.createElement('option');
|
||||
option.value = robot.id;
|
||||
option.textContent = robot.id;
|
||||
if (robot.id === state.selectedRobotId) {
|
||||
option.selected = true;
|
||||
}
|
||||
robotSelect.appendChild(option);
|
||||
});
|
||||
}
|
||||
|
||||
function renderTelemetry() {
|
||||
const telemetry = state.telemetry[state.selectedRobotId];
|
||||
if (!telemetry) {
|
||||
telemetrySummary.textContent = 'No telemetry';
|
||||
sensorList.textContent = '';
|
||||
return;
|
||||
}
|
||||
const { header, trailer, sensors } = telemetry;
|
||||
const flags = STATUS_FLAGS
|
||||
.filter((flag) => header.statusBits & flag.bit)
|
||||
.map((flag) => flag.label)
|
||||
.join(', ');
|
||||
const summaryLines = [
|
||||
`Seq: ${header.seq}`,
|
||||
`Uptime: ${header.uptimeMs} ms`,
|
||||
`Last Control Age: ${header.lastControlAgeMs} ms`,
|
||||
`WiFi RSSI: ${header.wifiRssiDbm} dBm`,
|
||||
`Status: ${flags || 'none'}`,
|
||||
`Applied mm/s: L ${trailer.appliedLeftMmps} | R ${trailer.appliedRightMmps}`,
|
||||
`Dropped control packets: ${trailer.droppedControlPackets}`,
|
||||
];
|
||||
telemetrySummary.textContent = summaryLines.join('\n');
|
||||
|
||||
if (sensors) {
|
||||
const flat = flattenSensors(sensors);
|
||||
sensorList.textContent = Object.entries(flat)
|
||||
.map(([key, value]) => `${key}: ${value}`)
|
||||
.join('\n');
|
||||
} else {
|
||||
sensorList.textContent = 'Sensor block missing';
|
||||
}
|
||||
}
|
||||
|
||||
function broadcastDrive() {
|
||||
if (!state.selectedRobotId) {
|
||||
return;
|
||||
}
|
||||
const vectors = { w: 0, a: 0, s: 0, d: 0 };
|
||||
state.activeKeys.forEach((key) => {
|
||||
if (vectors[key] !== undefined) {
|
||||
vectors[key] = 1;
|
||||
}
|
||||
});
|
||||
|
||||
let left = 0;
|
||||
let right = 0;
|
||||
if (vectors.w) {
|
||||
left += DRIVE_SPEED;
|
||||
right += DRIVE_SPEED;
|
||||
}
|
||||
if (vectors.s) {
|
||||
left -= DRIVE_SPEED;
|
||||
right -= DRIVE_SPEED;
|
||||
}
|
||||
if (vectors.a) {
|
||||
left -= TURN_SPEED;
|
||||
right += TURN_SPEED;
|
||||
}
|
||||
if (vectors.d) {
|
||||
left += TURN_SPEED;
|
||||
right -= TURN_SPEED;
|
||||
}
|
||||
|
||||
socket.emit('drive', {
|
||||
robotId: state.selectedRobotId,
|
||||
left,
|
||||
right,
|
||||
});
|
||||
}
|
||||
|
||||
function handleKey(event, isDown) {
|
||||
const key = event.key.toLowerCase();
|
||||
if (!['w', 'a', 's', 'd'].includes(key)) {
|
||||
return;
|
||||
}
|
||||
event.preventDefault();
|
||||
if (isDown) {
|
||||
state.activeKeys.add(key);
|
||||
} else {
|
||||
state.activeKeys.delete(key);
|
||||
}
|
||||
broadcastDrive();
|
||||
}
|
||||
|
||||
document.addEventListener('keydown', (event) => handleKey(event, true));
|
||||
document.addEventListener('keyup', (event) => handleKey(event, false));
|
||||
|
||||
robotSelect.addEventListener('change', (event) => {
|
||||
state.selectedRobotId = event.target.value;
|
||||
renderTelemetry();
|
||||
});
|
||||
|
||||
safeModeBtn.addEventListener('click', () => {
|
||||
if (!state.selectedRobotId) return;
|
||||
socket.emit('mode', { robotId: state.selectedRobotId, mode: 'SAFE' });
|
||||
});
|
||||
|
||||
fullModeBtn.addEventListener('click', () => {
|
||||
if (!state.selectedRobotId) return;
|
||||
socket.emit('mode', { robotId: state.selectedRobotId, mode: 'FULL' });
|
||||
});
|
||||
|
||||
enableOiBtn.addEventListener('click', () => {
|
||||
if (!state.selectedRobotId) return;
|
||||
socket.emit('enableOi', { robotId: state.selectedRobotId });
|
||||
});
|
||||
|
||||
seekDockBtn.addEventListener('click', () => {
|
||||
if (!state.selectedRobotId) return;
|
||||
socket.emit('seekDock', { robotId: state.selectedRobotId });
|
||||
});
|
||||
|
||||
playSongBtn.addEventListener('click', () => {
|
||||
if (!state.selectedRobotId) return;
|
||||
const slot = Number(songSlotInput.value) || 0;
|
||||
socket.emit('playSong', { robotId: state.selectedRobotId, slot });
|
||||
});
|
||||
|
||||
socket.on('robots', (robots) => {
|
||||
state.robots = robots;
|
||||
if (!state.selectedRobotId && robots.length > 0) {
|
||||
state.selectedRobotId = robots[0].id;
|
||||
}
|
||||
renderRobots();
|
||||
renderTelemetry();
|
||||
});
|
||||
|
||||
socket.on('telemetrySnapshot', (entries) => {
|
||||
entries.forEach(({ robotId, telemetry }) => {
|
||||
state.telemetry[robotId] = telemetry;
|
||||
});
|
||||
renderTelemetry();
|
||||
});
|
||||
|
||||
socket.on('telemetry', ({ robotId, telemetry }) => {
|
||||
state.telemetry[robotId] = telemetry;
|
||||
if (robotId === state.selectedRobotId) {
|
||||
renderTelemetry();
|
||||
}
|
||||
});
|
||||
@@ -0,0 +1,37 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8" />
|
||||
<title>MultiRoombaRover</title>
|
||||
</head>
|
||||
<body>
|
||||
<main>
|
||||
<h1>MultiRoombaRover</h1>
|
||||
<section>
|
||||
<label for="robotSelect">Select Roomba:</label>
|
||||
<select id="robotSelect"></select>
|
||||
</section>
|
||||
<section id="driveHints">
|
||||
<p>Use WASD for drive control. Release keys to stop.</p>
|
||||
<div>
|
||||
<button id="safeModeBtn">Safe Mode</button>
|
||||
<button id="fullModeBtn">Full Mode</button>
|
||||
<button id="enableOiBtn">Enable OI</button>
|
||||
<button id="seekDockBtn">Seek Dock</button>
|
||||
</div>
|
||||
</section>
|
||||
<section>
|
||||
<label for="songSlot">Song Slot:</label>
|
||||
<input type="number" id="songSlot" value="0" min="0" max="15" />
|
||||
<button id="playSongBtn">Play Song</button>
|
||||
</section>
|
||||
<section>
|
||||
<h2>Telemetry</h2>
|
||||
<pre id="telemetrySummary"></pre>
|
||||
<pre id="sensorList"></pre>
|
||||
</section>
|
||||
</main>
|
||||
<script src="/socket.io/socket.io.js"></script>
|
||||
<script type="module" src="./app.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,7 @@
|
||||
[
|
||||
{
|
||||
"id": "roomba-alpha",
|
||||
"controlPort": 50010,
|
||||
"maxWheelSpeed": 350
|
||||
}
|
||||
]
|
||||
@@ -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;
|
||||
}
|
||||
+369
@@ -0,0 +1,369 @@
|
||||
#include <Arduino.h>
|
||||
#include <WiFi.h>
|
||||
#include <WiFiUdp.h>
|
||||
#include <esp_wifi.h>
|
||||
#include <cstring>
|
||||
|
||||
#include "config.h"
|
||||
#include "protocol.h"
|
||||
|
||||
using namespace mrr;
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr gpio_num_t kRoombaRxPin = GPIO_NUM_16;
|
||||
constexpr gpio_num_t kRoombaTxPin = GPIO_NUM_17;
|
||||
constexpr gpio_num_t kRoombaBrcPin = GPIO_NUM_5;
|
||||
|
||||
constexpr uint32_t kControlLoopDelayMs = 5;
|
||||
constexpr uint32_t kControlTimeoutMs = 250;
|
||||
constexpr uint32_t kTelemetryIntervalMs = 500;
|
||||
constexpr uint32_t kSensorRequestTimeoutMs = 75;
|
||||
constexpr uint32_t kBrcPulseDurationMs = 1000;
|
||||
constexpr uint32_t kBrcPulsePeriodMs = 60000;
|
||||
|
||||
constexpr size_t kTelemetryBufferSize =
|
||||
sizeof(TelemetryPacketHeader) + kSensorGroup100Length + sizeof(TelemetryPacketTrailer);
|
||||
|
||||
HardwareSerial& kRoombaSerial = Serial2;
|
||||
WiFiUDP gControlSocket;
|
||||
WiFiUDP gTelemetrySocket;
|
||||
|
||||
IPAddress gServerIp;
|
||||
|
||||
TaskHandle_t gControlTaskHandle = nullptr;
|
||||
TaskHandle_t gTelemetryTaskHandle = nullptr;
|
||||
TaskHandle_t gBrcTaskHandle = nullptr;
|
||||
|
||||
struct ControlState {
|
||||
int16_t left_mmps{0};
|
||||
int16_t right_mmps{0};
|
||||
OiModeRequest requested_mode{OiModeRequest::kNoChange};
|
||||
uint8_t actions{0};
|
||||
uint8_t song_slot{0};
|
||||
uint16_t seq{0};
|
||||
uint32_t last_rx_ms{0};
|
||||
};
|
||||
|
||||
ControlState gLatestControl{};
|
||||
uint16_t gLastAppliedSeq = 0;
|
||||
uint16_t gDroppedControlPackets = 0;
|
||||
portMUX_TYPE gControlMux = portMUX_INITIALIZER_UNLOCKED;
|
||||
|
||||
uint32_t gLastSensorOkMs = 0;
|
||||
|
||||
enum StatusBits : uint8_t {
|
||||
kStatusWifiConnected = 0x01,
|
||||
kStatusRoombaReady = 0x02,
|
||||
kStatusSensorHealthy = 0x04,
|
||||
};
|
||||
|
||||
class RoombaInterface {
|
||||
public:
|
||||
void begin() {
|
||||
serial_ = &kRoombaSerial;
|
||||
serial_->begin(115200, SERIAL_8N1, kRoombaRxPin, kRoombaTxPin);
|
||||
serial_->setTimeout(30); // shorter timeout to avoid blocking control loop
|
||||
}
|
||||
|
||||
bool ensureStarted() {
|
||||
if (!serial_) {
|
||||
return false;
|
||||
}
|
||||
if (ready_) {
|
||||
return true;
|
||||
}
|
||||
sendOpcode(128); // Start => Passive
|
||||
delay(20);
|
||||
sendOpcode(131); // Safe by default
|
||||
ready_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool setMode(OiModeRequest request) {
|
||||
if (!ensureStarted()) {
|
||||
return false;
|
||||
}
|
||||
switch (request) {
|
||||
case OiModeRequest::kNoChange:
|
||||
return true;
|
||||
case OiModeRequest::kPassive:
|
||||
return sendOpcode(128);
|
||||
case OiModeRequest::kSafe:
|
||||
return sendOpcode(131);
|
||||
case OiModeRequest::kFull:
|
||||
return sendOpcode(132);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool driveDirect(int16_t left_mmps, int16_t right_mmps) {
|
||||
if (!ensureStarted()) {
|
||||
return false;
|
||||
}
|
||||
uint8_t payload[5];
|
||||
payload[0] = 145; // Drive Direct opcode
|
||||
payload[1] = static_cast<uint8_t>((right_mmps >> 8) & 0xFF);
|
||||
payload[2] = static_cast<uint8_t>(right_mmps & 0xFF);
|
||||
payload[3] = static_cast<uint8_t>((left_mmps >> 8) & 0xFF);
|
||||
payload[4] = static_cast<uint8_t>(left_mmps & 0xFF);
|
||||
return serial_->write(payload, sizeof(payload)) == sizeof(payload);
|
||||
}
|
||||
|
||||
bool seekDock() { return ensureStarted() && sendOpcode(143); }
|
||||
bool playSong(uint8_t slot) {
|
||||
if (!ensureStarted()) {
|
||||
return false;
|
||||
}
|
||||
uint8_t payload[2] = {141, slot};
|
||||
return serial_->write(payload, sizeof(payload)) == sizeof(payload);
|
||||
}
|
||||
|
||||
bool requestSensors(uint8_t packet_id, uint8_t* buffer, size_t expected_bytes) {
|
||||
if (!ensureStarted()) {
|
||||
return false;
|
||||
}
|
||||
serial_->write(142);
|
||||
serial_->write(packet_id);
|
||||
const size_t read = serial_->readBytes(buffer, expected_bytes);
|
||||
return read == expected_bytes;
|
||||
}
|
||||
|
||||
bool isReady() const { return ready_; }
|
||||
|
||||
private:
|
||||
bool sendOpcode(uint8_t opcode) { return serial_ && serial_->write(opcode) == 1; }
|
||||
|
||||
HardwareSerial* serial_{nullptr};
|
||||
bool ready_{false};
|
||||
};
|
||||
|
||||
RoombaInterface gRoomba;
|
||||
|
||||
void disableWifiPowerSave() {
|
||||
WiFi.setSleep(false);
|
||||
esp_wifi_set_ps(WIFI_PS_NONE);
|
||||
}
|
||||
|
||||
void connectWifi() {
|
||||
WiFi.mode(WIFI_STA);
|
||||
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
|
||||
Serial.printf("[wifi] connecting to %s\\n", WIFI_SSID);
|
||||
uint32_t start = millis();
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
delay(250);
|
||||
Serial.print(".");
|
||||
if (millis() - start > 20000) {
|
||||
Serial.println("\\n[wifi] retrying...");
|
||||
WiFi.disconnect();
|
||||
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
|
||||
start = millis();
|
||||
}
|
||||
}
|
||||
disableWifiPowerSave();
|
||||
Serial.printf("\\n[wifi] connected, ip=%s\\n", WiFi.localIP().toString().c_str());
|
||||
}
|
||||
|
||||
IPAddress resolveServerIp() {
|
||||
IPAddress ip;
|
||||
if (!ip.fromString(CONTROL_SERVER_IP)) {
|
||||
Serial.printf("[wifi] invalid CONTROL_SERVER_IP: %s\\n", CONTROL_SERVER_IP);
|
||||
}
|
||||
return ip;
|
||||
}
|
||||
|
||||
ControlState snapshotControl() {
|
||||
portENTER_CRITICAL(&gControlMux);
|
||||
ControlState copy = gLatestControl;
|
||||
portEXIT_CRITICAL(&gControlMux);
|
||||
return copy;
|
||||
}
|
||||
|
||||
void updateControl(const ControlPacket& pkt) {
|
||||
ControlState state;
|
||||
state.left_mmps = pkt.left_mmps;
|
||||
state.right_mmps = pkt.right_mmps;
|
||||
state.actions = pkt.actions;
|
||||
state.seq = pkt.seq;
|
||||
state.song_slot = pkt.song_slot;
|
||||
state.last_rx_ms = millis();
|
||||
state.requested_mode = static_cast<OiModeRequest>(pkt.oi_mode);
|
||||
portENTER_CRITICAL(&gControlMux);
|
||||
gLatestControl = state;
|
||||
portEXIT_CRITICAL(&gControlMux);
|
||||
}
|
||||
|
||||
void zeroDrive() {
|
||||
gRoomba.driveDirect(0, 0);
|
||||
}
|
||||
|
||||
void handleControlApplication(const ControlState& state) {
|
||||
const uint32_t now = millis();
|
||||
int16_t left = state.left_mmps;
|
||||
int16_t right = state.right_mmps;
|
||||
const bool stale = (now - state.last_rx_ms) > kControlTimeoutMs;
|
||||
if (stale) {
|
||||
left = 0;
|
||||
right = 0;
|
||||
}
|
||||
|
||||
if (state.seq != gLastAppliedSeq) {
|
||||
if (state.requested_mode != OiModeRequest::kNoChange) {
|
||||
gRoomba.setMode(state.requested_mode);
|
||||
} else if (state.actions & kActionEnableOi) {
|
||||
gRoomba.ensureStarted();
|
||||
}
|
||||
|
||||
if (state.actions & kActionSeekDock) {
|
||||
gRoomba.seekDock();
|
||||
}
|
||||
if (state.actions & kActionPlaySong) {
|
||||
gRoomba.playSong(state.song_slot);
|
||||
}
|
||||
// kActionLoadSong can be added later when song definitions are ready.
|
||||
|
||||
gLastAppliedSeq = state.seq;
|
||||
}
|
||||
|
||||
gRoomba.driveDirect(left, right);
|
||||
}
|
||||
|
||||
void controlTask(void*) {
|
||||
uint8_t buffer[sizeof(ControlPacket)] = {0};
|
||||
ControlPacket newest{};
|
||||
bool hasNewest = false;
|
||||
for (;;) {
|
||||
hasNewest = false;
|
||||
while (gControlSocket.parsePacket() >= static_cast<int>(sizeof(ControlPacket))) {
|
||||
const int read = gControlSocket.read(buffer, sizeof(ControlPacket));
|
||||
if (read != sizeof(ControlPacket)) {
|
||||
continue;
|
||||
}
|
||||
ControlPacket pkt;
|
||||
memcpy(&pkt, buffer, sizeof(ControlPacket));
|
||||
const uint8_t computed =
|
||||
checksum8(reinterpret_cast<const uint8_t*>(&pkt), sizeof(ControlPacket) - 1);
|
||||
if (pkt.magic == kControlMagic && pkt.version == kProtocolVersion && computed == pkt.checksum) {
|
||||
newest = pkt;
|
||||
hasNewest = true;
|
||||
} else {
|
||||
++gDroppedControlPackets;
|
||||
}
|
||||
}
|
||||
|
||||
if (hasNewest) {
|
||||
const uint16_t delta = static_cast<uint16_t>(newest.seq - gLastAppliedSeq);
|
||||
if (delta != 0 && delta < 0x8000) {
|
||||
updateControl(newest);
|
||||
}
|
||||
}
|
||||
|
||||
handleControlApplication(snapshotControl());
|
||||
vTaskDelay(pdMS_TO_TICKS(kControlLoopDelayMs));
|
||||
}
|
||||
}
|
||||
|
||||
void telemetryTask(void*) {
|
||||
uint8_t buffer[kTelemetryBufferSize];
|
||||
uint8_t sensorBlob[kSensorGroup100Length];
|
||||
uint16_t telemetrySeq = 0;
|
||||
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
vTaskDelay(pdMS_TO_TICKS(250));
|
||||
}
|
||||
|
||||
for (;;) {
|
||||
TelemetryPacketHeader header;
|
||||
memset(&header, 0, sizeof(header));
|
||||
header.magic = kTelemetryMagic;
|
||||
header.version = kProtocolVersion;
|
||||
header.seq = telemetrySeq++;
|
||||
header.uptime_ms = millis();
|
||||
const ControlState control = snapshotControl();
|
||||
header.last_control_age_ms = millis() - control.last_rx_ms;
|
||||
header.wifi_rssi_dbm = WiFi.RSSI();
|
||||
header.status_bits = 0;
|
||||
if (WiFi.status() == WL_CONNECTED) {
|
||||
header.status_bits |= kStatusWifiConnected;
|
||||
}
|
||||
if (gRoomba.isReady()) {
|
||||
header.status_bits |= kStatusRoombaReady;
|
||||
}
|
||||
|
||||
size_t sensorLen = 0;
|
||||
if (gRoomba.requestSensors(100, sensorBlob, kSensorGroup100Length)) {
|
||||
sensorLen = kSensorGroup100Length;
|
||||
gLastSensorOkMs = millis();
|
||||
}
|
||||
if (millis() - gLastSensorOkMs < 2000) {
|
||||
header.status_bits |= kStatusSensorHealthy;
|
||||
}
|
||||
header.sensor_bytes = static_cast<uint8_t>(sensorLen);
|
||||
const char* robotId = ROOMBA_ID;
|
||||
header.robot_id_length = static_cast<uint8_t>(strnlen(robotId, kMaxRobotIdLength));
|
||||
memcpy(header.robot_id, robotId, header.robot_id_length);
|
||||
|
||||
TelemetryPacketTrailer trailer;
|
||||
memset(&trailer, 0, sizeof(trailer));
|
||||
trailer.applied_left_mmps = control.left_mmps;
|
||||
trailer.applied_right_mmps = control.right_mmps;
|
||||
trailer.last_control_seq = control.seq;
|
||||
trailer.dropped_control_packets = gDroppedControlPackets;
|
||||
|
||||
size_t offset = 0;
|
||||
memcpy(buffer + offset, &header, sizeof(header));
|
||||
offset += sizeof(header);
|
||||
if (sensorLen > 0) {
|
||||
memcpy(buffer + offset, sensorBlob, sensorLen);
|
||||
offset += sensorLen;
|
||||
}
|
||||
memcpy(buffer + offset, &trailer, sizeof(trailer));
|
||||
offset += sizeof(trailer);
|
||||
|
||||
const uint8_t checksum = checksum8(buffer, offset - 1);
|
||||
buffer[offset - 1] = checksum;
|
||||
|
||||
if (gServerIp) {
|
||||
gTelemetrySocket.beginPacket(gServerIp, TELEMETRY_SERVER_PORT);
|
||||
gTelemetrySocket.write(buffer, offset);
|
||||
gTelemetrySocket.endPacket();
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(kTelemetryIntervalMs));
|
||||
}
|
||||
}
|
||||
|
||||
void brcTask(void*) {
|
||||
pinMode(kRoombaBrcPin, OUTPUT);
|
||||
digitalWrite(kRoombaBrcPin, HIGH);
|
||||
for (;;) {
|
||||
digitalWrite(kRoombaBrcPin, LOW);
|
||||
vTaskDelay(pdMS_TO_TICKS(kBrcPulseDurationMs));
|
||||
digitalWrite(kRoombaBrcPin, HIGH);
|
||||
vTaskDelay(pdMS_TO_TICKS(kBrcPulsePeriodMs - kBrcPulseDurationMs));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
delay(50);
|
||||
Serial.println("[boot] MultiRoombaRover firmware starting");
|
||||
|
||||
gRoomba.begin();
|
||||
connectWifi();
|
||||
gServerIp = resolveServerIp();
|
||||
|
||||
gControlSocket.begin(ESP32_CONTROL_PORT);
|
||||
gTelemetrySocket.begin(ESP32_TELEMETRY_PORT);
|
||||
|
||||
xTaskCreatePinnedToCore(controlTask, "control", 4096, nullptr, 3, &gControlTaskHandle, APP_CPU_NUM);
|
||||
xTaskCreatePinnedToCore(telemetryTask, "telemetry", 4096, nullptr, 2, &gTelemetryTaskHandle, PRO_CPU_NUM);
|
||||
xTaskCreatePinnedToCore(brcTask, "brc", 2048, nullptr, 1, &gBrcTaskHandle, APP_CPU_NUM);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Nothing to do. All work happens inside FreeRTOS tasks.
|
||||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||||
}
|
||||
Reference in New Issue
Block a user