might ditch the esp32, this is the best ive gotten with it.

.gitignore

@@ -3,7 +3,9 @@ create_2_Open_Interface_Spec.txt
 
 
 
-
+logs
-.node_modules
+node_modules/
 .pio
-.vscode
+.vscode/
+include/config.h
+server/robots.json

README.md

@@ -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
@@ -12,3 +14,51 @@ on each roomba:
   - microphone
   - speaker
 - MAYBE a master relay which can be turned off programatically to save the roomba from discharging. based on battery voltage plus urgent battery #?
+
+## 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)

include/config.example.h

@@ -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"

include/protocol.h

@@ -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

notes.md

@@ -1,26 +0,0 @@
-# notes and ideas
-
-## frontend
-
-each frontend module will have a JSON state update over socket.io
-each module will export a function to update it's state
-
-use these functions to update all modules on connect, from a UI init state event.
-this event will be sent all of the module data at once, in JSON
-
-split each JSON element to the UI element functions
-
-one single system for loading and saving user settings
-
-
-## esp-server comms
-
-the ESP will report to the server when it connects:
-
-rover's name
-enable / disable for each motor
-camera IP address
-battery info (for different battery behaviors):
-- full number
-- warn number
-- urgent number

outline.md

@@ -0,0 +1,67 @@
+## esp32 firmware
+- hooked up to the roomba's UART on pins 16 and 17
+- pin 5 is connected to the roomba's BRC pin
+  - pulse the BRC pin low for 1 second every minute to keep the roomba awake
+- connect to wifi
+- connect to the server
+- get a full frame of sensor group 100 from the roomba every 500ms
+  - send it to the server over the sensor UDP stream
+- listen to the server's control UDP stream (per roomba) and do the following accordingly:
+  - set wheel speeds
+  - seek dock
+  - enable OI
+  - safe mode
+  - full mode
+  - play song
+  - load song
+
+## esp32 -> server communication
+- one UDP stream to the esp32 for controlling the roomba
+  - might look like this:
+    - left wheel speed
+    - right wheel speed
+    - OI mode
+    - seek dock?
+  - blasts out at a constant rate from the server for each roomba
+  - the esp32 will listen, and follow the latest command that it sees
+- one UDP stream from the esp32 to the server for sending sensor data frames and other telemetry
+  - one full frame of sensor data per datagram
+  - send raw sensor data, the server will decode it
+  - add other telemetry from the esp32, like signal strength, etc. 
+  - maybe use this stream as a sign that the esp32 is still running healthily?
+
+## nodejs server
+- KISS
+- decode the sensor data from each roomba
+- can support multiple roombas connected from the ground up
+- keep it simple, worry about getting the esp32 firmware right.
+  - but the server DOES have to exist for testing
+- IS the web server, hosts an entire static folder for the web UI
+
+## server -> web UI communication
+- socket.io
+- don't do anything fancy with the socket.io setup
+  - it works fine out of the box, we will optimize it later
+
+## the web UI
+- KISS
+- plain old html. no styling even. just bare minimum for testing
+- what it needs to do:
+  - allow user to select the roomba from a list
+  - make the selected roomba drive with WASD
+  - have buttons to set the OI mode, and tell the roomba to dock
+  - show a plain list of the sensor data from the selected roomba
+
+### general javascript programming guidelines (applies to the web UI too)
+- everything ES6
+  - one entrypoint file in the web UI
+- everything modular
+- everything easy to read, understand, and work on
+- comment where you think is best to describe whats going on
+
+## closing notes
+- keep the user input path (web UI -> server -> roomba) as light and responsive as possible. responsiveness is key for this.
+- responsiveness is the name of the game. The future of this program is teleoperation over the internet, with a camera on each roomba. keyboard inputs from the user must be near instant.
+- on the esp32 firmware side of things, sensor data is second priority to having a responsive control system
+  - but sensor data DOES have to exist.
+- the future of this project will involve assigning one roomba to a user, make the server able to do that from the ground up.

platformio.ini

@@ -1,9 +1,5 @@
 [env:esp32s3]
 platform = espressif32
 board = esp32-s3-devkitc-1
-framework = arduino
 monitor_speed = 115200
-
+framework = arduino
-; Enable verbose FreeRTOS symbols in Arduino builds (optional but handy for demos)
-build_flags =
-    -DCORE_DEBUG_LEVEL=3

server/package.json

@@ -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"
+  }
+}

server/public/app.js

@@ -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();
+  }
+});

server/public/index.html

@@ -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>

server/robots.example.json

@@ -0,0 +1,7 @@
+[
+  {
+    "id": "roomba-alpha",
+    "controlPort": 50010,
+    "maxWheelSpeed": 350
+  }
+]

server/src/checksum.js

@@ -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;
+}

server/src/constants.js

@@ -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,
+};

server/src/robotRegistry.js

@@ -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),
+    };
+  });
+}

server/src/server.js

@@ -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}`);
+});

server/src/telemetryDecoder.js

@@ -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),
+    },
+  };
+}

server/src/udpPackets.js

@@ -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;
+}

src/main.cpp

@@ -1,131 +1,369 @@
 #include <Arduino.h>
+#include <WiFi.h>
+#include <WiFiUdp.h>
+#include <esp_wifi.h>
+#include <cstring>
 
-// --- Hardware mapping -------------------------------------------------------
+#include "config.h"
-// Adjust these pins to match how your level shifter connects the ESP32 to the Roomba.
+#include "protocol.h"
-constexpr int ROBO_UART_RX = 16;        // ESP32 pin receiving Roomba TX (ROI pin 4).
-constexpr int ROBO_UART_TX = 17;        // ESP32 pin driving Roomba RX (ROI pin 3).
-constexpr gpio_num_t ROBO_BRC_PIN = GPIO_NUM_5;  // GPIO pulsing the BRC line (ROI pin 5).
 
-// FreeRTOS cadences (1 Hz pulse, 150 ms low to ensure Roomba notices).
+using namespace mrr;
-constexpr TickType_t BRC_PERIOD = pdMS_TO_TICKS(1000);
-constexpr TickType_t BRC_LOW_PULSE = pdMS_TO_TICKS(150);
 
-// Simple helper to send an Open Interface command over UART.
+namespace {
-void sendRoombaCommand(std::initializer_list<uint8_t> bytes) {
+
-  Serial1.write(bytes.begin(), bytes.size());
+constexpr gpio_num_t kRoombaRxPin = GPIO_NUM_16;
-  Serial1.flush();  // Ensure command clears the UART FIFO before proceeding.
+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 brcTask(void * /*parameter*/) {
+void connectWifi() {
-  TickType_t nextWake = xTaskGetTickCount();
+  WiFi.mode(WIFI_STA);
-  uint32_t pulseCount = 0;
+  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
-
+  Serial.printf("[wifi] connecting to %s\\n", WIFI_SSID);
-  while (true) {
+  uint32_t start = millis();
-    // Idle high, pulse low to reset the five-minute sleep timer.
+  while (WiFi.status() != WL_CONNECTED) {
-    Serial.printf("[BRC] Pulse #%lu: pulling low\n", static_cast<unsigned long>(pulseCount));
+    delay(250);
-    gpio_set_level(ROBO_BRC_PIN, 0);
+    Serial.print(".");
-    vTaskDelay(BRC_LOW_PULSE);
+    if (millis() - start > 20000) {
-    gpio_set_level(ROBO_BRC_PIN, 1);
+      Serial.println("\\n[wifi] retrying...");
-    Serial.printf("[BRC] Pulse #%lu: released high\n", static_cast<unsigned long>(pulseCount));
+      WiFi.disconnect();
-    pulseCount++;
+      WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
-
+      start = millis();
-    vTaskDelayUntil(&nextWake, BRC_PERIOD);
     }
+  }
+  disableWifiPowerSave();
+  Serial.printf("\\n[wifi] connected, ip=%s\\n", WiFi.localIP().toString().c_str());
 }
 
-void roombaTask(void * /*parameter*/) {
+IPAddress resolveServerIp() {
-  // Give the Roomba a moment after wake-up before issuing commands.
+  IPAddress ip;
-  vTaskDelay(pdMS_TO_TICKS(500));
+  if (!ip.fromString(CONTROL_SERVER_IP)) {
-
+    Serial.printf("[wifi] invalid CONTROL_SERVER_IP: %s\\n", CONTROL_SERVER_IP);
-  Serial.println("Sending Start (128)...");
+  }
-  sendRoombaCommand({128});  // Start OI -> Passive mode.
+  return ip;
-  vTaskDelay(pdMS_TO_TICKS(100));
-
-  Serial.println("Switching to Safe mode (131)...");
-  sendRoombaCommand({131});  // Safe gives actuator control with failsafes.
-  vTaskDelay(pdMS_TO_TICKS(100));
-
-  Serial.println("Loading a short test song into slot 0 (Song, opcode 140)...");
-  // Song format: [140][song #][length][note][duration]...
-  // Duration units are 1/64ths of a second; 32 ≈ 0.5 s.
-  sendRoombaCommand({140, 0, 1, 69, 32});  // A4 for ~0.5 s.
-  vTaskDelay(pdMS_TO_TICKS(100));
-
-  Serial.println("Playing song 0 (Play, opcode 141)...");
-  sendRoombaCommand({141, 0});
-  vTaskDelay(pdMS_TO_TICKS(1500));  // Allow song to finish.
-
-  Serial.println("Initial song played. Roomba ready for bumper test.");
-
-  // Nothing else to do on this task; park it.
-  vTaskDelete(nullptr);
 }
 
-void sensorTask(void * /*parameter*/) {
+ControlState snapshotControl() {
-  constexpr TickType_t pollPeriod = pdMS_TO_TICKS(100);  // ~10 Hz polling.
+  portENTER_CRITICAL(&gControlMux);
-  TickType_t nextWake = xTaskGetTickCount();
+  ControlState copy = gLatestControl;
-  bool bumperActive = false;
+  portEXIT_CRITICAL(&gControlMux);
+  return copy;
+}
 
-  // Allow initialization commands to finish before polling.
+void updateControl(const ControlPacket& pkt) {
-  vTaskDelay(pdMS_TO_TICKS(1000));
+  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);
+}
 
-  while (true) {
+void zeroDrive() {
-    // Request packet 7 (Bumps & Wheel Drops).
+  gRoomba.driveDirect(0, 0);
-    sendRoombaCommand({142, 7});
+}
 
-    uint8_t packet = 0;
+void handleControlApplication(const ControlState& state) {
-    const size_t received = Serial1.readBytes(&packet, 1);
+  const uint32_t now = millis();
-    if (received == 1) {
+  int16_t left = state.left_mmps;
-      Serial.printf("[Sensor] Packet 7 raw byte: 0x%02X\n", packet);
+  int16_t right = state.right_mmps;
-      const bool bumpRight = packet & 0b00000001;
+  const bool stale = (now - state.last_rx_ms) > kControlTimeoutMs;
-      const bool bumpLeft = packet & 0b00000010;
+  if (stale) {
-      const bool wheelDropRight = packet & 0b00000100;
+    left = 0;
-      const bool wheelDropLeft = packet & 0b00001000;
+    right = 0;
-      const bool frontBump = bumpLeft || bumpRight;
-
-      if (wheelDropLeft || wheelDropRight) {
-        Serial.printf("[Sensor] Wheel drop detected (L:%d R:%d)\n",
-                      wheelDropLeft, wheelDropRight);
   }
-      if (frontBump && !bumperActive) {
+
-        Serial.println("Front bumper hit! Playing song 0.");
+  if (state.seq != gLastAppliedSeq) {
-        sendRoombaCommand({141, 0});
+    if (state.requested_mode != OiModeRequest::kNoChange) {
+      gRoomba.setMode(state.requested_mode);
+    } else if (state.actions & kActionEnableOi) {
+      gRoomba.ensureStarted();
     }
-      if (!frontBump && bumperActive) {
+
-        Serial.println("Front bumper released.");
+    if (state.actions & kActionSeekDock) {
+      gRoomba.seekDock();
     }
-      bumperActive = frontBump;
+    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 {
-      Serial.println("[Sensor] Timed out waiting for packet 7.");
+        ++gDroppedControlPackets;
+      }
     }
 
-    vTaskDelayUntil(&nextWake, pollPeriod);
+    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);
-  while (!Serial && millis() < 2000) {
+  delay(50);
-    delay(10);
+  Serial.println("[boot] MultiRoombaRover firmware starting");
-  }
-  Serial.println("\nRoomba hardware smoke test starting...");
 
-  // Configure the BRC pin; keep it high (inactive) until the pulse task starts.
+  gRoomba.begin();
-  pinMode(static_cast<uint8_t>(ROBO_BRC_PIN), OUTPUT);
+  connectWifi();
-  gpio_set_level(ROBO_BRC_PIN, 1);
+  gServerIp = resolveServerIp();
 
-  // Initialize UART1 for the Roomba Open Interface at its default baud (115200 8N1).
+  gControlSocket.begin(ESP32_CONTROL_PORT);
-  Serial1.begin(115200, SERIAL_8N1, ROBO_UART_RX, ROBO_UART_TX);
+  gTelemetrySocket.begin(ESP32_TELEMETRY_PORT);
-  Serial1.setTimeout(150);  // Enough headroom for sensor replies.
-  Serial.println("UART1 configured for Roomba at 115200 8N1.");
 
-  // Launch the tasks that drive the Roomba and keep it awake.
+  xTaskCreatePinnedToCore(controlTask, "control", 4096, nullptr, 3, &gControlTaskHandle, APP_CPU_NUM);
-  xTaskCreatePinnedToCore(brcTask, "BrcPulse", 2048, nullptr, 1, nullptr, APP_CPU_NUM);
+  xTaskCreatePinnedToCore(telemetryTask, "telemetry", 4096, nullptr, 2, &gTelemetryTaskHandle, PRO_CPU_NUM);
-  xTaskCreatePinnedToCore(roombaTask, "RoombaInit", 4096, nullptr, 1, nullptr, APP_CPU_NUM);
+  xTaskCreatePinnedToCore(brcTask, "brc", 2048, nullptr, 1, &gBrcTaskHandle, APP_CPU_NUM);
-  xTaskCreatePinnedToCore(sensorTask, "SensorPoll", 4096, nullptr, 1, nullptr, APP_CPU_NUM);
 }
 
 void loop() {
-  // Nothing needed here; FreeRTOS tasks run everything.
+  // Nothing to do. All work happens inside FreeRTOS tasks.
   vTaskDelay(pdMS_TO_TICKS(1000));
 }