mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-15 17:12:59 -04:00
testings are going goods
This commit is contained in:
legop3
Vendored
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
+146
-54
@@ -1,6 +1,6 @@
|
||||
# Boot-discovered rover peripherals
|
||||
|
||||
This document defines the planned system for attaching self-describing ESP32 peripherals to a rover over USB. A peripheral advertises a small ordered set of controls, the web UI renders those controls automatically, and the current driver can use them without adding device-specific configuration to the rover or server.
|
||||
This document defines the system for attaching self-describing ESP32 peripherals to a rover over USB. A peripheral advertises a small ordered set of controls, the web UI renders those controls automatically, and the current driver can use them without adding device-specific configuration to the rover or server.
|
||||
|
||||
The design deliberately stays small:
|
||||
|
||||
@@ -11,10 +11,12 @@ The design deliberately stays small:
|
||||
- Controls appear in one vertical column in the order registered by the ESP32 program.
|
||||
- Anyone who can currently drive the rover can use its peripheral controls.
|
||||
- Peripherals are discovered once when `roverd` starts; changing one requires restarting the rover.
|
||||
- ESP32 firmware is built and uploaded with PlatformIO.
|
||||
- The same firmware supports CH340/CP210x USB-to-UART boards and native USB CDC boards.
|
||||
- There is no peripheral configuration in the rover configuration file.
|
||||
- There is no separate rover-peripheral protocol version.
|
||||
|
||||
This is a design document. The names of proposed Go, JavaScript, and Arduino APIs describe the intended implementation and do not refer to code that already exists.
|
||||
This document is both the design contract and implementation guide. The PlatformIO firmware library, reference sketch, focused Go Firmata client, hardware probe, boot-time daemon discovery, fixed inventory, generic output dispatch, built-in hardware backend selection, and rover WebSocket message shapes now exist. Server forwarding and HUD rendering remain later implementation stages.
|
||||
|
||||
## System boundary
|
||||
|
||||
@@ -126,6 +128,8 @@ source byte = encoded byte 1 | (encoded byte 2 << 7)
|
||||
|
||||
Peripheral authors never perform this encoding themselves. It belongs in the ESP32 `RoverPeripheralFirmata` library and the Go Firmata client used by `roverd`.
|
||||
|
||||
ConfigurableFirmata's ESP32 parser accepts 252 bytes inside one incoming SysEx frame, including the feature and operation bytes. `CONTROL` values are not chunked in this deliberately simple design. The Go client checks the fully encoded message before writing it and returns an error if a particular control value cannot fit, rather than sending a frame the ESP32 would discard. Normal numeric, boolean, and short text controls fit comfortably; a text control's configured length should reflect this transport constraint.
|
||||
|
||||
## Peripheral description
|
||||
|
||||
The ESP32 library builds this description from the controls registered during `setup()`. The order of the `controls` array is the registration order and is also the UI order.
|
||||
@@ -324,44 +328,35 @@ An ESP32 that supplies all three roles describes:
|
||||
|
||||
```json
|
||||
{
|
||||
"name": "Laptop rover GPIO",
|
||||
"name": "Rover GPIO",
|
||||
"roverControls": {
|
||||
"cameraServo": {
|
||||
"output": {
|
||||
"type": "servo",
|
||||
"pin": 14
|
||||
},
|
||||
"minAngle": -15,
|
||||
"maxAngle": 30,
|
||||
"homeAngle": 0,
|
||||
"pin": 14,
|
||||
"minimumAngleDegrees": -15,
|
||||
"maximumAngleDegrees": 30,
|
||||
"homeAngleDegrees": 0,
|
||||
"nudgeDegrees": 2,
|
||||
"minPulseUs": 900,
|
||||
"maxPulseUs": 2100,
|
||||
"minimumPulseMicroseconds": 900,
|
||||
"maximumPulseMicroseconds": 2100,
|
||||
"allowRawPulse": false,
|
||||
"invert": false
|
||||
"inverted": false
|
||||
},
|
||||
"headlight": {
|
||||
"output": {
|
||||
"type": "digital",
|
||||
"pin": 18
|
||||
},
|
||||
"initialOn": false,
|
||||
"activeLow": false
|
||||
"pin": 18,
|
||||
"activeLow": false,
|
||||
"initiallyOn": false
|
||||
},
|
||||
"laser": {
|
||||
"output": {
|
||||
"type": "digital",
|
||||
"pin": 19
|
||||
},
|
||||
"initialOn": false,
|
||||
"activeLow": false
|
||||
"pin": 16,
|
||||
"activeLow": false,
|
||||
"initiallyOn": false
|
||||
}
|
||||
},
|
||||
"controls": []
|
||||
}
|
||||
```
|
||||
|
||||
The ESP32 description owns the calibration for hardware attached to that ESP32. Laptop rover YAML does not repeat the ESP32 pin numbers or servo calibration.
|
||||
The ESP32 description owns the calibration for hardware attached to that ESP32. Rover YAML does not repeat the ESP32 pin numbers or servo calibration.
|
||||
|
||||
### Optional backend-selection rule
|
||||
|
||||
@@ -373,7 +368,7 @@ The ESP32 description owns the calibration for hardware attached to that ESP32.
|
||||
|
||||
Native configuration deliberately wins. A Pi rover can attach an ESP32 for unrelated generic controls without unexpectedly moving its existing camera servo, headlight, or laser to the ESP32. To deliberately use the ESP32 for one of those features, disable only that native feature in rover YAML.
|
||||
|
||||
The normal laptop configuration keeps the unavailable native GPIO features disabled:
|
||||
Any rover configuration that should use the ESP32 for these roles keeps the corresponding native GPIO features disabled:
|
||||
|
||||
```yaml
|
||||
cameraServo:
|
||||
@@ -386,7 +381,7 @@ laser:
|
||||
enabled: false
|
||||
```
|
||||
|
||||
An attached ESP32 can then fill any or all of those roles automatically at the next `roverd` start. No USB path or backend name is added to YAML.
|
||||
An attached ESP32 can then fill any or all of those roles automatically at the next `roverd` start. This works identically on Raspberry Pi and laptop rover hosts; no USB path or backend name is added to YAML.
|
||||
|
||||
Conflict behavior is fixed and simple:
|
||||
|
||||
@@ -586,6 +581,7 @@ A complete sketch for one servo slider, one light-brightness slider, and one cus
|
||||
```cpp
|
||||
#include <Arduino.h>
|
||||
#include <ConfigurableFirmata.h>
|
||||
#include <FirmataExt.h>
|
||||
#include <RoverPeripheralFirmata.h>
|
||||
|
||||
/*
|
||||
@@ -593,6 +589,7 @@ A complete sketch for one servo slider, one light-brightness slider, and one cus
|
||||
* browser preserves that order when it renders the peripheral's column.
|
||||
*/
|
||||
RoverPeripheralFirmata peripheral("Example peripheral");
|
||||
FirmataExt firmataExtension;
|
||||
|
||||
/*
|
||||
* This is ordinary application code rather than Firmata plumbing. A real
|
||||
@@ -603,7 +600,8 @@ void runSpecialAction() {
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Firmata.begin(115200);
|
||||
Serial.begin(115200);
|
||||
Firmata.begin(Serial);
|
||||
|
||||
/*
|
||||
* roverd handles this control with standard Firmata SERVO commands. The
|
||||
@@ -655,7 +653,7 @@ void setup() {
|
||||
);
|
||||
|
||||
// Register the extension with Firmata and finalize the control description.
|
||||
peripheral.begin();
|
||||
peripheral.begin(firmataExtension);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
@@ -692,17 +690,20 @@ addRoverHeadlight(const RoverDigitalOutputConfig&)
|
||||
addRoverLaser(const RoverDigitalOutputConfig&)
|
||||
```
|
||||
|
||||
A laptop GPIO peripheral can combine built-in replacements and additional controls:
|
||||
A rover GPIO peripheral can combine built-in replacements and additional controls:
|
||||
|
||||
```cpp
|
||||
#include <Arduino.h>
|
||||
#include <ConfigurableFirmata.h>
|
||||
#include <FirmataExt.h>
|
||||
#include <RoverPeripheralFirmata.h>
|
||||
|
||||
RoverPeripheralFirmata peripheral("Laptop rover GPIO");
|
||||
RoverPeripheralFirmata peripheral("Rover GPIO");
|
||||
FirmataExt firmataExtension;
|
||||
|
||||
void setup() {
|
||||
Firmata.begin(115200);
|
||||
Serial.begin(115200);
|
||||
Firmata.begin(Serial);
|
||||
|
||||
/*
|
||||
* These declarations satisfy existing rover roles. They retain the normal
|
||||
@@ -729,7 +730,8 @@ void setup() {
|
||||
peripheral.addRoverHeadlight(headlight);
|
||||
|
||||
RoverDigitalOutputConfig laser;
|
||||
laser.pin = 19;
|
||||
// GPIO 19 and 20 are reserved for USB on native-USB ESP32-S3 boards.
|
||||
laser.pin = 16;
|
||||
laser.polarity = OutputPolarity::ActiveHigh;
|
||||
laser.initiallyOn = false;
|
||||
|
||||
@@ -752,7 +754,7 @@ void setup() {
|
||||
}
|
||||
);
|
||||
|
||||
peripheral.begin();
|
||||
peripheral.begin(firmataExtension);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
@@ -763,23 +765,95 @@ void loop() {
|
||||
}
|
||||
```
|
||||
|
||||
## PlatformIO firmware layout
|
||||
|
||||
PlatformIO is the only supported firmware workflow. The repository contains one shared library and one complete rover GPIO peripheral project:
|
||||
|
||||
```text
|
||||
esp32/
|
||||
├── libraries/
|
||||
│ └── RoverPeripheralFirmata/
|
||||
│ ├── library.json
|
||||
│ └── src/
|
||||
└── rover-gpio-peripheral/
|
||||
├── platformio.ini
|
||||
└── src/main.cpp
|
||||
```
|
||||
|
||||
The local library owns control registration, description generation, rover-peripheral SysEx handling, callback dispatch, and the standard digital, PWM, and servo output subset. The sketch only declares hardware and application behavior.
|
||||
|
||||
`platformio.ini` contains two environments:
|
||||
|
||||
| Environment | Intended hardware | Normal Linux device |
|
||||
| --- | --- | --- |
|
||||
| `esp32dev` | ESP32-WROOM-32/DevKitC boards using CH340 or CP210x USB-to-UART | `/dev/ttyUSB*` |
|
||||
| `esp32-s3-devkitc-1` | ESP32-S3 boards using native USB CDC | `/dev/ttyACM*` |
|
||||
|
||||
Both environments compile the same `main.cpp`. The S3 environment only adds the Arduino USB CDC build flags needed to make its native USB serial stream active at boot. The sketch passes Arduino's `Serial` object to `Firmata.begin(Stream&)`, so neither the helper library nor the Firmata messages depend on which USB transport produced the byte stream.
|
||||
|
||||
Typical commands are:
|
||||
|
||||
```bash
|
||||
cd esp32/rover-gpio-peripheral
|
||||
|
||||
# The TG34/CH340 DevKitC-style ESP32 used for initial testing.
|
||||
pio run -e esp32dev
|
||||
pio run -e esp32dev -t upload --upload-port /dev/ttyUSB0
|
||||
pio device monitor --port /dev/ttyUSB0 --baud 115200
|
||||
|
||||
# A native-USB ESP32-S3 DevKitC.
|
||||
pio run -e esp32-s3-devkitc-1
|
||||
pio run -e esp32-s3-devkitc-1 -t upload --upload-port /dev/ttyACM0
|
||||
pio device monitor --port /dev/ttyACM0 --baud 115200
|
||||
```
|
||||
|
||||
Do not keep PlatformIO's serial monitor open while `roverd` or the probe is using the peripheral. A serial device can have only one process actively consuming the Firmata stream.
|
||||
|
||||
After uploading, use the Go probe to perform the real handshake and print the self-description:
|
||||
|
||||
```bash
|
||||
cd pi/roverd
|
||||
go run ./cmd/peripheral-probe -port /dev/ttyUSB0
|
||||
|
||||
# Exercise the standard servo slider.
|
||||
go run ./cmd/peripheral-probe -port /dev/ttyUSB0 -control servoPosition -value 90
|
||||
|
||||
# Exercise the custom momentary callback. Run once for press and once for release.
|
||||
go run ./cmd/peripheral-probe -port /dev/ttyUSB0 -control specialAction -value true
|
||||
go run ./cmd/peripheral-probe -port /dev/ttyUSB0 -control specialAction -value false
|
||||
```
|
||||
|
||||
Use `/dev/ttyACM0` instead for a native-USB board. The probe waits two seconds after opening because either style of development board may reset when its serial connection opens. It then performs the standard Firmata firmware and capability queries before sending `DESCRIBE`. `-control` is intentionally a diagnostic option only; production control will enter through the server and `roverd` command path.
|
||||
|
||||
ConfigurableFirmata's stock example disables its servo feature on ESP32. `RoverPeripheralFirmata` therefore uses ConfigurableFirmata for standard framing, parsing, capability dispatch, and firmware queries, but supplies the ESP32 servo implementation with `ESP32Servo`. Servo writes still use the standard Firmata `SERVO_CONFIG`, `SET_PIN_MODE`, and `EXTENDED_ANALOG` messages; this is an implementation substitution inside the firmware, not a custom servo protocol.
|
||||
|
||||
The project pins ConfigurableFirmata `3.2.0` because PlatformIO's stable Espressif32 platform currently ships Arduino-ESP32 2.x. ConfigurableFirmata `3.4.0` compiles its bundled PWM source with Arduino-ESP32 3.x LEDC function names even when the sketch does not instantiate that feature. The pinned release uses the matching 2.x LEDC API and compiles for both configured boards. This pin is a build compatibility choice and does not change the Firmata messages used by the rover.
|
||||
|
||||
## Connection lifecycle
|
||||
|
||||
### Startup discovery
|
||||
|
||||
Peripheral discovery happens exactly once per `roverd` process. Before constructing the built-in hardware controllers or connecting to the server, `roverd`:
|
||||
Peripheral discovery happens exactly once per `roverd` process. The currently implemented startup path runs before `roverd` constructs its existing built-in hardware controllers or connects to the server:
|
||||
|
||||
1. Enumerates the serial devices present on Linux.
|
||||
2. Opens each candidate device found by the startup scan.
|
||||
3. Starts one Firmata client per opened connection.
|
||||
4. Performs the normal Firmata firmware and capability queries.
|
||||
5. Sends the rover-peripheral `DESCRIBE` operation.
|
||||
6. Decodes and parses each `DESCRIPTION` response.
|
||||
7. Assigns process-local IDs such as `firmata-0` and `firmata-1`.
|
||||
8. Resolves the optional `cameraServo`, `headlight`, and `laser` roles.
|
||||
9. Builds the fixed generic peripheral list.
|
||||
10. Constructs `WSClient` with the resolved built-in controllers and peripherals.
|
||||
11. Connects to the server and sends the normal rover hello.
|
||||
3. Waits for a possible board reset and drains stale serial bytes to a quiet read boundary.
|
||||
4. Starts one Firmata client per opened connection.
|
||||
5. Performs the normal Firmata firmware and capability queries.
|
||||
6. Sends the rover-peripheral `DESCRIBE` operation.
|
||||
7. Decodes and validates each `DESCRIPTION` response.
|
||||
8. Validates every advertised standard-output pin against Firmata capabilities.
|
||||
9. Configures generic digital, PWM, and servo pin modes once.
|
||||
10. Assigns process-local IDs such as `firmata-0` and `firmata-1` in discovery order.
|
||||
11. Resolves `cameraServo`, `headlight`, and `laser` against the native configuration.
|
||||
12. Rejects duplicate ESP32 providers only when the corresponding native role is disabled and Firmata selection would otherwise be ambiguous.
|
||||
13. Constructs the selected native or Firmata controllers and fixed generic inventory.
|
||||
14. Constructs `WSClient` with those controllers and that inventory.
|
||||
15. Connects to the server and includes the effective built-in configuration and renderable inventory in the normal rover hello.
|
||||
|
||||
The Linux scan checks stable `/dev/serial/by-id/*` names first, then `/dev/ttyUSB*` and `/dev/ttyACM*`. It canonicalizes symlinks so one device is not opened twice under its stable name and kernel name, and it excludes the configured Roomba Open Interface serial device. Each opened candidate receives the same reset wait used by the probe, followed by a read-until-quiet drain so an old partial SysEx cannot contaminate the new handshake.
|
||||
|
||||
Firmware and capability queries remain standard Firmata. `RoverPeripheralFirmata::begin()` registers the standard Firmata firmware name `RoverPeripheralFirmata`, so individual sketches do not repeat that discovery detail. A Firmata device with another firmware name is closed and ignored. Once a device identifies itself as rover-peripheral firmware, a malformed capability or description response is a startup error rather than a silently missing configured accessory.
|
||||
|
||||
Linux paths such as `/dev/ttyACM0` remain private `roverd` connection details. The browser and server use only the process-local peripheral ID from the hello.
|
||||
|
||||
@@ -802,8 +876,8 @@ If an ESP32 is unplugged or its serial connection fails while `roverd` is runnin
|
||||
|
||||
1. Its Firmata client marks the connection unavailable.
|
||||
2. Commands routed to that peripheral or one of its built-in roles return an error.
|
||||
3. `roverd` logs that the peripheral requires reconnection followed by a restart.
|
||||
4. The advertised roster and visible controls do not change during that process lifetime.
|
||||
3. The failed command is logged by the existing rover WebSocket command path.
|
||||
4. The advertised inventory does not change during that process lifetime.
|
||||
|
||||
The disconnected device is never replaced automatically by another serial device. This ensures that a command cannot be redirected merely because Linux reused a `/dev/ttyACM*` path.
|
||||
|
||||
@@ -1034,7 +1108,15 @@ The generic renderer maps:
|
||||
- `number` to a labeled numeric input.
|
||||
- `text` to a labeled single-line text input.
|
||||
|
||||
The same generic component is reused by desktop and mobile layouts. Layout wrappers decide where the column appears; device-specific components are not created for individual peripherals.
|
||||
Generic peripheral controls are rover controls, so they follow the new driver's HUD language. They do not belong in either sidebar: the sidebars contain chat, queues, room controls, settings, and other controls that are not direct rover actuation.
|
||||
|
||||
The standardized replacements do not create any new UI. `cameraServo`, `headlight`, and `laser` continue to use their current camera-tilt, headlight, and laser HUD controls. Only entries in the generic `controls` arrays appear in a new surface named `Accessories`.
|
||||
|
||||
On desktop, `Accessories` is a collapsible HUD drawer connected to the bottom-left rover-control pod. This keeps additional actuation beside the existing horn, headlight, and laser controls without permanently covering the video. The drawer is absent when the assigned rover advertises no generic controls.
|
||||
|
||||
On mobile, the HUD launcher opens an unscaled, vertically scrollable sheet over the video stage. Generic controls must not be placed in the fixed `AuxColumn`: an arbitrary device-defined list cannot fit that column's intentionally fixed set of large driving controls. The mobile sheet closes without changing control values and disappears when there are no generic controls.
|
||||
|
||||
Desktop and mobile reuse one generic renderer inside their different HUD containers. Device-specific React components are not created for individual peripherals. The renderer sends actions through `ControlSystemProvider`, `ControlContext`, and the existing command pipeline so assignment gating, input cancellation, and command behavior remain consistent with other rover HUD controls.
|
||||
|
||||
Control values are local UI values in the first implementation. Slider and toggle changes update the displayed value immediately and are then sent to the server. Restarting `roverd` recreates controls from the new hello rather than persisting peripheral values in `roverSettings`.
|
||||
|
||||
@@ -1054,7 +1136,7 @@ Implementation should remain concentrated in a few clear areas.
|
||||
|
||||
### ESP32 library
|
||||
|
||||
Create a small Arduino-compatible `RoverPeripheralFirmata` library containing:
|
||||
The Arduino-compatible `RoverPeripheralFirmata` library now contains:
|
||||
|
||||
- Ordered control registration.
|
||||
- Standardized `cameraServo`, `headlight`, and `laser` role registration.
|
||||
@@ -1067,11 +1149,13 @@ Create a small Arduino-compatible `RoverPeripheralFirmata` library containing:
|
||||
|
||||
Example ESP32 sketches should use this library rather than hand-writing SysEx parsing.
|
||||
|
||||
The first implementation lives in `esp32/libraries/RoverPeripheralFirmata`, with the complete `esp32/rover-gpio-peripheral` PlatformIO project serving as both the reference firmware and an example usable by either rover host type.
|
||||
|
||||
### `pi/roverd`
|
||||
|
||||
Add a peripheral manager responsible for:
|
||||
`PeripheralManager` is responsible for:
|
||||
|
||||
- One-time Linux USB serial discovery during startup.
|
||||
- One-time Linux USB serial discovery during startup on either rover host type.
|
||||
- One Firmata client per connected peripheral.
|
||||
- Firmata handshake and capability queries.
|
||||
- Rover-peripheral description queries.
|
||||
@@ -1081,9 +1165,15 @@ Add a peripheral manager responsible for:
|
||||
- Custom `CONTROL` dispatch.
|
||||
- Rejecting commands for disconnected peripheral IDs.
|
||||
|
||||
The manager validates every advertised standard-output pin against the device's Firmata capability response and configures each generic pin mode once during startup. Runtime servo and PWM changes therefore send only value writes; they do not repeatedly detach and reconfigure the hardware output. Generic digital controls start logically off, including the corresponding high electrical level for active-low declarations.
|
||||
|
||||
Only renderable control metadata leaves `roverd`. Firmata pin numbers, output mappings, Linux paths, live clients, capabilities, and built-in-role declarations stay in the manager's private fixed inventory. The rover hello contains process-local peripheral IDs, names, and ordered generic controls.
|
||||
|
||||
The manager should remain independent of the existing Roomba Open Interface serial adapter. A peripheral serial connection is not the Roomba base serial connection and must not be routed through `SerialAdapter`.
|
||||
|
||||
Refactor camera servo and GPIO toggles so `WSClient` depends on the shared controller interfaces rather than platform-selected concrete types. Preserve the existing logical servo movement and toggle-state behavior above the Pi and Firmata physical writers.
|
||||
The transport foundation is a focused Firmata parser/client in `pi/roverd/firmata.go`. It operates on `io.ReadWriteCloser`, which keeps byte-stream behavior testable without hardware and lets discovery pass either `/dev/ttyUSB*` or `/dev/ttyACM*` ports into the same client. `pi/roverd/cmd/peripheral-probe` remains the direct hardware diagnostic entry point, while `PeripheralManager` now connects the same client to automatic daemon startup discovery.
|
||||
|
||||
`WSClient` now depends on shared camera-servo and toggle controller interfaces rather than platform-selected concrete types. The startup resolver uses the same native-first rule in the ARM Pi and amd64 laptop binaries. Firmata camera movement retains the established limits, home position, nudging, inversion, pulse calibration, raw-pulse policy, and movement-rate behavior; Firmata toggles retain logical state and polarity conversion.
|
||||
|
||||
### Server
|
||||
|
||||
@@ -1105,7 +1195,8 @@ Add one generic peripheral control renderer that:
|
||||
- Renders only the four agreed control types.
|
||||
- Sends every interaction through the same `peripheral:set` event.
|
||||
- Supports momentary press and release for pointer, touch, and keyboard activation.
|
||||
- Fits into the existing desktop and mobile driver control layouts.
|
||||
- Mounts in the desktop Accessories HUD drawer and mobile Accessories HUD sheet.
|
||||
- Uses the shared control context and command pipeline rather than emitting directly from layout code.
|
||||
- Disappears completely when the assigned rover has no peripherals.
|
||||
|
||||
## Implementation sequence
|
||||
@@ -1160,7 +1251,7 @@ The completed system should be verified with a real ESP32 and rover Linux comput
|
||||
|
||||
### Built-in GPIO replacement
|
||||
|
||||
- Start a laptop rover with native camera servo, headlight, and laser disabled and an ESP32 declaring all three roles.
|
||||
- Start either rover host type with native camera servo, headlight, and laser disabled and an ESP32 declaring all three roles.
|
||||
- Confirm the normal camera tilt, headlight, and laser UI appears without generic duplicates.
|
||||
- Confirm camera angle limits, home position, nudge amount, inversion, pulse calibration, and rate limiting match the declared ESP32 configuration.
|
||||
- Confirm headlight and laser toggle state events remain identical to the native Pi path.
|
||||
@@ -1168,6 +1259,7 @@ The completed system should be verified with a real ESP32 and rover Linux comput
|
||||
- Enable a native role and declare the same ESP32 role; confirm native wins and the ignored role is logged.
|
||||
- Disable native and declare the same role from two ESP32s; confirm startup fails with a clear duplicate-role error.
|
||||
- Confirm a Pi rover can use native built-in controls and generic ESP32 controls simultaneously.
|
||||
- Repeat ESP32 role selection on both the ARM Pi binary and amd64 laptop binary and confirm their commands and advertised configurations match.
|
||||
|
||||
### Fixed-device lifecycle
|
||||
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
{
|
||||
"name": "RoverPeripheralFirmata",
|
||||
"version": "0.1.0",
|
||||
"description": "Self-describing Firmata controls for MultiRoombaRover ESP32 peripherals",
|
||||
"frameworks": "arduino",
|
||||
"platforms": "espressif32",
|
||||
"dependencies": {
|
||||
"bblanchon/ArduinoJson": "^7.4.2",
|
||||
"madhephaestus/ESP32Servo": "^3.0.8"
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,511 @@
|
||||
#include "RoverPeripheralFirmata.h"
|
||||
|
||||
namespace {
|
||||
constexpr byte kPeripheralFeature = 0x01;
|
||||
constexpr byte kDescribeOperation = 0x00;
|
||||
constexpr byte kDescriptionOperation = 0x01;
|
||||
constexpr byte kControlOperation = 0x02;
|
||||
|
||||
const char* buttonModeName(ButtonMode mode) {
|
||||
return mode == ButtonMode::Toggle ? "toggle" : "momentary";
|
||||
}
|
||||
|
||||
const char* outputTypeName(uint8_t value) {
|
||||
switch (value) {
|
||||
case 0:
|
||||
return "servo";
|
||||
case 1:
|
||||
return "pwm";
|
||||
case 2:
|
||||
return "digital";
|
||||
default:
|
||||
return "custom";
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
|
||||
RoverPeripheralFirmata* RoverPeripheralFirmata::instance_ = nullptr;
|
||||
|
||||
RoverPeripheralFirmata::RoverPeripheralFirmata(const String& name) : name_(name) {}
|
||||
|
||||
void RoverPeripheralFirmata::validateControlIdentity(const String& id, const String& name) const {
|
||||
if (id.length() == 0 || name.length() == 0) {
|
||||
// Registration errors are programmer errors discovered during setup. A
|
||||
// hard stop is preferable to advertising a partially usable device whose
|
||||
// behavior depends on which malformed control the driver touches first.
|
||||
abort();
|
||||
}
|
||||
for (const ControlRegistration& existing : controls_) {
|
||||
if (existing.id == id) {
|
||||
abort();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::validateRange(const String& id, int minimum, int maximum) const {
|
||||
if (id.length() == 0 || minimum > maximum) {
|
||||
abort();
|
||||
}
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addServoSlider(const SliderControlConfig& config, const FirmataServoOutput& output) {
|
||||
validateControlIdentity(config.id, config.name);
|
||||
validateRange(config.id, config.minimum, config.maximum);
|
||||
ControlRegistration control;
|
||||
control.id = config.id;
|
||||
control.name = config.name;
|
||||
control.type = ControlType::Slider;
|
||||
control.output = OutputType::Servo;
|
||||
control.minimum = config.minimum;
|
||||
control.maximum = config.maximum;
|
||||
control.pin = output.pin;
|
||||
controls_.push_back(control);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addPwmSlider(const SliderControlConfig& config, const FirmataPwmOutput& output) {
|
||||
validateControlIdentity(config.id, config.name);
|
||||
validateRange(config.id, config.minimum, config.maximum);
|
||||
ControlRegistration control;
|
||||
control.id = config.id;
|
||||
control.name = config.name;
|
||||
control.type = ControlType::Slider;
|
||||
control.output = OutputType::Pwm;
|
||||
control.minimum = config.minimum;
|
||||
control.maximum = config.maximum;
|
||||
control.pin = output.pin;
|
||||
controls_.push_back(control);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addDigitalButton(const ButtonControlConfig& config, const FirmataDigitalOutput& output) {
|
||||
validateControlIdentity(config.id, config.name);
|
||||
ControlRegistration control;
|
||||
control.id = config.id;
|
||||
control.name = config.name;
|
||||
control.type = ControlType::Button;
|
||||
control.output = OutputType::Digital;
|
||||
control.buttonMode = config.mode;
|
||||
control.pin = output.pin;
|
||||
control.polarity = output.polarity;
|
||||
controls_.push_back(control);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addSlider(const SliderControlConfig& config, SliderCallback callback) {
|
||||
validateControlIdentity(config.id, config.name);
|
||||
validateRange(config.id, config.minimum, config.maximum);
|
||||
ControlRegistration control;
|
||||
control.id = config.id;
|
||||
control.name = config.name;
|
||||
control.type = ControlType::Slider;
|
||||
control.output = OutputType::Custom;
|
||||
control.minimum = config.minimum;
|
||||
control.maximum = config.maximum;
|
||||
control.sliderCallback = callback;
|
||||
controls_.push_back(control);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addButton(const ButtonControlConfig& config, ButtonCallback callback) {
|
||||
validateControlIdentity(config.id, config.name);
|
||||
ControlRegistration control;
|
||||
control.id = config.id;
|
||||
control.name = config.name;
|
||||
control.type = ControlType::Button;
|
||||
control.output = OutputType::Custom;
|
||||
control.buttonMode = config.mode;
|
||||
control.buttonCallback = callback;
|
||||
controls_.push_back(control);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addNumber(const NumberControlConfig& config, NumberCallback callback) {
|
||||
validateControlIdentity(config.id, config.name);
|
||||
validateRange(config.id, config.minimum, config.maximum);
|
||||
ControlRegistration control;
|
||||
control.id = config.id;
|
||||
control.name = config.name;
|
||||
control.type = ControlType::Number;
|
||||
control.output = OutputType::Custom;
|
||||
control.minimum = config.minimum;
|
||||
control.maximum = config.maximum;
|
||||
control.numberCallback = callback;
|
||||
controls_.push_back(control);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addText(const TextControlConfig& config, TextCallback callback) {
|
||||
validateControlIdentity(config.id, config.name);
|
||||
if (config.maximumLength == 0) {
|
||||
abort();
|
||||
}
|
||||
ControlRegistration control;
|
||||
control.id = config.id;
|
||||
control.name = config.name;
|
||||
control.type = ControlType::Text;
|
||||
control.output = OutputType::Custom;
|
||||
control.maximumLength = config.maximumLength;
|
||||
control.textCallback = callback;
|
||||
controls_.push_back(control);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addRoverCameraServo(const RoverCameraServoConfig& config) {
|
||||
cameraServo_ = config;
|
||||
hasCameraServo_ = true;
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addRoverHeadlight(const RoverDigitalOutputConfig& config) {
|
||||
headlight_ = config;
|
||||
hasHeadlight_ = true;
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::addRoverLaser(const RoverDigitalOutputConfig& config) {
|
||||
laser_ = config;
|
||||
hasLaser_ = true;
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::begin(FirmataExt& extension) {
|
||||
if (instance_ != nullptr && instance_ != this) {
|
||||
abort();
|
||||
}
|
||||
instance_ = this;
|
||||
extension.addFeature(*this);
|
||||
|
||||
// Discovery uses Firmata's standard REPORT_FIRMWARE query to distinguish a
|
||||
// rover peripheral from unrelated Firmata devices. The helper owns this
|
||||
// identity so every sketch gets it without repeating protocol boilerplate.
|
||||
Firmata.setFirmwareNameAndVersion("RoverPeripheralFirmata", 1, 0);
|
||||
|
||||
// SET_DIGITAL_PIN_VALUE is a fixed Firmata command rather than SysEx, so it
|
||||
// cannot travel through FirmataFeature::handleSysex. Firmata exposes one
|
||||
// callback for it and this peripheral owns the standard output implementation.
|
||||
Firmata.attach(SET_DIGITAL_PIN_VALUE, digitalPinValueCallback);
|
||||
Firmata.attach(SYSTEM_RESET, systemResetCallback);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::update() {
|
||||
// Custom callbacks execute synchronously from Firmata's parser for now. This
|
||||
// method intentionally remains available so future non-blocking peripheral
|
||||
// work can be serviced without changing the sketch's main loop shape.
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::handleCapability(byte pin) {
|
||||
if (!IS_PIN_DIGITAL(pin)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// The peripheral supports the output modes roverd may select. Capability
|
||||
// reporting stays standard Firmata, so the Linux probe can also inspect it
|
||||
// with any other conforming client.
|
||||
Firmata.write(PIN_MODE_OUTPUT);
|
||||
Firmata.write(1);
|
||||
if (IS_PIN_PWM(pin)) {
|
||||
Firmata.write(PIN_MODE_PWM);
|
||||
Firmata.write(DEFAULT_PWM_RESOLUTION);
|
||||
}
|
||||
Firmata.write(PIN_MODE_SERVO);
|
||||
Firmata.write(14);
|
||||
}
|
||||
|
||||
boolean RoverPeripheralFirmata::handlePinMode(byte pin, int mode) {
|
||||
if (pin >= TOTAL_PINS || !IS_PIN_DIGITAL(pin)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// A pin can only have one active hardware generator. Detaching a previous
|
||||
// servo before switching modes prevents it from continuing to pulse after a
|
||||
// later digital or PWM configuration takes ownership of the pin.
|
||||
if (mode != PIN_MODE_SERVO) {
|
||||
detachServo(pin);
|
||||
}
|
||||
|
||||
switch (mode) {
|
||||
case PIN_MODE_OUTPUT:
|
||||
pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
|
||||
digitalWrite(PIN_TO_DIGITAL(pin), LOW);
|
||||
Firmata.setPinState(pin, 0);
|
||||
return true;
|
||||
case PIN_MODE_PWM:
|
||||
if (!IS_PIN_PWM(pin)) {
|
||||
return false;
|
||||
}
|
||||
pinMode(PIN_TO_PWM(pin), OUTPUT);
|
||||
analogWrite(PIN_TO_PWM(pin), 0);
|
||||
Firmata.setPinState(pin, 0);
|
||||
return true;
|
||||
case PIN_MODE_SERVO:
|
||||
attachServo(pin);
|
||||
Firmata.setPinState(pin, 0);
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
boolean RoverPeripheralFirmata::handleSysex(byte command, byte argc, byte* argv) {
|
||||
if (command == kPeripheralFeature) {
|
||||
if (argc == 0) {
|
||||
return true;
|
||||
}
|
||||
if (argv[0] == kDescribeOperation) {
|
||||
buildAndSendDescription();
|
||||
} else if (argv[0] == kControlOperation) {
|
||||
dispatchCustomControl(argc, argv);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
if (command == SERVO_CONFIG && argc >= 5) {
|
||||
const byte pin = argv[0];
|
||||
const int minimumPulse = argv[1] | (argv[2] << 7);
|
||||
const int maximumPulse = argv[3] | (argv[4] << 7);
|
||||
if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
|
||||
Firmata.setPinMode(pin, PIN_MODE_SERVO);
|
||||
attachServo(pin, minimumPulse, maximumPulse);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
if (command == EXTENDED_ANALOG && argc >= 2) {
|
||||
const byte pin = argv[0];
|
||||
if (pin >= TOTAL_PINS) {
|
||||
return true;
|
||||
}
|
||||
|
||||
int value = 0;
|
||||
// Extended analog values contain a variable number of seven-bit chunks.
|
||||
// Reassembling every received chunk keeps servo angles and PWM values fully
|
||||
// compatible with normal Firmata clients rather than assuming eight bits.
|
||||
for (byte index = 1; index < argc && index <= 4; ++index) {
|
||||
value |= static_cast<int>(argv[index]) << (7 * (index - 1));
|
||||
}
|
||||
|
||||
const byte mode = Firmata.getPinMode(pin);
|
||||
if (mode == PIN_MODE_PWM && IS_PIN_PWM(pin)) {
|
||||
analogWrite(PIN_TO_PWM(pin), value);
|
||||
Firmata.setPinState(pin, value);
|
||||
} else if (mode == PIN_MODE_SERVO && servos_[pin] != nullptr) {
|
||||
servos_[pin]->write(value);
|
||||
Firmata.setPinState(pin, value);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::reset() {
|
||||
for (byte pin = 0; pin < TOTAL_PINS; ++pin) {
|
||||
detachServo(pin);
|
||||
}
|
||||
|
||||
// Built-in role defaults are applied on Firmata reset as well as boot. This
|
||||
// makes reconnecting a client deterministic without creating a second state
|
||||
// model on the ESP32.
|
||||
if (hasHeadlight_) {
|
||||
pinMode(headlight_.pin, OUTPUT);
|
||||
const bool physicalHigh = headlight_.initiallyOn != (headlight_.polarity == OutputPolarity::ActiveLow);
|
||||
writeDigitalPin(headlight_.pin, physicalHigh);
|
||||
}
|
||||
if (hasLaser_) {
|
||||
pinMode(laser_.pin, OUTPUT);
|
||||
const bool physicalHigh = laser_.initiallyOn != (laser_.polarity == OutputPolarity::ActiveLow);
|
||||
writeDigitalPin(laser_.pin, physicalHigh);
|
||||
}
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::buildAndSendDescription() {
|
||||
JsonDocument document;
|
||||
document["name"] = name_;
|
||||
|
||||
if (hasCameraServo_ || hasHeadlight_ || hasLaser_) {
|
||||
JsonObject roverControls = document["roverControls"].to<JsonObject>();
|
||||
if (hasCameraServo_) {
|
||||
JsonObject servo = roverControls["cameraServo"].to<JsonObject>();
|
||||
servo["pin"] = cameraServo_.pin;
|
||||
servo["minimumAngleDegrees"] = cameraServo_.minimumAngleDegrees;
|
||||
servo["maximumAngleDegrees"] = cameraServo_.maximumAngleDegrees;
|
||||
servo["homeAngleDegrees"] = cameraServo_.homeAngleDegrees;
|
||||
servo["nudgeDegrees"] = cameraServo_.nudgeDegrees;
|
||||
servo["minimumPulseMicroseconds"] = cameraServo_.minimumPulseMicroseconds;
|
||||
servo["maximumPulseMicroseconds"] = cameraServo_.maximumPulseMicroseconds;
|
||||
servo["allowRawPulse"] = cameraServo_.allowRawPulse;
|
||||
servo["inverted"] = cameraServo_.inverted;
|
||||
}
|
||||
|
||||
auto addDigitalRole = [&roverControls](const char* key, const RoverDigitalOutputConfig& config) {
|
||||
JsonObject role = roverControls[key].to<JsonObject>();
|
||||
role["pin"] = config.pin;
|
||||
role["activeLow"] = config.polarity == OutputPolarity::ActiveLow;
|
||||
role["initiallyOn"] = config.initiallyOn;
|
||||
};
|
||||
if (hasHeadlight_) {
|
||||
addDigitalRole("headlight", headlight_);
|
||||
}
|
||||
if (hasLaser_) {
|
||||
addDigitalRole("laser", laser_);
|
||||
}
|
||||
}
|
||||
|
||||
JsonArray controls = document["controls"].to<JsonArray>();
|
||||
for (const ControlRegistration& registration : controls_) {
|
||||
JsonObject control = controls.add<JsonObject>();
|
||||
control["id"] = registration.id;
|
||||
control["name"] = registration.name;
|
||||
|
||||
switch (registration.type) {
|
||||
case ControlType::Slider:
|
||||
control["type"] = "slider";
|
||||
control["min"] = registration.minimum;
|
||||
control["max"] = registration.maximum;
|
||||
break;
|
||||
case ControlType::Button:
|
||||
control["type"] = "button";
|
||||
control["mode"] = buttonModeName(registration.buttonMode);
|
||||
break;
|
||||
case ControlType::Number:
|
||||
control["type"] = "number";
|
||||
control["min"] = registration.minimum;
|
||||
control["max"] = registration.maximum;
|
||||
break;
|
||||
case ControlType::Text:
|
||||
control["type"] = "text";
|
||||
control["maxLength"] = registration.maximumLength;
|
||||
break;
|
||||
}
|
||||
|
||||
JsonObject output = control["output"].to<JsonObject>();
|
||||
output["type"] = outputTypeName(static_cast<uint8_t>(registration.output));
|
||||
if (registration.output != OutputType::Custom) {
|
||||
output["pin"] = registration.pin;
|
||||
}
|
||||
if (registration.output == OutputType::Digital && registration.polarity == OutputPolarity::ActiveLow) {
|
||||
output["activeLow"] = true;
|
||||
}
|
||||
}
|
||||
|
||||
String payload;
|
||||
serializeJson(document, payload);
|
||||
|
||||
// ConfigurableFirmata's convenience sendSysex takes a byte-sized raw length.
|
||||
// Descriptions can exceed that, so write the standard framing and each 7-bit
|
||||
// pair directly. This remains one ordinary Firmata SysEx message on the wire.
|
||||
Firmata.startSysex();
|
||||
Firmata.write(kPeripheralFeature);
|
||||
Firmata.write(kDescriptionOperation);
|
||||
for (size_t index = 0; index < payload.length(); ++index) {
|
||||
Firmata.sendValueAsTwo7bitBytes(static_cast<uint8_t>(payload[index]));
|
||||
}
|
||||
Firmata.endSysex();
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::dispatchCustomControl(byte argc, byte* argv) {
|
||||
if (argc < 3 || ((argc - 1) % 2) != 0) {
|
||||
Firmata.sendString(F("Invalid rover control payload"));
|
||||
return;
|
||||
}
|
||||
|
||||
String decoded;
|
||||
decoded.reserve((argc - 1) / 2);
|
||||
for (byte index = 1; index + 1 < argc; index += 2) {
|
||||
if (argv[index + 1] > 1) {
|
||||
Firmata.sendString(F("Invalid rover control encoding"));
|
||||
return;
|
||||
}
|
||||
decoded += static_cast<char>(argv[index] | (argv[index + 1] << 7));
|
||||
}
|
||||
|
||||
JsonDocument document;
|
||||
if (deserializeJson(document, decoded) != DeserializationError::Ok) {
|
||||
Firmata.sendString(F("Invalid rover control JSON"));
|
||||
return;
|
||||
}
|
||||
|
||||
const String controlID = document["control"].as<String>();
|
||||
for (ControlRegistration& registration : controls_) {
|
||||
if (registration.id != controlID || registration.output != OutputType::Custom) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// The registration type is the source of truth for value conversion. This
|
||||
// prevents an unexpected JSON value from silently selecting a different
|
||||
// callback signature or invoking unrelated application behavior.
|
||||
switch (registration.type) {
|
||||
case ControlType::Slider:
|
||||
if (registration.sliderCallback) {
|
||||
registration.sliderCallback(document["value"].as<int>());
|
||||
}
|
||||
break;
|
||||
case ControlType::Button:
|
||||
if (registration.buttonCallback) {
|
||||
registration.buttonCallback(document["value"].as<bool>());
|
||||
}
|
||||
break;
|
||||
case ControlType::Number:
|
||||
if (registration.numberCallback) {
|
||||
registration.numberCallback(document["value"].as<int>());
|
||||
}
|
||||
break;
|
||||
case ControlType::Text:
|
||||
if (registration.textCallback) {
|
||||
String value = document["value"].as<String>();
|
||||
if (value.length() > registration.maximumLength) {
|
||||
value.remove(registration.maximumLength);
|
||||
}
|
||||
registration.textCallback(value);
|
||||
}
|
||||
break;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
Firmata.sendString(F("Unknown rover control"));
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::writeDigitalPin(byte pin, bool physicalHigh) {
|
||||
// Standard Firmata digital values represent the electrical pin level. roverd
|
||||
// applies the advertised activeLow mapping before sending a command, keeping
|
||||
// this firmware compatible with raw Firmata clients and avoiding inversion in
|
||||
// two different layers.
|
||||
digitalWrite(PIN_TO_DIGITAL(pin), physicalHigh ? HIGH : LOW);
|
||||
Firmata.setPinState(pin, physicalHigh ? 1 : 0);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::attachServo(byte pin, int minimumPulseMicroseconds, int maximumPulseMicroseconds) {
|
||||
if (pin >= TOTAL_PINS || !IS_PIN_DIGITAL(pin)) {
|
||||
return;
|
||||
}
|
||||
if (servos_[pin] == nullptr) {
|
||||
servos_[pin] = new Servo();
|
||||
}
|
||||
if (servos_[pin]->attached()) {
|
||||
servos_[pin]->detach();
|
||||
}
|
||||
if (minimumPulseMicroseconds > 0 && maximumPulseMicroseconds > minimumPulseMicroseconds) {
|
||||
servos_[pin]->attach(PIN_TO_SERVO(pin), minimumPulseMicroseconds, maximumPulseMicroseconds);
|
||||
} else {
|
||||
servos_[pin]->attach(PIN_TO_SERVO(pin));
|
||||
}
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::detachServo(byte pin) {
|
||||
if (pin >= TOTAL_PINS || servos_[pin] == nullptr) {
|
||||
return;
|
||||
}
|
||||
if (servos_[pin]->attached()) {
|
||||
servos_[pin]->detach();
|
||||
}
|
||||
delete servos_[pin];
|
||||
servos_[pin] = nullptr;
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::digitalPinValueCallback(byte pin, int value) {
|
||||
if (instance_ == nullptr || pin >= TOTAL_PINS || Firmata.getPinMode(pin) != PIN_MODE_OUTPUT) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Polarity is advertised by the peripheral and applied by roverd before this
|
||||
// standard raw pin-level command reaches the ESP32.
|
||||
instance_->writeDigitalPin(pin, value != 0);
|
||||
}
|
||||
|
||||
void RoverPeripheralFirmata::systemResetCallback() {
|
||||
if (instance_ != nullptr) {
|
||||
instance_->reset();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,170 @@
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <ArduinoJson.h>
|
||||
#include <ConfigurableFirmata.h>
|
||||
#include <ESP32Servo.h>
|
||||
#include <FirmataExt.h>
|
||||
|
||||
#include <functional>
|
||||
#include <vector>
|
||||
|
||||
enum class OutputPolarity {
|
||||
ActiveHigh,
|
||||
ActiveLow,
|
||||
};
|
||||
|
||||
enum class ButtonMode {
|
||||
Toggle,
|
||||
Momentary,
|
||||
};
|
||||
|
||||
struct FirmataServoOutput {
|
||||
uint8_t pin = 0;
|
||||
};
|
||||
|
||||
struct FirmataPwmOutput {
|
||||
uint8_t pin = 0;
|
||||
};
|
||||
|
||||
struct FirmataDigitalOutput {
|
||||
uint8_t pin = 0;
|
||||
OutputPolarity polarity = OutputPolarity::ActiveHigh;
|
||||
};
|
||||
|
||||
struct RoverCameraServoConfig {
|
||||
uint8_t pin = 0;
|
||||
float minimumAngleDegrees = -15;
|
||||
float maximumAngleDegrees = 30;
|
||||
float homeAngleDegrees = 0;
|
||||
float nudgeDegrees = 2;
|
||||
uint16_t minimumPulseMicroseconds = 900;
|
||||
uint16_t maximumPulseMicroseconds = 2100;
|
||||
bool allowRawPulse = false;
|
||||
bool inverted = false;
|
||||
};
|
||||
|
||||
struct RoverDigitalOutputConfig {
|
||||
uint8_t pin = 0;
|
||||
OutputPolarity polarity = OutputPolarity::ActiveHigh;
|
||||
bool initiallyOn = false;
|
||||
};
|
||||
|
||||
struct SliderControlConfig {
|
||||
String id;
|
||||
String name;
|
||||
int minimum = 0;
|
||||
int maximum = 100;
|
||||
};
|
||||
|
||||
struct ButtonControlConfig {
|
||||
String id;
|
||||
String name;
|
||||
ButtonMode mode = ButtonMode::Momentary;
|
||||
};
|
||||
|
||||
struct NumberControlConfig {
|
||||
String id;
|
||||
String name;
|
||||
int minimum = 0;
|
||||
int maximum = 100;
|
||||
};
|
||||
|
||||
struct TextControlConfig {
|
||||
String id;
|
||||
String name;
|
||||
size_t maximumLength = 32;
|
||||
};
|
||||
|
||||
using SliderCallback = std::function<void(int)>;
|
||||
using ButtonCallback = std::function<void(bool)>;
|
||||
using NumberCallback = std::function<void(int)>;
|
||||
using TextCallback = std::function<void(const String&)>;
|
||||
|
||||
/*
|
||||
* RoverPeripheralFirmata is both the sketch-facing registration API and one
|
||||
* ConfigurableFirmata feature. Keeping those responsibilities together gives a
|
||||
* peripheral author one object to configure while still allowing ordinary
|
||||
* Firmata tooling to use digital, PWM, and servo commands on the same stream.
|
||||
*/
|
||||
class RoverPeripheralFirmata : public FirmataFeature {
|
||||
public:
|
||||
explicit RoverPeripheralFirmata(const String& name);
|
||||
|
||||
void addServoSlider(const SliderControlConfig& config, const FirmataServoOutput& output);
|
||||
void addPwmSlider(const SliderControlConfig& config, const FirmataPwmOutput& output);
|
||||
void addDigitalButton(const ButtonControlConfig& config, const FirmataDigitalOutput& output);
|
||||
void addSlider(const SliderControlConfig& config, SliderCallback callback);
|
||||
void addButton(const ButtonControlConfig& config, ButtonCallback callback);
|
||||
void addNumber(const NumberControlConfig& config, NumberCallback callback);
|
||||
void addText(const TextControlConfig& config, TextCallback callback);
|
||||
|
||||
void addRoverCameraServo(const RoverCameraServoConfig& config);
|
||||
void addRoverHeadlight(const RoverDigitalOutputConfig& config);
|
||||
void addRoverLaser(const RoverDigitalOutputConfig& config);
|
||||
|
||||
void begin(FirmataExt& extension);
|
||||
void update();
|
||||
|
||||
// FirmataFeature methods let FirmataExt route standard and custom SysEx
|
||||
// operations through the same parser that owns the serial connection.
|
||||
void handleCapability(byte pin) override;
|
||||
boolean handlePinMode(byte pin, int mode) override;
|
||||
boolean handleSysex(byte command, byte argc, byte* argv) override;
|
||||
void reset() override;
|
||||
|
||||
private:
|
||||
enum class ControlType {
|
||||
Slider,
|
||||
Button,
|
||||
Number,
|
||||
Text,
|
||||
};
|
||||
|
||||
enum class OutputType {
|
||||
Servo,
|
||||
Pwm,
|
||||
Digital,
|
||||
Custom,
|
||||
};
|
||||
|
||||
struct ControlRegistration {
|
||||
String id;
|
||||
String name;
|
||||
ControlType type;
|
||||
OutputType output;
|
||||
int minimum = 0;
|
||||
int maximum = 0;
|
||||
size_t maximumLength = 0;
|
||||
ButtonMode buttonMode = ButtonMode::Momentary;
|
||||
uint8_t pin = 0;
|
||||
OutputPolarity polarity = OutputPolarity::ActiveHigh;
|
||||
SliderCallback sliderCallback;
|
||||
ButtonCallback buttonCallback;
|
||||
NumberCallback numberCallback;
|
||||
TextCallback textCallback;
|
||||
};
|
||||
|
||||
String name_;
|
||||
std::vector<ControlRegistration> controls_;
|
||||
bool hasCameraServo_ = false;
|
||||
bool hasHeadlight_ = false;
|
||||
bool hasLaser_ = false;
|
||||
RoverCameraServoConfig cameraServo_;
|
||||
RoverDigitalOutputConfig headlight_;
|
||||
RoverDigitalOutputConfig laser_;
|
||||
Servo* servos_[TOTAL_PINS] = {};
|
||||
|
||||
void validateControlIdentity(const String& id, const String& name) const;
|
||||
void validateRange(const String& id, int minimum, int maximum) const;
|
||||
void buildAndSendDescription();
|
||||
void dispatchCustomControl(byte argc, byte* argv);
|
||||
void writeDigitalPin(byte pin, bool enabled);
|
||||
void attachServo(byte pin, int minimumPulseMicroseconds = -1, int maximumPulseMicroseconds = -1);
|
||||
void detachServo(byte pin);
|
||||
|
||||
static RoverPeripheralFirmata* instance_;
|
||||
static void digitalPinValueCallback(byte pin, int value);
|
||||
static void systemResetCallback();
|
||||
};
|
||||
|
||||
@@ -0,0 +1,28 @@
|
||||
[platformio]
|
||||
default_envs = esp32dev
|
||||
|
||||
[env]
|
||||
platform = espressif32
|
||||
framework = arduino
|
||||
monitor_speed = 115200
|
||||
lib_extra_dirs = ../libraries
|
||||
lib_deps =
|
||||
; 3.2.0 targets the Arduino 2.x core shipped by PlatformIO's stable ESP32
|
||||
; platform. ConfigurableFirmata 3.4.0 switched its bundled PWM source to the
|
||||
; Arduino 3.x LEDC API even when that unused source is compiled as a dependency.
|
||||
https://github.com/firmata/ConfigurableFirmata.git#3.2.0
|
||||
bblanchon/ArduinoJson@^7.4.2
|
||||
madhephaestus/ESP32Servo@^3.0.8
|
||||
|
||||
; This is the generic ESP32-WROOM-32/DevKitC target used by boards carrying a
|
||||
; CH340 or CP210x USB-to-UART bridge. Linux normally exposes it as ttyUSB*.
|
||||
[env:esp32dev]
|
||||
board = esp32dev
|
||||
|
||||
; Native USB boards use the same sketch and Firmata stream. These flags make the
|
||||
; ESP32-S3's USB CDC serial port active at boot, normally appearing as ttyACM*.
|
||||
[env:esp32-s3-devkitc-1]
|
||||
board = esp32-s3-devkitc-1
|
||||
build_flags =
|
||||
-D ARDUINO_USB_MODE=1
|
||||
-D ARDUINO_USB_CDC_ON_BOOT=1
|
||||
@@ -0,0 +1,122 @@
|
||||
#include <Arduino.h>
|
||||
#include <ConfigurableFirmata.h>
|
||||
#include <FirmataExt.h>
|
||||
#include <RoverPeripheralFirmata.h>
|
||||
|
||||
FirmataExt firmataExtension;
|
||||
RoverPeripheralFirmata peripheral("Rover GPIO");
|
||||
|
||||
namespace {
|
||||
// Every example pin is present on both the classic ESP32 DevKitC and the
|
||||
// ESP32-S3 DevKitC. GPIO 19 and 20 are deliberately avoided because native-USB
|
||||
// S3 boards use them for USB D- and D+.
|
||||
constexpr uint8_t kSpecialActionPin = 21;
|
||||
|
||||
void runSpecialAction() {
|
||||
// This intentionally represents arbitrary device behavior rather than a raw
|
||||
// pin mapping. Replace it with a motor sequence, LED animation, actuator
|
||||
// routine, or any other application-specific function the accessory needs.
|
||||
digitalWrite(kSpecialActionPin, HIGH);
|
||||
delay(80);
|
||||
digitalWrite(kSpecialActionPin, LOW);
|
||||
}
|
||||
|
||||
void registerBuiltInRoverControls() {
|
||||
// These three roles replace physical GPIO backends while preserving the
|
||||
// existing camera, headlight, and laser commands and HUD controls.
|
||||
RoverCameraServoConfig cameraServo;
|
||||
cameraServo.pin = 14;
|
||||
cameraServo.minimumAngleDegrees = -15;
|
||||
cameraServo.maximumAngleDegrees = 30;
|
||||
cameraServo.homeAngleDegrees = 0;
|
||||
cameraServo.nudgeDegrees = 2;
|
||||
cameraServo.minimumPulseMicroseconds = 900;
|
||||
cameraServo.maximumPulseMicroseconds = 2100;
|
||||
cameraServo.allowRawPulse = false;
|
||||
cameraServo.inverted = false;
|
||||
peripheral.addRoverCameraServo(cameraServo);
|
||||
|
||||
RoverDigitalOutputConfig headlight;
|
||||
headlight.pin = 18;
|
||||
headlight.polarity = OutputPolarity::ActiveHigh;
|
||||
headlight.initiallyOn = false;
|
||||
peripheral.addRoverHeadlight(headlight);
|
||||
|
||||
RoverDigitalOutputConfig laser;
|
||||
laser.pin = 16;
|
||||
laser.polarity = OutputPolarity::ActiveHigh;
|
||||
laser.initiallyOn = false;
|
||||
peripheral.addRoverLaser(laser);
|
||||
}
|
||||
|
||||
void registerGenericControls() {
|
||||
// Registration order is UI order. This servo slider is handled entirely by
|
||||
// standard Firmata SET_PIN_MODE and EXTENDED_ANALOG messages from roverd.
|
||||
SliderControlConfig servoPosition;
|
||||
servoPosition.id = "servoPosition";
|
||||
servoPosition.name = "Servo position";
|
||||
servoPosition.minimum = 0;
|
||||
servoPosition.maximum = 180;
|
||||
|
||||
FirmataServoOutput servoOutput;
|
||||
servoOutput.pin = 13;
|
||||
peripheral.addServoSlider(servoPosition, servoOutput);
|
||||
|
||||
// PWM brightness is another standard Firmata output. No sketch callback is
|
||||
// involved when the driver moves this slider.
|
||||
SliderControlConfig lightBrightness;
|
||||
lightBrightness.id = "lightBrightness";
|
||||
lightBrightness.name = "Light brightness";
|
||||
lightBrightness.minimum = 0;
|
||||
lightBrightness.maximum = 255;
|
||||
|
||||
FirmataPwmOutput lightOutput;
|
||||
lightOutput.pin = 17;
|
||||
peripheral.addPwmSlider(lightBrightness, lightOutput);
|
||||
|
||||
// A custom momentary control receives both press and release. This example
|
||||
// runs a one-shot action only on press, but a motor could use both values to
|
||||
// start while held and stop on release.
|
||||
ButtonControlConfig specialAction;
|
||||
specialAction.id = "specialAction";
|
||||
specialAction.name = "Run special action";
|
||||
specialAction.mode = ButtonMode::Momentary;
|
||||
peripheral.addButton(specialAction, [](bool pressed) {
|
||||
if (pressed) {
|
||||
runSpecialAction();
|
||||
}
|
||||
});
|
||||
}
|
||||
} // namespace
|
||||
|
||||
void setup() {
|
||||
pinMode(kSpecialActionPin, OUTPUT);
|
||||
digitalWrite(kSpecialActionPin, LOW);
|
||||
|
||||
registerBuiltInRoverControls();
|
||||
registerGenericControls();
|
||||
|
||||
// Supplying Serial as a Stream keeps all protocol code identical between a
|
||||
// CH340/CP210x UART bridge and native ESP32-S3 USB CDC. Only PlatformIO's S3
|
||||
// build flags differ.
|
||||
Serial.begin(115200);
|
||||
Firmata.begin(Serial);
|
||||
peripheral.begin(firmataExtension);
|
||||
|
||||
// A Firmata system reset establishes declared initial output states and also
|
||||
// proves that all callbacks were installed before normal traffic begins.
|
||||
Firmata.parse(SYSTEM_RESET);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Processing one complete parser unit at a time prevents a long serial burst
|
||||
// from starving application work while still draining ordinary USB traffic
|
||||
// quickly on both supported transports.
|
||||
while (Firmata.available()) {
|
||||
Firmata.processInput();
|
||||
if (!Firmata.isParsingMessage()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
peripheral.update();
|
||||
}
|
||||
@@ -25,10 +25,6 @@ type CameraServo struct {
|
||||
closed bool
|
||||
}
|
||||
|
||||
const maxServoDegPerSec = 60.0
|
||||
const servoStepInterval = 20 * time.Millisecond
|
||||
const servoAngleEpsilon = 0.01
|
||||
|
||||
func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, error) {
|
||||
if !cfg.Enabled {
|
||||
return nil, fmt.Errorf("camera servo disabled")
|
||||
@@ -132,6 +128,12 @@ func (s *CameraServo) CurrentAngle() float64 {
|
||||
return s.currentAngle
|
||||
}
|
||||
|
||||
// Configuration reports the effective public behavior advertised to the
|
||||
// server. The native implementation simply returns its validated YAML config.
|
||||
func (s *CameraServo) Configuration() CameraServoConfig {
|
||||
return s.cfg
|
||||
}
|
||||
|
||||
func (s *CameraServo) applyPulseLocked(micros int) {
|
||||
micros = clampInt(micros, s.cfg.MinPulseUs, s.cfg.MaxPulseUs)
|
||||
s.pin.DutyCycle(uint32(micros), uint32(s.cfg.CycleLen))
|
||||
|
||||
@@ -11,10 +11,9 @@ type CameraServo struct{}
|
||||
|
||||
func NewCameraServo(_ CameraServoConfig, _ *log.Logger) (*CameraServo, error) {
|
||||
/*
|
||||
The Debian laptop profile starts with the laptop's built-in webcam and no
|
||||
Pi PWM servo. If a laptop rover eventually grows an external servo board,
|
||||
it should get its own implementation instead of reusing Raspberry Pi GPIO
|
||||
assumptions.
|
||||
This constructor represents only native host GPIO. The shared startup
|
||||
resolver selects the normal Firmata implementation when an ESP32 provides
|
||||
the role, so external hardware is not laptop-specific code.
|
||||
*/
|
||||
return nil, fmt.Errorf("camera servo not supported in the debian-laptop build")
|
||||
}
|
||||
@@ -36,3 +35,7 @@ func (c *CameraServo) SetPulseWidth(micros int) error {
|
||||
func (c *CameraServo) CurrentAngle() float64 {
|
||||
return 0
|
||||
}
|
||||
|
||||
func (c *CameraServo) Configuration() CameraServoConfig {
|
||||
return CameraServoConfig{}
|
||||
}
|
||||
|
||||
@@ -30,3 +30,7 @@ func (c *CameraServo) SetPulseWidth(micros int) error {
|
||||
func (c *CameraServo) CurrentAngle() float64 {
|
||||
return 0
|
||||
}
|
||||
|
||||
func (c *CameraServo) Configuration() CameraServoConfig {
|
||||
return CameraServoConfig{}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,151 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"flag"
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"time"
|
||||
|
||||
roverd "multiroombarover/pi/roverd"
|
||||
|
||||
"github.com/tarm/serial"
|
||||
)
|
||||
|
||||
func main() {
|
||||
var portName string
|
||||
var baud int
|
||||
var timeout time.Duration
|
||||
var startupWait time.Duration
|
||||
var controlID string
|
||||
var rawValue string
|
||||
|
||||
flag.StringVar(&portName, "port", "", "serial device, for example /dev/ttyUSB0 or /dev/ttyACM0")
|
||||
flag.IntVar(&baud, "baud", 115200, "Firmata serial baud rate")
|
||||
flag.DurationVar(&timeout, "timeout", 5*time.Second, "timeout for each Firmata response")
|
||||
flag.DurationVar(&startupWait, "startup-wait", 2*time.Second, "time allowed for boards that reset when the port opens")
|
||||
flag.StringVar(&controlID, "control", "", "optional declared control ID to exercise")
|
||||
flag.StringVar(&rawValue, "value", "", "JSON value for -control, such as 90, true, or \"hello\"")
|
||||
flag.Parse()
|
||||
|
||||
if portName == "" {
|
||||
log.Fatal("-port is required")
|
||||
}
|
||||
if (controlID == "") != (rawValue == "") {
|
||||
log.Fatal("-control and -value must be provided together")
|
||||
}
|
||||
|
||||
port, err := serial.OpenPort(&serial.Config{
|
||||
Name: portName,
|
||||
Baud: baud,
|
||||
ReadTimeout: 100 * time.Millisecond,
|
||||
})
|
||||
if err != nil {
|
||||
log.Fatalf("open %s: %v", portName, err)
|
||||
}
|
||||
defer port.Close()
|
||||
|
||||
// CH340 and native-USB development boards may reset when the host opens the
|
||||
// port. Waiting here makes the same probe work with both connection styles
|
||||
// without baking that diagnostic delay into the production Firmata client.
|
||||
time.Sleep(startupWait)
|
||||
|
||||
rootContext, cancelRoot := context.WithCancel(context.Background())
|
||||
defer cancelRoot()
|
||||
client := roverd.NewFirmataClient(port)
|
||||
client.Start(rootContext)
|
||||
|
||||
firmware, err := withTimeout(timeout, client.QueryFirmware)
|
||||
if err != nil {
|
||||
log.Fatalf("query firmware: %v", err)
|
||||
}
|
||||
fmt.Printf("Firmata firmware: %s %d.%d\n", firmware.Name, firmware.Major, firmware.Minor)
|
||||
|
||||
capabilities, err := withTimeout(timeout, client.QueryCapabilities)
|
||||
if err != nil {
|
||||
log.Fatalf("query capabilities: %v", err)
|
||||
}
|
||||
fmt.Printf("Firmata pins described: %d\n", len(capabilities))
|
||||
|
||||
description, err := withTimeout(timeout, client.Describe)
|
||||
if err != nil {
|
||||
log.Fatalf("describe rover peripheral: %v", err)
|
||||
}
|
||||
formatted, err := json.MarshalIndent(description, "", " ")
|
||||
if err != nil {
|
||||
log.Fatalf("format description: %v", err)
|
||||
}
|
||||
fmt.Printf("Peripheral description:\n%s\n", formatted)
|
||||
|
||||
if controlID != "" {
|
||||
if err := exerciseControl(client, description, controlID, json.RawMessage(rawValue)); err != nil {
|
||||
log.Fatalf("exercise control %q: %v", controlID, err)
|
||||
}
|
||||
fmt.Fprintf(os.Stdout, "Control %q accepted.\n", controlID)
|
||||
}
|
||||
}
|
||||
|
||||
// withTimeout gives every boot-time exchange its own deadline. A missing board
|
||||
// therefore reports the exact handshake stage that failed instead of consuming
|
||||
// one shared timeout and obscuring which response was absent.
|
||||
func withTimeout[T any](timeout time.Duration, operation func(context.Context) (T, error)) (T, error) {
|
||||
ctx, cancel := context.WithTimeout(context.Background(), timeout)
|
||||
defer cancel()
|
||||
return operation(ctx)
|
||||
}
|
||||
|
||||
func exerciseControl(client *roverd.FirmataClient, description roverd.PeripheralDescription, controlID string, rawValue json.RawMessage) error {
|
||||
var selected *roverd.PeripheralControl
|
||||
for index := range description.Controls {
|
||||
if description.Controls[index].ID == controlID {
|
||||
selected = &description.Controls[index]
|
||||
break
|
||||
}
|
||||
}
|
||||
if selected == nil {
|
||||
return errors.New("control is not present in the device description")
|
||||
}
|
||||
|
||||
var value any
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return fmt.Errorf("parse -value as JSON: %w", err)
|
||||
}
|
||||
|
||||
// Standard outputs deliberately use standard Firmata commands. Only custom
|
||||
// callbacks use the rover-peripheral CONTROL operation, which is the central
|
||||
// distinction the probe is intended to validate on real hardware.
|
||||
switch selected.Output.Type {
|
||||
case "custom":
|
||||
return client.SendPeripheralControl(selected.ID, value)
|
||||
case "digital":
|
||||
enabled, ok := value.(bool)
|
||||
if !ok {
|
||||
return errors.New("digital control value must be true or false")
|
||||
}
|
||||
if selected.Output.ActiveLow {
|
||||
enabled = !enabled
|
||||
}
|
||||
if err := client.SetPinMode(byte(*selected.Output.Pin), roverd.FirmataPinModeOutput); err != nil {
|
||||
return err
|
||||
}
|
||||
return client.SetDigitalPin(byte(*selected.Output.Pin), enabled)
|
||||
case "pwm", "servo":
|
||||
number, ok := value.(float64)
|
||||
if !ok || number != float64(int(number)) {
|
||||
return errors.New("PWM and servo control values must be whole numbers")
|
||||
}
|
||||
mode := roverd.FirmataPinModePWM
|
||||
if selected.Output.Type == "servo" {
|
||||
mode = roverd.FirmataPinModeServo
|
||||
}
|
||||
if err := client.SetPinMode(byte(*selected.Output.Pin), mode); err != nil {
|
||||
return err
|
||||
}
|
||||
return client.ExtendedAnalog(byte(*selected.Output.Pin), int(number))
|
||||
default:
|
||||
return fmt.Errorf("unsupported output %q", selected.Output.Type)
|
||||
}
|
||||
}
|
||||
@@ -37,6 +37,15 @@ func main() {
|
||||
}
|
||||
defer serialPort.Close()
|
||||
|
||||
// Peripheral discovery is intentionally a boot-time operation. The manager
|
||||
// keeps successful USB ports open across server WebSocket reconnects and is
|
||||
// rebuilt only when the roverd process itself restarts.
|
||||
peripherals, err := roverd.DiscoverPeripheralManager(ctx, cfg.Serial.Device, logger)
|
||||
if err != nil {
|
||||
logger.Fatalf("discover rover peripherals: %v", err)
|
||||
}
|
||||
defer peripherals.Close()
|
||||
|
||||
var pulser *roverd.BRCPulser
|
||||
if cfg.BRC.Enabled() {
|
||||
pulser, err = roverd.NewBRCPulser(cfg.BRC, logger)
|
||||
@@ -61,37 +70,18 @@ func main() {
|
||||
mediaSupervisor.Start(ctx)
|
||||
}
|
||||
|
||||
var cameraServo *roverd.CameraServo
|
||||
if cfg.CameraServo.Enabled {
|
||||
cameraServo, err = roverd.NewCameraServo(cfg.CameraServo, logger)
|
||||
if err != nil {
|
||||
logger.Fatalf("init camera servo: %v", err)
|
||||
}
|
||||
defer cameraServo.Close()
|
||||
}
|
||||
|
||||
var headlight *roverd.GPIOToggle
|
||||
if cfg.Headlight.Enabled {
|
||||
headlight, err = roverd.NewGPIOToggle("headlight", cfg.Headlight, logger)
|
||||
if err != nil {
|
||||
logger.Fatalf("init headlight: %v", err)
|
||||
}
|
||||
defer headlight.Close()
|
||||
}
|
||||
|
||||
var laser *roverd.GPIOToggle
|
||||
if cfg.Laser.Enabled {
|
||||
laser, err = roverd.NewGPIOToggle("laser", cfg.Laser, logger)
|
||||
if err != nil {
|
||||
logger.Fatalf("init laser: %v", err)
|
||||
}
|
||||
defer laser.Close()
|
||||
// Backend selection is identical on Pi and laptop hosts: enabled native
|
||||
// GPIO wins, otherwise a discovered ESP32 may provide the built-in role.
|
||||
hardwareControllers, err := roverd.ResolveRoverHardwareControllers(cfg, peripherals, logger)
|
||||
if err != nil {
|
||||
logger.Fatalf("resolve rover hardware controllers: %v", err)
|
||||
}
|
||||
defer hardwareControllers.Close()
|
||||
|
||||
autoCharge := roverd.NewAutoChargeController(adapter, eventStream, logger)
|
||||
go autoCharge.Run(ctx, sensorSamples)
|
||||
|
||||
client := roverd.NewWSClient(cfg, adapter, sensorFrames, eventStream, mediaSupervisor, cameraServo, headlight, laser, logger, console)
|
||||
client := roverd.NewWSClient(cfg, adapter, sensorFrames, eventStream, mediaSupervisor, hardwareControllers.CameraServo, hardwareControllers.Headlight, hardwareControllers.Laser, peripherals, logger, console)
|
||||
|
||||
// Startup is announced only after every configured hardware dependency has
|
||||
// initialized successfully. A message here therefore means the control loop
|
||||
|
||||
+23
-13
@@ -1,19 +1,22 @@
|
||||
package roverd
|
||||
|
||||
import "encoding/json"
|
||||
|
||||
type helloMessage struct {
|
||||
Type string `json:"type"`
|
||||
Name string `json:"name"`
|
||||
Description string `json:"description,omitempty"`
|
||||
Color string `json:"color,omitempty"`
|
||||
Battery BatteryConfig `json:"battery"`
|
||||
MaxWheelSpeed int `json:"maxWheelSpeed"`
|
||||
Media MediaConfig `json:"media"`
|
||||
CameraServo CameraServoConfig `json:"cameraServo"`
|
||||
Audio AudioConfig `json:"audio"`
|
||||
Horn HornConfig `json:"horn"`
|
||||
Headlight GPIOToggleConfig `json:"headlight"`
|
||||
Laser GPIOToggleConfig `json:"laser"`
|
||||
Private PrivateConfig `json:"private"`
|
||||
Type string `json:"type"`
|
||||
Name string `json:"name"`
|
||||
Description string `json:"description,omitempty"`
|
||||
Color string `json:"color,omitempty"`
|
||||
Battery BatteryConfig `json:"battery"`
|
||||
MaxWheelSpeed int `json:"maxWheelSpeed"`
|
||||
Media MediaConfig `json:"media"`
|
||||
CameraServo CameraServoConfig `json:"cameraServo"`
|
||||
Audio AudioConfig `json:"audio"`
|
||||
Horn HornConfig `json:"horn"`
|
||||
Headlight GPIOToggleConfig `json:"headlight"`
|
||||
Laser GPIOToggleConfig `json:"laser"`
|
||||
Peripherals []RoverPeripheralMetadata `json:"peripherals,omitempty"`
|
||||
Private PrivateConfig `json:"private"`
|
||||
}
|
||||
|
||||
type sensorMessage struct {
|
||||
@@ -45,6 +48,7 @@ type inboundMessage struct {
|
||||
AudioLevels *audioLevelsPayload `json:"audioLevels,omitempty"`
|
||||
Headlight *togglePayload `json:"headlight,omitempty"`
|
||||
Laser *togglePayload `json:"laser,omitempty"`
|
||||
Peripheral *peripheralPayload `json:"peripheral,omitempty"`
|
||||
Song *songPayload `json:"song,omitempty"`
|
||||
Reboot *rebootPayload `json:"reboot,omitempty"`
|
||||
// Update is intentionally just a marker payload. The server can request the
|
||||
@@ -103,6 +107,12 @@ type togglePayload struct {
|
||||
Action string `json:"action"`
|
||||
}
|
||||
|
||||
type peripheralPayload struct {
|
||||
ID string `json:"id"`
|
||||
Control string `json:"control"`
|
||||
Value json.RawMessage `json:"value"`
|
||||
}
|
||||
|
||||
type songPayload struct {
|
||||
Slot *int `json:"slot,omitempty"`
|
||||
Notes []songNote `json:"notes"`
|
||||
|
||||
@@ -0,0 +1,628 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"fmt"
|
||||
"io"
|
||||
"sync"
|
||||
)
|
||||
|
||||
// Firmata command and mode constants are kept here instead of scattering raw
|
||||
// bytes through the peripheral code. The values come directly from the Firmata
|
||||
// protocol, so captures from a rover can be compared with the specification.
|
||||
const (
|
||||
firmataReportVersion byte = 0xF9
|
||||
firmataSetPinMode byte = 0xF4
|
||||
firmataSetDigitalPin byte = 0xF5
|
||||
firmataStartSysex byte = 0xF0
|
||||
firmataEndSysex byte = 0xF7
|
||||
firmataReportFirmware byte = 0x79
|
||||
firmataCapabilityQuery byte = 0x6B
|
||||
firmataCapabilityReply byte = 0x6C
|
||||
firmataExtendedAnalog byte = 0x6F
|
||||
firmataServoConfig byte = 0x70
|
||||
firmataPeripheralFeature byte = 0x01
|
||||
|
||||
firmataPeripheralDescribe byte = 0x00
|
||||
firmataPeripheralDescription byte = 0x01
|
||||
firmataPeripheralControl byte = 0x02
|
||||
firmataMaximumSysexDataBytes = 252
|
||||
|
||||
FirmataPinModeOutput byte = 0x01
|
||||
FirmataPinModePWM byte = 0x03
|
||||
FirmataPinModeServo byte = 0x04
|
||||
)
|
||||
|
||||
// FirmataMessage is the transport-neutral result of parsing one complete
|
||||
// Firmata message. For SysEx messages Command is the SysEx feature byte and
|
||||
// Data is everything between that feature byte and END_SYSEX.
|
||||
type FirmataMessage struct {
|
||||
Command byte
|
||||
Data []byte
|
||||
Sysex bool
|
||||
}
|
||||
|
||||
// FirmataParser incrementally parses a byte stream. USB serial reads may split
|
||||
// a message anywhere or combine several messages, so parsing whole Read calls
|
||||
// as though they were packets would intermittently corrupt valid traffic.
|
||||
type FirmataParser struct {
|
||||
inSysex bool
|
||||
sysex []byte
|
||||
command byte
|
||||
data []byte
|
||||
expected int
|
||||
}
|
||||
|
||||
// Feed accepts any fragment of the serial stream and returns every complete
|
||||
// message found in it, preserving wire order.
|
||||
func (p *FirmataParser) Feed(fragment []byte) ([]FirmataMessage, error) {
|
||||
var messages []FirmataMessage
|
||||
|
||||
for _, value := range fragment {
|
||||
if p.inSysex {
|
||||
switch {
|
||||
case value == firmataEndSysex:
|
||||
if len(p.sysex) == 0 {
|
||||
p.resetSysex()
|
||||
return messages, errors.New("Firmata SysEx message is missing a feature byte")
|
||||
}
|
||||
messages = append(messages, FirmataMessage{
|
||||
Command: p.sysex[0],
|
||||
Data: append([]byte(nil), p.sysex[1:]...),
|
||||
Sysex: true,
|
||||
})
|
||||
p.resetSysex()
|
||||
case value&0x80 != 0:
|
||||
// Bytes inside SysEx must be seven-bit clean. Reset immediately so
|
||||
// a damaged frame cannot consume every later message on the port.
|
||||
p.resetSysex()
|
||||
return messages, fmt.Errorf("invalid 8-bit value 0x%02x inside Firmata SysEx", value)
|
||||
default:
|
||||
p.sysex = append(p.sysex, value)
|
||||
}
|
||||
continue
|
||||
}
|
||||
|
||||
if value == firmataStartSysex {
|
||||
p.inSysex = true
|
||||
p.sysex = p.sysex[:0]
|
||||
p.resetFixed()
|
||||
continue
|
||||
}
|
||||
|
||||
if value&0x80 != 0 {
|
||||
p.command = value
|
||||
p.data = p.data[:0]
|
||||
p.expected = firmataDataLength(value)
|
||||
if p.expected == 0 {
|
||||
messages = append(messages, FirmataMessage{Command: value})
|
||||
p.resetFixed()
|
||||
}
|
||||
continue
|
||||
}
|
||||
|
||||
// Stray data before a status byte is harmless serial noise. Firmata
|
||||
// has no framing information that could assign it to a command.
|
||||
if p.expected == 0 {
|
||||
continue
|
||||
}
|
||||
p.data = append(p.data, value)
|
||||
if len(p.data) == p.expected {
|
||||
messages = append(messages, FirmataMessage{
|
||||
Command: p.command,
|
||||
Data: append([]byte(nil), p.data...),
|
||||
})
|
||||
p.resetFixed()
|
||||
}
|
||||
}
|
||||
|
||||
return messages, nil
|
||||
}
|
||||
|
||||
func (p *FirmataParser) resetSysex() {
|
||||
p.inSysex = false
|
||||
p.sysex = p.sysex[:0]
|
||||
}
|
||||
|
||||
func (p *FirmataParser) resetFixed() {
|
||||
p.command = 0
|
||||
p.data = p.data[:0]
|
||||
p.expected = 0
|
||||
}
|
||||
|
||||
// firmataDataLength returns the number of seven-bit data bytes used by the
|
||||
// fixed-length messages relevant to normal Firmata traffic. Unknown system
|
||||
// commands are treated as single-byte messages so they cannot stall parsing of
|
||||
// the rover-peripheral SysEx frames that follow them.
|
||||
func firmataDataLength(command byte) int {
|
||||
switch command {
|
||||
case firmataReportVersion, firmataSetPinMode, firmataSetDigitalPin:
|
||||
return 2
|
||||
}
|
||||
|
||||
switch command & 0xF0 {
|
||||
case 0x80, 0x90, 0xA0, 0xE0:
|
||||
return 2
|
||||
case 0xC0, 0xD0:
|
||||
return 1
|
||||
default:
|
||||
return 0
|
||||
}
|
||||
}
|
||||
|
||||
// EncodeFirmata7Bit converts arbitrary bytes into the two-byte representation
|
||||
// required inside Firmata SysEx. Keeping this transform below the JSON layer
|
||||
// means firmware authors and UI code never need to think about wire encoding.
|
||||
func EncodeFirmata7Bit(raw []byte) []byte {
|
||||
encoded := make([]byte, 0, len(raw)*2)
|
||||
for _, value := range raw {
|
||||
encoded = append(encoded, value&0x7F, (value>>7)&0x01)
|
||||
}
|
||||
return encoded
|
||||
}
|
||||
|
||||
// DecodeFirmata7Bit reverses EncodeFirmata7Bit and rejects malformed pairs.
|
||||
func DecodeFirmata7Bit(encoded []byte) ([]byte, error) {
|
||||
if len(encoded)%2 != 0 {
|
||||
return nil, fmt.Errorf("Firmata 7-bit payload has odd length %d", len(encoded))
|
||||
}
|
||||
|
||||
decoded := make([]byte, 0, len(encoded)/2)
|
||||
for index := 0; index < len(encoded); index += 2 {
|
||||
low, high := encoded[index], encoded[index+1]
|
||||
if low&0x80 != 0 || high > 1 {
|
||||
return nil, fmt.Errorf("invalid Firmata 7-bit pair at byte %d", index)
|
||||
}
|
||||
decoded = append(decoded, low|(high<<7))
|
||||
}
|
||||
return decoded, nil
|
||||
}
|
||||
|
||||
// PeripheralDescription is generated by the ESP32 at boot. Controls is a slice
|
||||
// intentionally: registration order is part of the UI contract and must never
|
||||
// be replaced by map iteration or alphabetical sorting.
|
||||
type PeripheralDescription struct {
|
||||
Name string `json:"name"`
|
||||
RoverControls PeripheralRoverControls `json:"roverControls,omitempty"`
|
||||
Controls []PeripheralControl `json:"controls"`
|
||||
}
|
||||
|
||||
type PeripheralRoverControls struct {
|
||||
CameraServo *PeripheralCameraServo `json:"cameraServo,omitempty"`
|
||||
Headlight *PeripheralDigitalRole `json:"headlight,omitempty"`
|
||||
Laser *PeripheralDigitalRole `json:"laser,omitempty"`
|
||||
}
|
||||
|
||||
type PeripheralCameraServo struct {
|
||||
Pin int `json:"pin"`
|
||||
MinimumAngleDegrees float64 `json:"minimumAngleDegrees"`
|
||||
MaximumAngleDegrees float64 `json:"maximumAngleDegrees"`
|
||||
HomeAngleDegrees float64 `json:"homeAngleDegrees"`
|
||||
NudgeDegrees float64 `json:"nudgeDegrees"`
|
||||
MinimumPulseMicroseconds int `json:"minimumPulseMicroseconds"`
|
||||
MaximumPulseMicroseconds int `json:"maximumPulseMicroseconds"`
|
||||
AllowRawPulse bool `json:"allowRawPulse"`
|
||||
Inverted bool `json:"inverted"`
|
||||
}
|
||||
|
||||
type PeripheralDigitalRole struct {
|
||||
Pin int `json:"pin"`
|
||||
ActiveLow bool `json:"activeLow"`
|
||||
InitiallyOn bool `json:"initiallyOn"`
|
||||
}
|
||||
|
||||
type PeripheralControl struct {
|
||||
ID string `json:"id"`
|
||||
Type string `json:"type"`
|
||||
Name string `json:"name"`
|
||||
Mode string `json:"mode,omitempty"`
|
||||
Minimum *int `json:"min,omitempty"`
|
||||
Maximum *int `json:"max,omitempty"`
|
||||
MaximumLength *int `json:"maxLength,omitempty"`
|
||||
Output PeripheralOutput `json:"output"`
|
||||
}
|
||||
|
||||
type PeripheralOutput struct {
|
||||
Type string `json:"type"`
|
||||
Pin *int `json:"pin,omitempty"`
|
||||
ActiveLow bool `json:"activeLow,omitempty"`
|
||||
}
|
||||
|
||||
// Validate catches authoring mistakes at connection time, where the error can
|
||||
// name the offending peripheral, instead of allowing a malformed declaration
|
||||
// to turn into a confusing no-op later when a driver uses the control.
|
||||
func (description PeripheralDescription) Validate() error {
|
||||
if description.Name == "" {
|
||||
return errors.New("peripheral description requires a name")
|
||||
}
|
||||
if camera := description.RoverControls.CameraServo; camera != nil {
|
||||
if err := validateFirmataPin("cameraServo", camera.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
if camera.MinimumAngleDegrees >= camera.MaximumAngleDegrees {
|
||||
return errors.New("cameraServo angle range must be increasing")
|
||||
}
|
||||
if camera.HomeAngleDegrees < camera.MinimumAngleDegrees || camera.HomeAngleDegrees > camera.MaximumAngleDegrees {
|
||||
return errors.New("cameraServo home angle must be inside its angle range")
|
||||
}
|
||||
if camera.NudgeDegrees <= 0 {
|
||||
return errors.New("cameraServo nudge must be positive")
|
||||
}
|
||||
if camera.MinimumPulseMicroseconds <= 0 || camera.MaximumPulseMicroseconds <= camera.MinimumPulseMicroseconds {
|
||||
return errors.New("cameraServo pulse range must be positive and increasing")
|
||||
}
|
||||
}
|
||||
if role := description.RoverControls.Headlight; role != nil {
|
||||
if err := validateFirmataPin("headlight", role.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
if role := description.RoverControls.Laser; role != nil {
|
||||
if err := validateFirmataPin("laser", role.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
|
||||
seen := make(map[string]struct{}, len(description.Controls))
|
||||
for index, control := range description.Controls {
|
||||
if control.ID == "" || control.Name == "" {
|
||||
return fmt.Errorf("control %d requires both id and name", index)
|
||||
}
|
||||
if _, exists := seen[control.ID]; exists {
|
||||
return fmt.Errorf("control id %q is duplicated", control.ID)
|
||||
}
|
||||
seen[control.ID] = struct{}{}
|
||||
|
||||
switch control.Type {
|
||||
case "slider", "number":
|
||||
if control.Minimum == nil || control.Maximum == nil || *control.Minimum > *control.Maximum {
|
||||
return fmt.Errorf("control %q requires a valid min and max", control.ID)
|
||||
}
|
||||
case "button":
|
||||
if control.Mode != "toggle" && control.Mode != "momentary" {
|
||||
return fmt.Errorf("button %q requires toggle or momentary mode", control.ID)
|
||||
}
|
||||
case "text":
|
||||
if control.MaximumLength == nil || *control.MaximumLength <= 0 {
|
||||
return fmt.Errorf("text control %q requires a positive maxLength", control.ID)
|
||||
}
|
||||
default:
|
||||
return fmt.Errorf("control %q has unsupported type %q", control.ID, control.Type)
|
||||
}
|
||||
|
||||
switch control.Output.Type {
|
||||
case "digital":
|
||||
if control.Output.Pin == nil {
|
||||
return fmt.Errorf("control %q output %q requires a pin", control.ID, control.Output.Type)
|
||||
}
|
||||
if err := validateFirmataPin("control "+control.ID, *control.Output.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
if control.Type != "button" {
|
||||
return fmt.Errorf("digital output control %q must be a button", control.ID)
|
||||
}
|
||||
case "pwm", "servo":
|
||||
if control.Output.Pin == nil {
|
||||
return fmt.Errorf("control %q output %q requires a pin", control.ID, control.Output.Type)
|
||||
}
|
||||
if err := validateFirmataPin("control "+control.ID, *control.Output.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
if control.Type != "slider" && control.Type != "number" {
|
||||
return fmt.Errorf("%s output control %q must be a slider or number", control.Output.Type, control.ID)
|
||||
}
|
||||
case "custom":
|
||||
default:
|
||||
return fmt.Errorf("control %q has unsupported output %q", control.ID, control.Output.Type)
|
||||
}
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func validateFirmataPin(owner string, pin int) error {
|
||||
// Firmata represents pin numbers with one seven-bit byte. Rejecting values
|
||||
// outside that wire range avoids silently wrapping a declaration when it is
|
||||
// converted to a byte for output commands.
|
||||
if pin < 0 || pin > 127 {
|
||||
return fmt.Errorf("%s pin must be between 0 and 127", owner)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
// FirmataFirmware identifies the implementation answering the standard
|
||||
// REPORT_FIRMWARE query. It is diagnostic metadata, not a protocol gate.
|
||||
type FirmataFirmware struct {
|
||||
Major int
|
||||
Minor int
|
||||
Name string
|
||||
}
|
||||
|
||||
// FirmataPinCapability is one mode/resolution pair from CAPABILITY_RESPONSE.
|
||||
type FirmataPinCapability struct {
|
||||
Mode byte
|
||||
Resolution byte
|
||||
}
|
||||
|
||||
// FirmataClient owns one already-open serial connection. Its reader goroutine
|
||||
// separates arbitrary USB read boundaries from request/response handling while
|
||||
// writeMu prevents two commands from interleaving on the byte stream.
|
||||
type FirmataClient struct {
|
||||
connection io.ReadWriteCloser
|
||||
parser FirmataParser
|
||||
messages chan FirmataMessage
|
||||
errors chan error
|
||||
writeMu sync.Mutex
|
||||
requestMu sync.Mutex
|
||||
stateMu sync.RWMutex
|
||||
terminalErr error
|
||||
}
|
||||
|
||||
func NewFirmataClient(connection io.ReadWriteCloser) *FirmataClient {
|
||||
return &FirmataClient{
|
||||
connection: connection,
|
||||
messages: make(chan FirmataMessage, 16),
|
||||
errors: make(chan error, 1),
|
||||
}
|
||||
}
|
||||
|
||||
// Start begins consuming the serial stream. The caller still owns the port and
|
||||
// closes it during shutdown; this makes the client usable with both real serial
|
||||
// ports and deterministic in-memory test connections.
|
||||
func (client *FirmataClient) Start(ctx context.Context) {
|
||||
go client.readLoop(ctx)
|
||||
}
|
||||
|
||||
func (client *FirmataClient) readLoop(ctx context.Context) {
|
||||
buffer := make([]byte, 256)
|
||||
for {
|
||||
count, err := client.connection.Read(buffer)
|
||||
if count > 0 {
|
||||
messages, parseErr := client.parser.Feed(buffer[:count])
|
||||
if parseErr != nil {
|
||||
client.publishError(ctx, parseErr)
|
||||
return
|
||||
}
|
||||
for _, message := range messages {
|
||||
select {
|
||||
case client.messages <- message:
|
||||
case <-ctx.Done():
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
if err != nil {
|
||||
if errors.Is(err, io.EOF) {
|
||||
// tarm/serial represents an ordinary ReadTimeout with io.EOF. A
|
||||
// Firmata connection is expected to be quiet between commands, so
|
||||
// treating that timeout as a closed device kills the reader before
|
||||
// the next request can receive its reply. A real USB removal is
|
||||
// reported by the serial driver as a non-EOF error.
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
default:
|
||||
continue
|
||||
}
|
||||
}
|
||||
client.publishError(ctx, err)
|
||||
return
|
||||
}
|
||||
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (client *FirmataClient) publishError(ctx context.Context, err error) {
|
||||
client.stateMu.Lock()
|
||||
if client.terminalErr == nil {
|
||||
client.terminalErr = err
|
||||
}
|
||||
client.stateMu.Unlock()
|
||||
|
||||
select {
|
||||
case client.errors <- err:
|
||||
case <-ctx.Done():
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
func (client *FirmataClient) write(message []byte) error {
|
||||
client.writeMu.Lock()
|
||||
defer client.writeMu.Unlock()
|
||||
client.stateMu.RLock()
|
||||
terminalErr := client.terminalErr
|
||||
client.stateMu.RUnlock()
|
||||
if terminalErr != nil {
|
||||
return fmt.Errorf("Firmata connection unavailable: %w", terminalErr)
|
||||
}
|
||||
|
||||
written, err := client.connection.Write(message)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
if written != len(message) {
|
||||
return fmt.Errorf("short Firmata write %d/%d", written, len(message))
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (client *FirmataClient) writeSysex(command byte, data []byte) error {
|
||||
message := make([]byte, 0, len(data)+3)
|
||||
message = append(message, firmataStartSysex, command)
|
||||
message = append(message, data...)
|
||||
message = append(message, firmataEndSysex)
|
||||
return client.write(message)
|
||||
}
|
||||
|
||||
func (client *FirmataClient) waitFor(ctx context.Context, match func(FirmataMessage) bool) (FirmataMessage, error) {
|
||||
for {
|
||||
select {
|
||||
case message := <-client.messages:
|
||||
if match(message) {
|
||||
return message, nil
|
||||
}
|
||||
case err := <-client.errors:
|
||||
return FirmataMessage{}, err
|
||||
case <-ctx.Done():
|
||||
return FirmataMessage{}, ctx.Err()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (client *FirmataClient) QueryFirmware(ctx context.Context) (FirmataFirmware, error) {
|
||||
client.requestMu.Lock()
|
||||
defer client.requestMu.Unlock()
|
||||
|
||||
if err := client.writeSysex(firmataReportFirmware, nil); err != nil {
|
||||
return FirmataFirmware{}, err
|
||||
}
|
||||
message, err := client.waitFor(ctx, func(message FirmataMessage) bool {
|
||||
return message.Sysex && message.Command == firmataReportFirmware
|
||||
})
|
||||
if err != nil {
|
||||
return FirmataFirmware{}, err
|
||||
}
|
||||
if len(message.Data) < 2 {
|
||||
return FirmataFirmware{}, errors.New("Firmata firmware response is missing version bytes")
|
||||
}
|
||||
name, err := DecodeFirmata7Bit(message.Data[2:])
|
||||
if err != nil {
|
||||
return FirmataFirmware{}, fmt.Errorf("decode Firmata firmware name: %w", err)
|
||||
}
|
||||
return FirmataFirmware{Major: int(message.Data[0]), Minor: int(message.Data[1]), Name: string(name)}, nil
|
||||
}
|
||||
|
||||
func (client *FirmataClient) QueryCapabilities(ctx context.Context) ([][]FirmataPinCapability, error) {
|
||||
client.requestMu.Lock()
|
||||
defer client.requestMu.Unlock()
|
||||
|
||||
if err := client.writeSysex(firmataCapabilityQuery, nil); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
message, err := client.waitFor(ctx, func(message FirmataMessage) bool {
|
||||
return message.Sysex && message.Command == firmataCapabilityReply
|
||||
})
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
return parseFirmataCapabilities(message.Data)
|
||||
}
|
||||
|
||||
func parseFirmataCapabilities(data []byte) ([][]FirmataPinCapability, error) {
|
||||
var pins [][]FirmataPinCapability
|
||||
var pin []FirmataPinCapability
|
||||
for index := 0; index < len(data); {
|
||||
if data[index] == 0x7F {
|
||||
pins = append(pins, pin)
|
||||
pin = nil
|
||||
index++
|
||||
continue
|
||||
}
|
||||
if index+1 >= len(data) {
|
||||
return nil, errors.New("Firmata capability response ends inside a mode pair")
|
||||
}
|
||||
pin = append(pin, FirmataPinCapability{Mode: data[index], Resolution: data[index+1]})
|
||||
index += 2
|
||||
}
|
||||
if pin != nil {
|
||||
return nil, errors.New("Firmata capability response is missing its final pin separator")
|
||||
}
|
||||
return pins, nil
|
||||
}
|
||||
|
||||
func (client *FirmataClient) Describe(ctx context.Context) (PeripheralDescription, error) {
|
||||
client.requestMu.Lock()
|
||||
defer client.requestMu.Unlock()
|
||||
|
||||
if err := client.writeSysex(firmataPeripheralFeature, []byte{firmataPeripheralDescribe}); err != nil {
|
||||
return PeripheralDescription{}, err
|
||||
}
|
||||
message, err := client.waitFor(ctx, func(message FirmataMessage) bool {
|
||||
return message.Sysex && message.Command == firmataPeripheralFeature && len(message.Data) > 0 && message.Data[0] == firmataPeripheralDescription
|
||||
})
|
||||
if err != nil {
|
||||
return PeripheralDescription{}, err
|
||||
}
|
||||
|
||||
raw, err := DecodeFirmata7Bit(message.Data[1:])
|
||||
if err != nil {
|
||||
return PeripheralDescription{}, fmt.Errorf("decode peripheral description: %w", err)
|
||||
}
|
||||
var description PeripheralDescription
|
||||
if err := json.Unmarshal(raw, &description); err != nil {
|
||||
return PeripheralDescription{}, fmt.Errorf("parse peripheral description: %w", err)
|
||||
}
|
||||
if err := description.Validate(); err != nil {
|
||||
return PeripheralDescription{}, fmt.Errorf("validate peripheral description: %w", err)
|
||||
}
|
||||
return description, nil
|
||||
}
|
||||
|
||||
func (client *FirmataClient) SetPinMode(pin, mode byte) error {
|
||||
return client.write([]byte{firmataSetPinMode, pin & 0x7F, mode & 0x7F})
|
||||
}
|
||||
|
||||
func (client *FirmataClient) SetDigitalPin(pin byte, enabled bool) error {
|
||||
value := byte(0)
|
||||
if enabled {
|
||||
value = 1
|
||||
}
|
||||
return client.write([]byte{firmataSetDigitalPin, pin & 0x7F, value})
|
||||
}
|
||||
|
||||
func (client *FirmataClient) ExtendedAnalog(pin byte, value int) error {
|
||||
if value < 0 {
|
||||
return fmt.Errorf("Firmata analog value cannot be negative: %d", value)
|
||||
}
|
||||
|
||||
payload := []byte{pin & 0x7F}
|
||||
// Firmata encodes integers as many seven-bit chunks as necessary. Zero
|
||||
// still needs one value byte so the receiver can distinguish it from a
|
||||
// message that contains only the pin.
|
||||
for {
|
||||
payload = append(payload, byte(value&0x7F))
|
||||
value >>= 7
|
||||
if value == 0 {
|
||||
break
|
||||
}
|
||||
}
|
||||
return client.writeSysex(firmataExtendedAnalog, payload)
|
||||
}
|
||||
|
||||
func (client *FirmataClient) ConfigureServo(pin byte, minimumPulseMicroseconds, maximumPulseMicroseconds int) error {
|
||||
if minimumPulseMicroseconds <= 0 || maximumPulseMicroseconds <= minimumPulseMicroseconds {
|
||||
return errors.New("servo pulse range must be positive and increasing")
|
||||
}
|
||||
payload := []byte{
|
||||
pin & 0x7F,
|
||||
byte(minimumPulseMicroseconds & 0x7F), byte((minimumPulseMicroseconds >> 7) & 0x7F),
|
||||
byte(maximumPulseMicroseconds & 0x7F), byte((maximumPulseMicroseconds >> 7) & 0x7F),
|
||||
}
|
||||
return client.writeSysex(firmataServoConfig, payload)
|
||||
}
|
||||
|
||||
func (client *FirmataClient) SendPeripheralControl(controlID string, value any) error {
|
||||
payload, err := json.Marshal(struct {
|
||||
Control string `json:"control"`
|
||||
Value any `json:"value"`
|
||||
}{Control: controlID, Value: value})
|
||||
if err != nil {
|
||||
return fmt.Errorf("encode peripheral control: %w", err)
|
||||
}
|
||||
data := append([]byte{firmataPeripheralControl}, EncodeFirmata7Bit(payload)...)
|
||||
// ConfigurableFirmata on ESP32 stores at most 252 bytes including the SysEx
|
||||
// feature byte. Refuse a value that the board would otherwise discard as an
|
||||
// incomplete frame; this is a transport constraint, not an application-level
|
||||
// text policy.
|
||||
if len(data)+1 > firmataMaximumSysexDataBytes {
|
||||
return fmt.Errorf("peripheral control needs %d SysEx data bytes; Firmata accepts at most %d", len(data)+1, firmataMaximumSysexDataBytes)
|
||||
}
|
||||
return client.writeSysex(firmataPeripheralFeature, data)
|
||||
}
|
||||
@@ -0,0 +1,286 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"math"
|
||||
"strings"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
// FirmataCameraServo preserves the established logical camera movement model
|
||||
// while replacing only the final physical write. The ESP32 receives ordinary
|
||||
// Firmata servo configuration and angle messages, regardless of rover host.
|
||||
type FirmataCameraServo struct {
|
||||
cfg CameraServoConfig
|
||||
client *FirmataClient
|
||||
pin byte
|
||||
mu sync.Mutex
|
||||
currentAngle float64
|
||||
desiredAngle float64
|
||||
lastMove time.Time
|
||||
moving bool
|
||||
stopCh chan struct{}
|
||||
closed bool
|
||||
}
|
||||
|
||||
func newFirmataCameraServo(peripheral *managedPeripheral, declaration PeripheralCameraServo, logger *log.Logger) (*FirmataCameraServo, error) {
|
||||
cfg := CameraServoConfig{
|
||||
Enabled: true,
|
||||
Pin: declaration.Pin,
|
||||
FreqHz: 50,
|
||||
CycleLen: 20000,
|
||||
MinPulseUs: declaration.MinimumPulseMicroseconds,
|
||||
MaxPulseUs: declaration.MaximumPulseMicroseconds,
|
||||
MinAngle: declaration.MinimumAngleDegrees,
|
||||
MaxAngle: declaration.MaximumAngleDegrees,
|
||||
HomeAngle: declaration.HomeAngleDegrees,
|
||||
NudgeDegrees: declaration.NudgeDegrees,
|
||||
AllowRawPulse: declaration.AllowRawPulse,
|
||||
Invert: declaration.Inverted,
|
||||
}
|
||||
servo := &FirmataCameraServo{
|
||||
cfg: cfg,
|
||||
client: peripheral.client,
|
||||
pin: byte(declaration.Pin),
|
||||
stopCh: make(chan struct{}),
|
||||
}
|
||||
|
||||
// SERVO_CONFIG establishes the peripheral-owned pulse calibration before
|
||||
// selecting servo mode. This is standard Firmata, not a rover extension.
|
||||
if err := servo.client.ConfigureServo(servo.pin, cfg.MinPulseUs, cfg.MaxPulseUs); err != nil {
|
||||
return nil, fmt.Errorf("configure Firmata servo: %w", err)
|
||||
}
|
||||
if err := servo.client.SetPinMode(servo.pin, FirmataPinModeServo); err != nil {
|
||||
return nil, fmt.Errorf("select Firmata servo mode: %w", err)
|
||||
}
|
||||
if err := servo.setAngleLocked(cfg.HomeAngle); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
logger.Printf("camera servo using ESP32 %s pin %d (%.1f..%.1f deg)", peripheral.metadata.ID, declaration.Pin, cfg.MinAngle, cfg.MaxAngle)
|
||||
return servo, nil
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) SetAngle(angle float64) error {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
return servo.setAngleLocked(angle)
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) setAngleLocked(angle float64) error {
|
||||
if servo.closed {
|
||||
return errorsNewControllerClosed("camera servo")
|
||||
}
|
||||
servo.desiredAngle = clampFloat(angle, servo.cfg.MinAngle, servo.cfg.MaxAngle)
|
||||
limited := servo.rateLimitAngleLocked(servo.desiredAngle)
|
||||
if err := servo.writeAngleLocked(limited); err != nil {
|
||||
return err
|
||||
}
|
||||
servo.currentAngle = limited
|
||||
if math.Abs(limited-servo.desiredAngle) > servoAngleEpsilon {
|
||||
servo.startMoveLoopLocked()
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) Nudge(delta float64) error {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
if delta == 0 {
|
||||
delta = servo.cfg.NudgeDegrees
|
||||
}
|
||||
return servo.setAngleLocked(servo.currentAngle + delta)
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) SetPulseWidth(micros int) error {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
if !servo.cfg.AllowRawPulse {
|
||||
return fmt.Errorf("raw pulse commands disabled")
|
||||
}
|
||||
if micros <= 0 {
|
||||
return fmt.Errorf("pulse width must be > 0")
|
||||
}
|
||||
pulse := clampInt(micros, servo.cfg.MinPulseUs, servo.cfg.MaxPulseUs)
|
||||
return servo.setAngleLocked(servo.pulseToAngle(pulse))
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) CurrentAngle() float64 {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
return servo.currentAngle
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) Configuration() CameraServoConfig {
|
||||
return servo.cfg
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) Close() {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
if servo.closed {
|
||||
return
|
||||
}
|
||||
// Returning home matches the native Pi implementation. Any write failure is
|
||||
// ignored during shutdown because the serial connection may already be gone.
|
||||
_ = servo.writeAngleLocked(servo.cfg.HomeAngle)
|
||||
close(servo.stopCh)
|
||||
servo.closed = true
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) writeAngleLocked(angle float64) error {
|
||||
rangeDegrees := servo.cfg.MaxAngle - servo.cfg.MinAngle
|
||||
normalized := (angle - servo.cfg.MinAngle) / rangeDegrees
|
||||
normalized = math.Max(0, math.Min(1, normalized))
|
||||
if servo.cfg.Invert {
|
||||
normalized = 1 - normalized
|
||||
}
|
||||
// Standard Firmata servo values are positions from 0 through 180. Pulse
|
||||
// calibration was already supplied through SERVO_CONFIG above.
|
||||
position := int(math.Round(normalized * 180))
|
||||
return servo.client.ExtendedAnalog(servo.pin, position)
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) pulseToAngle(pulse int) float64 {
|
||||
normalized := float64(pulse-servo.cfg.MinPulseUs) / float64(servo.cfg.MaxPulseUs-servo.cfg.MinPulseUs)
|
||||
if servo.cfg.Invert {
|
||||
normalized = 1 - normalized
|
||||
}
|
||||
return servo.cfg.MinAngle + normalized*(servo.cfg.MaxAngle-servo.cfg.MinAngle)
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) rateLimitAngleLocked(target float64) float64 {
|
||||
now := time.Now()
|
||||
if servo.lastMove.IsZero() {
|
||||
servo.lastMove = now
|
||||
}
|
||||
elapsed := now.Sub(servo.lastMove).Seconds()
|
||||
if elapsed > servoStepInterval.Seconds() {
|
||||
elapsed = servoStepInterval.Seconds()
|
||||
}
|
||||
maximumDelta := maxServoDegPerSec * elapsed
|
||||
delta := target - servo.currentAngle
|
||||
if math.Abs(delta) <= maximumDelta {
|
||||
servo.lastMove = now
|
||||
return target
|
||||
}
|
||||
servo.lastMove = now
|
||||
if delta > 0 {
|
||||
return servo.currentAngle + maximumDelta
|
||||
}
|
||||
return servo.currentAngle - maximumDelta
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) startMoveLoopLocked() {
|
||||
if servo.moving || servo.closed {
|
||||
return
|
||||
}
|
||||
servo.moving = true
|
||||
go func() {
|
||||
ticker := time.NewTicker(servoStepInterval)
|
||||
defer ticker.Stop()
|
||||
for {
|
||||
select {
|
||||
case <-ticker.C:
|
||||
servo.mu.Lock()
|
||||
if servo.closed || math.Abs(servo.currentAngle-servo.desiredAngle) <= servoAngleEpsilon {
|
||||
servo.moving = false
|
||||
servo.mu.Unlock()
|
||||
return
|
||||
}
|
||||
limited := servo.rateLimitAngleLocked(servo.desiredAngle)
|
||||
if err := servo.writeAngleLocked(limited); err != nil {
|
||||
// A failed serial write makes further automatic steps pointless.
|
||||
// The next user command returns the connection error normally.
|
||||
servo.moving = false
|
||||
servo.mu.Unlock()
|
||||
return
|
||||
}
|
||||
servo.currentAngle = limited
|
||||
servo.mu.Unlock()
|
||||
case <-servo.stopCh:
|
||||
return
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
// FirmataToggle owns logical state exactly like GPIOToggle but sends the final
|
||||
// electrical level through Firmata's standard digital-pin command.
|
||||
type FirmataToggle struct {
|
||||
cfg GPIOToggleConfig
|
||||
name string
|
||||
client *FirmataClient
|
||||
pin byte
|
||||
mu sync.Mutex
|
||||
on bool
|
||||
closed bool
|
||||
}
|
||||
|
||||
func newFirmataToggle(name string, peripheral *managedPeripheral, declaration PeripheralDigitalRole, logger *log.Logger) (*FirmataToggle, error) {
|
||||
cfg := GPIOToggleConfig{Enabled: true, GPIOPin: declaration.Pin, InitialOn: declaration.InitiallyOn, ActiveLow: declaration.ActiveLow}
|
||||
toggle := &FirmataToggle{cfg: cfg, name: name, client: peripheral.client, pin: byte(declaration.Pin), on: cfg.InitialOn}
|
||||
if err := toggle.client.SetPinMode(toggle.pin, FirmataPinModeOutput); err != nil {
|
||||
return nil, fmt.Errorf("select Firmata output mode: %w", err)
|
||||
}
|
||||
if err := toggle.writeLocked(toggle.on); err != nil {
|
||||
return nil, fmt.Errorf("initialize Firmata output: %w", err)
|
||||
}
|
||||
logger.Printf("%s using ESP32 %s pin %d (initial=%v activeLow=%v)", name, peripheral.metadata.ID, declaration.Pin, cfg.InitialOn, cfg.ActiveLow)
|
||||
return toggle, nil
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) HandleAction(action string) error {
|
||||
toggle.mu.Lock()
|
||||
defer toggle.mu.Unlock()
|
||||
if toggle.closed {
|
||||
return errorsNewControllerClosed(toggle.name)
|
||||
}
|
||||
switch strings.ToLower(strings.TrimSpace(action)) {
|
||||
case "", "toggle":
|
||||
return toggle.setLocked(!toggle.on)
|
||||
case "on":
|
||||
return toggle.setLocked(true)
|
||||
case "off":
|
||||
return toggle.setLocked(false)
|
||||
default:
|
||||
return fmt.Errorf("unknown action %q", action)
|
||||
}
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) setLocked(on bool) error {
|
||||
if err := toggle.writeLocked(on); err != nil {
|
||||
return err
|
||||
}
|
||||
toggle.on = on
|
||||
return nil
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) writeLocked(on bool) error {
|
||||
physicalHigh := on
|
||||
if toggle.cfg.ActiveLow {
|
||||
physicalHigh = !physicalHigh
|
||||
}
|
||||
return toggle.client.SetDigitalPin(toggle.pin, physicalHigh)
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) On() bool {
|
||||
toggle.mu.Lock()
|
||||
defer toggle.mu.Unlock()
|
||||
return toggle.on
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) Configuration() GPIOToggleConfig {
|
||||
return toggle.cfg
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) Close() {
|
||||
toggle.mu.Lock()
|
||||
defer toggle.mu.Unlock()
|
||||
toggle.closed = true
|
||||
}
|
||||
|
||||
func errorsNewControllerClosed(name string) error {
|
||||
return fmt.Errorf("%s controller closed", name)
|
||||
}
|
||||
@@ -0,0 +1,153 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"context"
|
||||
"log"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestDisabledNativeRolesResolveToFirmataOnEveryHostBuild(t *testing.T) {
|
||||
description := PeripheralDescription{
|
||||
Name: "Rover GPIO",
|
||||
RoverControls: PeripheralRoverControls{
|
||||
CameraServo: &PeripheralCameraServo{
|
||||
Pin: 14, MinimumAngleDegrees: -15, MaximumAngleDegrees: 30,
|
||||
HomeAngleDegrees: 0, NudgeDegrees: 2,
|
||||
MinimumPulseMicroseconds: 900, MaximumPulseMicroseconds: 2100,
|
||||
},
|
||||
Headlight: &PeripheralDigitalRole{Pin: 18, ActiveLow: true, InitiallyOn: true},
|
||||
Laser: &PeripheralDigitalRole{Pin: 16, ActiveLow: false, InitiallyOn: false},
|
||||
},
|
||||
Controls: []PeripheralControl{},
|
||||
}
|
||||
connection := scriptedPeripheralConnection(t, description)
|
||||
manager, err := discoverPeripheralManager(
|
||||
context.Background(),
|
||||
"/dev/roomba",
|
||||
discardLogger(),
|
||||
testPeripheralDiscoveryDependencies([]string{"/dev/rover-gpio"}, map[string]*scriptedConnection{"/dev/rover-gpio": connection}),
|
||||
)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
// All native entries are disabled, exactly as they can be on either a Pi or
|
||||
// laptop rover. The shared resolver must therefore select every ESP32 role.
|
||||
baseline := len(connection.Bytes())
|
||||
controllers, err := ResolveRoverHardwareControllers(&Config{}, manager, discardLogger())
|
||||
if err != nil {
|
||||
t.Fatalf("resolve: %v", err)
|
||||
}
|
||||
defer controllers.Close()
|
||||
if controllers.CameraServo == nil || controllers.Headlight == nil || controllers.Laser == nil {
|
||||
t.Fatalf("missing Firmata controller: %#v", controllers)
|
||||
}
|
||||
if !controllers.CameraServo.Configuration().Enabled || !controllers.Headlight.Configuration().Enabled || !controllers.Laser.Configuration().Enabled {
|
||||
t.Fatal("ESP32-backed roles were not advertised as enabled")
|
||||
}
|
||||
|
||||
// Initialization uses only standard Firmata: servo calibration and mode,
|
||||
// followed by the home position and digital initial states. The active-low
|
||||
// headlight starts logically on, so its physical output is low.
|
||||
writes := connection.Bytes()[baseline:]
|
||||
wantPrefix := []byte{
|
||||
firmataStartSysex, firmataServoConfig, 14, 4, 7, 52, 16, firmataEndSysex,
|
||||
firmataSetPinMode, 14, FirmataPinModeServo,
|
||||
firmataStartSysex, firmataExtendedAnalog, 14, 60, firmataEndSysex,
|
||||
firmataSetPinMode, 18, FirmataPinModeOutput,
|
||||
firmataSetDigitalPin, 18, 0,
|
||||
firmataSetPinMode, 16, FirmataPinModeOutput,
|
||||
firmataSetDigitalPin, 16, 0,
|
||||
}
|
||||
if !bytes.Equal(writes, wantPrefix) {
|
||||
t.Fatalf("initial controller bytes = %v, want %v", writes, wantPrefix)
|
||||
}
|
||||
|
||||
baseline = len(connection.Bytes())
|
||||
if err := controllers.Headlight.HandleAction("off"); err != nil {
|
||||
t.Fatalf("turn headlight off: %v", err)
|
||||
}
|
||||
if controllers.Headlight.On() {
|
||||
t.Fatal("headlight remained logically on")
|
||||
}
|
||||
// Active-low means logical off becomes a high electrical output.
|
||||
if got, want := connection.Bytes()[baseline:], []byte{firmataSetDigitalPin, 18, 1}; !bytes.Equal(got, want) {
|
||||
t.Fatalf("headlight bytes = %v, want %v", got, want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestMissingNativeAndFirmataRolesRemainDisabled(t *testing.T) {
|
||||
manager := &PeripheralManager{byID: make(map[string]*managedPeripheral)}
|
||||
controllers, err := ResolveRoverHardwareControllers(&Config{}, manager, discardLogger())
|
||||
if err != nil {
|
||||
t.Fatalf("resolve: %v", err)
|
||||
}
|
||||
if controllers.CameraServo != nil || controllers.Headlight != nil || controllers.Laser != nil {
|
||||
t.Fatalf("unexpected controllers without providers: %#v", controllers)
|
||||
}
|
||||
}
|
||||
|
||||
func TestEnabledNativeRolesWinEvenWithSeveralFirmataProviders(t *testing.T) {
|
||||
roleDescription := PeripheralDescription{RoverControls: PeripheralRoverControls{
|
||||
CameraServo: &PeripheralCameraServo{},
|
||||
Headlight: &PeripheralDigitalRole{},
|
||||
Laser: &PeripheralDigitalRole{},
|
||||
}}
|
||||
manager := &PeripheralManager{
|
||||
byID: make(map[string]*managedPeripheral),
|
||||
peripherals: []*managedPeripheral{
|
||||
{metadata: RoverPeripheralMetadata{ID: "firmata-0"}, description: roleDescription},
|
||||
{metadata: RoverPeripheralMetadata{ID: "firmata-1"}, description: roleDescription},
|
||||
},
|
||||
}
|
||||
cfg := &Config{
|
||||
CameraServo: CameraServoConfig{Enabled: true},
|
||||
Headlight: GPIOToggleConfig{Enabled: true},
|
||||
Laser: GPIOToggleConfig{Enabled: true},
|
||||
}
|
||||
nativeCamera := &testCameraServoController{cfg: cfg.CameraServo}
|
||||
nativeToggles := map[string]*testToggleController{}
|
||||
factories := nativeHardwareControllerFactories{
|
||||
newCameraServo: func(_ CameraServoConfig, _ *log.Logger) (CameraServoController, error) {
|
||||
return nativeCamera, nil
|
||||
},
|
||||
newToggle: func(name string, config GPIOToggleConfig, _ *log.Logger) (ToggleController, error) {
|
||||
controller := &testToggleController{cfg: config}
|
||||
nativeToggles[name] = controller
|
||||
return controller, nil
|
||||
},
|
||||
}
|
||||
|
||||
// Duplicate Firmata declarations are irrelevant when native hardware wins;
|
||||
// selection must neither fail nor initialize either ESP32 provider.
|
||||
controllers, err := resolveRoverHardwareControllers(cfg, manager, discardLogger(), factories)
|
||||
if err != nil {
|
||||
t.Fatalf("resolve native precedence: %v", err)
|
||||
}
|
||||
if controllers.CameraServo != nativeCamera || controllers.Headlight != nativeToggles["headlight"] || controllers.Laser != nativeToggles["laser"] {
|
||||
t.Fatal("resolver did not retain native controllers")
|
||||
}
|
||||
}
|
||||
|
||||
type testCameraServoController struct {
|
||||
cfg CameraServoConfig
|
||||
}
|
||||
|
||||
func (controller *testCameraServoController) SetAngle(float64) error { return nil }
|
||||
func (controller *testCameraServoController) Nudge(float64) error { return nil }
|
||||
func (controller *testCameraServoController) SetPulseWidth(int) error { return nil }
|
||||
func (controller *testCameraServoController) CurrentAngle() float64 { return 0 }
|
||||
func (controller *testCameraServoController) Configuration() CameraServoConfig { return controller.cfg }
|
||||
func (controller *testCameraServoController) Close() {}
|
||||
|
||||
type testToggleController struct {
|
||||
cfg GPIOToggleConfig
|
||||
on bool
|
||||
}
|
||||
|
||||
func (controller *testToggleController) HandleAction(string) error { return nil }
|
||||
func (controller *testToggleController) On() bool { return controller.on }
|
||||
func (controller *testToggleController) Configuration() GPIOToggleConfig { return controller.cfg }
|
||||
func (controller *testToggleController) Close() {}
|
||||
@@ -0,0 +1,438 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"context"
|
||||
"encoding/json"
|
||||
"io"
|
||||
"reflect"
|
||||
"sync"
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
|
||||
func TestFirmataParserHandlesFragmentedSysex(t *testing.T) {
|
||||
parser := FirmataParser{}
|
||||
|
||||
first, err := parser.Feed([]byte{firmataStartSysex, firmataPeripheralFeature, firmataPeripheralDescription, 1})
|
||||
if err != nil {
|
||||
t.Fatalf("first fragment: %v", err)
|
||||
}
|
||||
if len(first) != 0 {
|
||||
t.Fatalf("first fragment unexpectedly produced %d messages", len(first))
|
||||
}
|
||||
|
||||
second, err := parser.Feed([]byte{0, 2, 0, firmataEndSysex})
|
||||
if err != nil {
|
||||
t.Fatalf("second fragment: %v", err)
|
||||
}
|
||||
want := []FirmataMessage{{
|
||||
Command: firmataPeripheralFeature,
|
||||
Data: []byte{firmataPeripheralDescription, 1, 0, 2, 0},
|
||||
Sysex: true,
|
||||
}}
|
||||
if !reflect.DeepEqual(second, want) {
|
||||
t.Fatalf("messages = %#v, want %#v", second, want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataParserReturnsSeveralMessagesFromOneRead(t *testing.T) {
|
||||
parser := FirmataParser{}
|
||||
messages, err := parser.Feed([]byte{
|
||||
firmataReportVersion, 2, 5,
|
||||
firmataStartSysex, firmataCapabilityReply, 0x01, 0x01, 0x7F, firmataEndSysex,
|
||||
firmataSetDigitalPin, 18, 1,
|
||||
})
|
||||
if err != nil {
|
||||
t.Fatalf("feed: %v", err)
|
||||
}
|
||||
if len(messages) != 3 {
|
||||
t.Fatalf("got %d messages, want 3", len(messages))
|
||||
}
|
||||
if messages[0].Command != firmataReportVersion || messages[1].Command != firmataCapabilityReply || messages[2].Command != firmataSetDigitalPin {
|
||||
t.Fatalf("commands were not preserved in wire order: %#v", messages)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataParserRejectsEightBitSysexDataAndRecovers(t *testing.T) {
|
||||
parser := FirmataParser{}
|
||||
if _, err := parser.Feed([]byte{firmataStartSysex, firmataPeripheralFeature, 0x80}); err == nil {
|
||||
t.Fatal("expected invalid SysEx data to fail")
|
||||
}
|
||||
|
||||
messages, err := parser.Feed([]byte{firmataReportVersion, 2, 5})
|
||||
if err != nil {
|
||||
t.Fatalf("feed after invalid SysEx: %v", err)
|
||||
}
|
||||
if len(messages) != 1 || messages[0].Command != firmataReportVersion {
|
||||
t.Fatalf("parser did not recover: %#v", messages)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataSevenBitRoundTripIncludesUTF8(t *testing.T) {
|
||||
raw := []byte(`{"name":"Café lights","value":255}`)
|
||||
encoded := EncodeFirmata7Bit(raw)
|
||||
for index, value := range encoded {
|
||||
if value&0x80 != 0 {
|
||||
t.Fatalf("encoded byte %d is not seven-bit clean: 0x%02x", index, value)
|
||||
}
|
||||
}
|
||||
decoded, err := DecodeFirmata7Bit(encoded)
|
||||
if err != nil {
|
||||
t.Fatalf("decode: %v", err)
|
||||
}
|
||||
if !bytes.Equal(decoded, raw) {
|
||||
t.Fatalf("decoded %q, want %q", decoded, raw)
|
||||
}
|
||||
}
|
||||
|
||||
func TestDecodeFirmataSevenBitRejectsMalformedPairs(t *testing.T) {
|
||||
for name, encoded := range map[string][]byte{
|
||||
"odd length": {1},
|
||||
"high byte": {1, 2},
|
||||
"eight bit": {0x80, 0},
|
||||
} {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
if _, err := DecodeFirmata7Bit(encoded); err == nil {
|
||||
t.Fatal("expected malformed pair to fail")
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralDescriptionPreservesControlOrder(t *testing.T) {
|
||||
raw := []byte(`{
|
||||
"name":"Test peripheral",
|
||||
"controls":[
|
||||
{"id":"servo","type":"slider","name":"Servo","min":0,"max":180,"output":{"type":"servo","pin":14}},
|
||||
{"id":"lights","type":"slider","name":"Lights","min":0,"max":255,"output":{"type":"pwm","pin":18}},
|
||||
{"id":"action","type":"button","name":"Action","mode":"momentary","output":{"type":"custom"}}
|
||||
]
|
||||
}`)
|
||||
var description PeripheralDescription
|
||||
if err := json.Unmarshal(raw, &description); err != nil {
|
||||
t.Fatalf("unmarshal: %v", err)
|
||||
}
|
||||
if err := description.Validate(); err != nil {
|
||||
t.Fatalf("validate: %v", err)
|
||||
}
|
||||
want := []string{"servo", "lights", "action"}
|
||||
for index, id := range want {
|
||||
if description.Controls[index].ID != id {
|
||||
t.Fatalf("control %d = %q, want %q", index, description.Controls[index].ID, id)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralDescriptionRejectsInvalidDeclarations(t *testing.T) {
|
||||
minimum, maximum, pin := 10, 1, 200
|
||||
for name, description := range map[string]PeripheralDescription{
|
||||
"duplicate id": {
|
||||
Name: "device",
|
||||
Controls: []PeripheralControl{
|
||||
{ID: "same", Name: "First", Type: "button", Mode: "toggle", Output: PeripheralOutput{Type: "custom"}},
|
||||
{ID: "same", Name: "Second", Type: "button", Mode: "toggle", Output: PeripheralOutput{Type: "custom"}},
|
||||
},
|
||||
},
|
||||
"reversed range": {
|
||||
Name: "device",
|
||||
Controls: []PeripheralControl{{
|
||||
ID: "level", Name: "Level", Type: "slider", Minimum: &minimum, Maximum: &maximum, Output: PeripheralOutput{Type: "custom"},
|
||||
}},
|
||||
},
|
||||
"pin outside Firmata": {
|
||||
Name: "device",
|
||||
Controls: []PeripheralControl{{
|
||||
ID: "switch", Name: "Switch", Type: "button", Mode: "toggle", Output: PeripheralOutput{Type: "digital", Pin: &pin},
|
||||
}},
|
||||
},
|
||||
} {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
if err := description.Validate(); err == nil {
|
||||
t.Fatal("expected invalid description to fail")
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestParseFirmataCapabilities(t *testing.T) {
|
||||
pins, err := parseFirmataCapabilities([]byte{
|
||||
FirmataPinModeOutput, 1, FirmataPinModePWM, 8, 0x7F,
|
||||
FirmataPinModeOutput, 1, FirmataPinModeServo, 14, 0x7F,
|
||||
})
|
||||
if err != nil {
|
||||
t.Fatalf("parse capabilities: %v", err)
|
||||
}
|
||||
if len(pins) != 2 || len(pins[0]) != 2 || pins[1][1].Mode != FirmataPinModeServo {
|
||||
t.Fatalf("unexpected capabilities: %#v", pins)
|
||||
}
|
||||
|
||||
if _, err := parseFirmataCapabilities([]byte{FirmataPinModeOutput}); err == nil {
|
||||
t.Fatal("expected incomplete capability pair to fail")
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientWritesStandardCommands(t *testing.T) {
|
||||
connection := &recordingConnection{}
|
||||
client := NewFirmataClient(connection)
|
||||
|
||||
if err := client.SetPinMode(14, FirmataPinModeServo); err != nil {
|
||||
t.Fatalf("set pin mode: %v", err)
|
||||
}
|
||||
if err := client.ConfigureServo(14, 900, 2100); err != nil {
|
||||
t.Fatalf("configure servo: %v", err)
|
||||
}
|
||||
if err := client.ExtendedAnalog(14, 180); err != nil {
|
||||
t.Fatalf("extended analog: %v", err)
|
||||
}
|
||||
if err := client.SetDigitalPin(19, true); err != nil {
|
||||
t.Fatalf("digital write: %v", err)
|
||||
}
|
||||
|
||||
want := []byte{
|
||||
firmataSetPinMode, 14, FirmataPinModeServo,
|
||||
firmataStartSysex, firmataServoConfig, 14, 4, 7, 52, 16, firmataEndSysex,
|
||||
firmataStartSysex, firmataExtendedAnalog, 14, 52, 1, firmataEndSysex,
|
||||
firmataSetDigitalPin, 19, 1,
|
||||
}
|
||||
if got := connection.Bytes(); !bytes.Equal(got, want) {
|
||||
t.Fatalf("wire bytes = %v, want %v", got, want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientQueriesAndDecodesDescription(t *testing.T) {
|
||||
descriptionJSON := []byte(`{"name":"Bench device","controls":[{"id":"go","type":"button","name":"Go","mode":"momentary","output":{"type":"custom"}}]}`)
|
||||
firmwareName := EncodeFirmata7Bit([]byte("RoverPeripheralFirmata"))
|
||||
description := append([]byte{firmataStartSysex, firmataPeripheralFeature, firmataPeripheralDescription}, EncodeFirmata7Bit(descriptionJSON)...)
|
||||
description = append(description, firmataEndSysex)
|
||||
|
||||
connection := newScriptedConnection(
|
||||
append(append([]byte{firmataStartSysex, firmataReportFirmware, 1, 0}, firmwareName...), firmataEndSysex),
|
||||
description,
|
||||
)
|
||||
client := NewFirmataClient(connection)
|
||||
ctx, cancel := context.WithTimeout(context.Background(), time.Second)
|
||||
defer cancel()
|
||||
client.Start(ctx)
|
||||
|
||||
firmware, err := client.QueryFirmware(ctx)
|
||||
if err != nil {
|
||||
t.Fatalf("query firmware: %v", err)
|
||||
}
|
||||
if firmware.Name != "RoverPeripheralFirmata" || firmware.Major != 1 || firmware.Minor != 0 {
|
||||
t.Fatalf("unexpected firmware: %#v", firmware)
|
||||
}
|
||||
|
||||
got, err := client.Describe(ctx)
|
||||
if err != nil {
|
||||
t.Fatalf("describe: %v", err)
|
||||
}
|
||||
if got.Name != "Bench device" || len(got.Controls) != 1 || got.Controls[0].ID != "go" {
|
||||
t.Fatalf("unexpected description: %#v", got)
|
||||
}
|
||||
|
||||
writes := connection.Bytes()
|
||||
wantWrites := []byte{
|
||||
firmataStartSysex, firmataReportFirmware, firmataEndSysex,
|
||||
firmataStartSysex, firmataPeripheralFeature, firmataPeripheralDescribe, firmataEndSysex,
|
||||
}
|
||||
if !bytes.Equal(writes, wantWrites) {
|
||||
t.Fatalf("queries = %v, want %v", writes, wantWrites)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientKeepsReadingAfterSerialTimeoutEOF(t *testing.T) {
|
||||
firmwareName := EncodeFirmata7Bit([]byte("RoverPeripheralFirmata"))
|
||||
response := append([]byte{firmataStartSysex, firmataReportFirmware, 1, 0}, firmwareName...)
|
||||
response = append(response, firmataEndSysex)
|
||||
|
||||
// tarm/serial returns io.EOF when its ReadTimeout expires without bytes.
|
||||
// Reproducing that behavior before the response prevents this regression
|
||||
// from being hidden by an in-memory reader that blocks indefinitely instead.
|
||||
connection := newScriptedConnection(response)
|
||||
connection.timeoutsBeforeRead = 1
|
||||
client := NewFirmataClient(connection)
|
||||
ctx, cancel := context.WithTimeout(context.Background(), time.Second)
|
||||
defer cancel()
|
||||
client.Start(ctx)
|
||||
|
||||
firmware, err := client.QueryFirmware(ctx)
|
||||
if err != nil {
|
||||
t.Fatalf("query firmware after timeout: %v", err)
|
||||
}
|
||||
if firmware.Name != "RoverPeripheralFirmata" {
|
||||
t.Fatalf("firmware name = %q", firmware.Name)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientEncodesCustomControl(t *testing.T) {
|
||||
for name, testCase := range map[string]struct {
|
||||
controlID string
|
||||
value any
|
||||
wantJSON string
|
||||
}{
|
||||
"button": {controlID: "specialAction", value: true, wantJSON: `{"control":"specialAction","value":true}`},
|
||||
"text": {controlID: "displayText", value: "Café ready", wantJSON: `{"control":"displayText","value":"Café ready"}`},
|
||||
} {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
connection := &recordingConnection{}
|
||||
client := NewFirmataClient(connection)
|
||||
if err := client.SendPeripheralControl(testCase.controlID, testCase.value); err != nil {
|
||||
t.Fatalf("send control: %v", err)
|
||||
}
|
||||
|
||||
wire := connection.Bytes()
|
||||
if len(wire) < 5 || wire[0] != firmataStartSysex || wire[1] != firmataPeripheralFeature || wire[2] != firmataPeripheralControl || wire[len(wire)-1] != firmataEndSysex {
|
||||
t.Fatalf("invalid control frame: %v", wire)
|
||||
}
|
||||
raw, err := DecodeFirmata7Bit(wire[3 : len(wire)-1])
|
||||
if err != nil {
|
||||
t.Fatalf("decode control: %v", err)
|
||||
}
|
||||
if string(raw) != testCase.wantJSON {
|
||||
t.Fatalf("control JSON = %s, want %s", raw, testCase.wantJSON)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientQueriesCapabilities(t *testing.T) {
|
||||
response := []byte{
|
||||
firmataStartSysex, firmataCapabilityReply,
|
||||
FirmataPinModeOutput, 1, FirmataPinModePWM, 8, 0x7F,
|
||||
FirmataPinModeOutput, 1, FirmataPinModeServo, 14, 0x7F,
|
||||
firmataEndSysex,
|
||||
}
|
||||
connection := newScriptedConnection(response)
|
||||
client := NewFirmataClient(connection)
|
||||
ctx, cancel := context.WithTimeout(context.Background(), time.Second)
|
||||
defer cancel()
|
||||
client.Start(ctx)
|
||||
|
||||
pins, err := client.QueryCapabilities(ctx)
|
||||
if err != nil {
|
||||
t.Fatalf("query capabilities: %v", err)
|
||||
}
|
||||
if len(pins) != 2 || pins[0][1].Mode != FirmataPinModePWM || pins[1][1].Mode != FirmataPinModeServo {
|
||||
t.Fatalf("unexpected capabilities: %#v", pins)
|
||||
}
|
||||
if want := []byte{firmataStartSysex, firmataCapabilityQuery, firmataEndSysex}; !bytes.Equal(connection.Bytes(), want) {
|
||||
t.Fatalf("query bytes = %v, want %v", connection.Bytes(), want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientRejectsControlTooLargeForFirmwareParser(t *testing.T) {
|
||||
connection := &recordingConnection{}
|
||||
client := NewFirmataClient(connection)
|
||||
if err := client.SendPeripheralControl("displayText", string(bytes.Repeat([]byte{'x'}, 200))); err == nil {
|
||||
t.Fatal("expected oversized control to fail")
|
||||
}
|
||||
if len(connection.Bytes()) != 0 {
|
||||
t.Fatalf("oversized control wrote bytes: %v", connection.Bytes())
|
||||
}
|
||||
}
|
||||
|
||||
// recordingConnection is deliberately minimal: write-focused tests should not
|
||||
// need goroutines or a real serial device merely to inspect exact Firmata bytes.
|
||||
type recordingConnection struct {
|
||||
mu sync.Mutex
|
||||
writes bytes.Buffer
|
||||
closed bool
|
||||
writeErr error
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) Read(_ []byte) (int, error) { return 0, io.EOF }
|
||||
|
||||
func (connection *recordingConnection) Write(data []byte) (int, error) {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
if connection.closed {
|
||||
return 0, io.ErrClosedPipe
|
||||
}
|
||||
if connection.writeErr != nil {
|
||||
return 0, connection.writeErr
|
||||
}
|
||||
return connection.writes.Write(data)
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) Close() error {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
connection.closed = true
|
||||
return nil
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) Bytes() []byte {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
return append([]byte(nil), connection.writes.Bytes()...)
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) Closed() bool {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
return connection.closed
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) SetWriteError(err error) {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
connection.writeErr = err
|
||||
}
|
||||
|
||||
// scriptedConnection releases one response after each client write. This
|
||||
// mirrors request/response serial behavior and prevents a fast reader goroutine
|
||||
// from publishing all scripted answers before the matching query is sent.
|
||||
type scriptedConnection struct {
|
||||
recordingConnection
|
||||
responses chan []byte
|
||||
reads chan []byte
|
||||
timeoutsBeforeRead int
|
||||
pendingRead []byte
|
||||
}
|
||||
|
||||
func newScriptedConnection(responses ...[]byte) *scriptedConnection {
|
||||
connection := &scriptedConnection{
|
||||
responses: make(chan []byte, len(responses)),
|
||||
reads: make(chan []byte, len(responses)),
|
||||
}
|
||||
for _, response := range responses {
|
||||
connection.responses <- append([]byte(nil), response...)
|
||||
}
|
||||
return connection
|
||||
}
|
||||
|
||||
func (connection *scriptedConnection) Read(target []byte) (int, error) {
|
||||
if connection.timeoutsBeforeRead > 0 {
|
||||
connection.timeoutsBeforeRead--
|
||||
return 0, io.EOF
|
||||
}
|
||||
if len(connection.pendingRead) == 0 {
|
||||
response, ok := <-connection.reads
|
||||
if !ok {
|
||||
return 0, io.ErrClosedPipe
|
||||
}
|
||||
connection.pendingRead = response
|
||||
}
|
||||
written := copy(target, connection.pendingRead)
|
||||
connection.pendingRead = connection.pendingRead[written:]
|
||||
return written, nil
|
||||
}
|
||||
|
||||
func (connection *scriptedConnection) Write(data []byte) (int, error) {
|
||||
written, err := connection.recordingConnection.Write(data)
|
||||
if err == nil {
|
||||
select {
|
||||
case response := <-connection.responses:
|
||||
connection.reads <- response
|
||||
default:
|
||||
}
|
||||
}
|
||||
return written, err
|
||||
}
|
||||
|
||||
func (connection *scriptedConnection) Close() error {
|
||||
_ = connection.recordingConnection.Close()
|
||||
close(connection.reads)
|
||||
return nil
|
||||
}
|
||||
@@ -87,6 +87,11 @@ func (g *GPIOToggle) On() bool {
|
||||
return g.on
|
||||
}
|
||||
|
||||
// Configuration returns the native toggle behavior used in the rover hello.
|
||||
func (g *GPIOToggle) Configuration() GPIOToggleConfig {
|
||||
return g.cfg
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) setLocked(on bool) error {
|
||||
// This is the only place a logical device state becomes an electrical GPIO
|
||||
// value. Hardware that turns on when pulled low sets activeLow in roverd
|
||||
|
||||
@@ -13,10 +13,10 @@ type GPIOToggle struct {
|
||||
|
||||
func NewGPIOToggle(name string, _ GPIOToggleConfig, _ *log.Logger) (*GPIOToggle, error) {
|
||||
/*
|
||||
A Debian laptop has no Raspberry Pi GPIO character-device contract for
|
||||
headlights or lasers. Returning an error when enabled makes bad laptop
|
||||
configs fail during startup instead of advertising controls that cannot
|
||||
change any hardware.
|
||||
A Debian laptop has no native Raspberry Pi GPIO contract. Returning an
|
||||
error here catches an invalid native configuration; the shared resolver
|
||||
selects an ESP32 Firmata toggle before this constructor when native GPIO
|
||||
is disabled.
|
||||
*/
|
||||
return nil, fmt.Errorf("%s not supported in the debian-laptop build", name)
|
||||
}
|
||||
@@ -30,3 +30,7 @@ func (g *GPIOToggle) HandleAction(action string) error {
|
||||
func (g *GPIOToggle) On() bool {
|
||||
return false
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) Configuration() GPIOToggleConfig {
|
||||
return GPIOToggleConfig{}
|
||||
}
|
||||
|
||||
@@ -24,3 +24,7 @@ func (g *GPIOToggle) HandleAction(action string) error {
|
||||
func (g *GPIOToggle) On() bool {
|
||||
return false
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) Configuration() GPIOToggleConfig {
|
||||
return GPIOToggleConfig{}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,122 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"time"
|
||||
)
|
||||
|
||||
// Both physical servo backends consume these exact motion constants. Keeping
|
||||
// them in shared code prevents Pi PWM and ESP32 Firmata movement from drifting
|
||||
// apart as either implementation evolves.
|
||||
const (
|
||||
maxServoDegPerSec = 60.0
|
||||
servoStepInterval = 20 * time.Millisecond
|
||||
servoAngleEpsilon = 0.01
|
||||
)
|
||||
|
||||
// CameraServoController is the hardware-neutral camera-tilt contract used by
|
||||
// WSClient. Native Pi PWM and ESP32 Firmata implementations expose identical
|
||||
// logical behavior, so command handling never branches on the rover host type.
|
||||
type CameraServoController interface {
|
||||
SetAngle(angle float64) error
|
||||
Nudge(delta float64) error
|
||||
SetPulseWidth(micros int) error
|
||||
CurrentAngle() float64
|
||||
Configuration() CameraServoConfig
|
||||
Close()
|
||||
}
|
||||
|
||||
// ToggleController keeps headlight and laser command/state behavior independent
|
||||
// of whether the electrical write happens on native Pi GPIO or an ESP32 pin.
|
||||
type ToggleController interface {
|
||||
HandleAction(action string) error
|
||||
On() bool
|
||||
Configuration() GPIOToggleConfig
|
||||
Close()
|
||||
}
|
||||
|
||||
// RoverHardwareControllers is the result of the single startup-time backend
|
||||
// decision. Its effective configurations are derived from whichever backend
|
||||
// won, making the normal rover hello accurate on both Pi and laptop hosts.
|
||||
type RoverHardwareControllers struct {
|
||||
CameraServo CameraServoController
|
||||
Headlight ToggleController
|
||||
Laser ToggleController
|
||||
}
|
||||
|
||||
type nativeHardwareControllerFactories struct {
|
||||
newCameraServo func(CameraServoConfig, *log.Logger) (CameraServoController, error)
|
||||
newToggle func(string, GPIOToggleConfig, *log.Logger) (ToggleController, error)
|
||||
}
|
||||
|
||||
// ResolveRoverHardwareControllers applies one rule on every real rover build:
|
||||
// enabled native GPIO wins, otherwise one discovered ESP32 may fill the role.
|
||||
// The rule is intentionally not selected by GOARCH or the debian_laptop tag.
|
||||
func ResolveRoverHardwareControllers(cfg *Config, peripherals *PeripheralManager, logger *log.Logger) (RoverHardwareControllers, error) {
|
||||
factories := nativeHardwareControllerFactories{
|
||||
newCameraServo: func(config CameraServoConfig, logger *log.Logger) (CameraServoController, error) {
|
||||
return NewCameraServo(config, logger)
|
||||
},
|
||||
newToggle: func(name string, config GPIOToggleConfig, logger *log.Logger) (ToggleController, error) {
|
||||
return NewGPIOToggle(name, config, logger)
|
||||
},
|
||||
}
|
||||
return resolveRoverHardwareControllers(cfg, peripherals, logger, factories)
|
||||
}
|
||||
|
||||
func resolveRoverHardwareControllers(cfg *Config, peripherals *PeripheralManager, logger *log.Logger, factories nativeHardwareControllerFactories) (RoverHardwareControllers, error) {
|
||||
var controllers RoverHardwareControllers
|
||||
var err error
|
||||
|
||||
controllers.CameraServo, err = resolveCameraServoController(cfg.CameraServo, peripherals, logger, factories.newCameraServo)
|
||||
if err != nil {
|
||||
return RoverHardwareControllers{}, fmt.Errorf("init camera servo: %w", err)
|
||||
}
|
||||
controllers.Headlight, err = resolveToggleController("headlight", cfg.Headlight, peripherals, logger, factories.newToggle)
|
||||
if err != nil {
|
||||
controllers.Close()
|
||||
return RoverHardwareControllers{}, fmt.Errorf("init headlight: %w", err)
|
||||
}
|
||||
controllers.Laser, err = resolveToggleController("laser", cfg.Laser, peripherals, logger, factories.newToggle)
|
||||
if err != nil {
|
||||
controllers.Close()
|
||||
return RoverHardwareControllers{}, fmt.Errorf("init laser: %w", err)
|
||||
}
|
||||
return controllers, nil
|
||||
}
|
||||
|
||||
func resolveCameraServoController(nativeConfig CameraServoConfig, peripherals *PeripheralManager, logger *log.Logger, newNative func(CameraServoConfig, *log.Logger) (CameraServoController, error)) (CameraServoController, error) {
|
||||
if nativeConfig.Enabled {
|
||||
if peripherals.HasRoverRole("cameraServo") {
|
||||
logger.Printf("ignoring ESP32 cameraServo because native camera servo is enabled")
|
||||
}
|
||||
return newNative(nativeConfig, logger)
|
||||
}
|
||||
return peripherals.NewFirmataCameraServo(logger)
|
||||
}
|
||||
|
||||
func resolveToggleController(name string, nativeConfig GPIOToggleConfig, peripherals *PeripheralManager, logger *log.Logger, newNative func(string, GPIOToggleConfig, *log.Logger) (ToggleController, error)) (ToggleController, error) {
|
||||
if nativeConfig.Enabled {
|
||||
if peripherals.HasRoverRole(name) {
|
||||
logger.Printf("ignoring ESP32 %s because native %s is enabled", name, name)
|
||||
}
|
||||
return newNative(name, nativeConfig, logger)
|
||||
}
|
||||
return peripherals.NewFirmataToggle(name, logger)
|
||||
}
|
||||
|
||||
// Close releases selected controller resources in reverse dependency order.
|
||||
// Firmata controllers do not close the shared serial connection; that remains
|
||||
// owned by PeripheralManager and is released by its separate shutdown defer.
|
||||
func (controllers *RoverHardwareControllers) Close() {
|
||||
if controllers.Laser != nil {
|
||||
controllers.Laser.Close()
|
||||
}
|
||||
if controllers.Headlight != nil {
|
||||
controllers.Headlight.Close()
|
||||
}
|
||||
if controllers.CameraServo != nil {
|
||||
controllers.CameraServo.Close()
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"encoding/json"
|
||||
"strings"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestHelloPeripheralMetadataContainsOnlyRenderableFields(t *testing.T) {
|
||||
minimum, maximum := 0, 180
|
||||
message := helloMessage{
|
||||
Type: "hello",
|
||||
Name: "test-rover",
|
||||
Peripherals: []RoverPeripheralMetadata{{
|
||||
ID: "firmata-0",
|
||||
Name: "Camera arm",
|
||||
Controls: []RoverPeripheralControl{{
|
||||
ID: "position", Type: "slider", Name: "Position", Minimum: &minimum, Maximum: &maximum,
|
||||
}},
|
||||
}},
|
||||
}
|
||||
|
||||
encoded, err := json.Marshal(message)
|
||||
if err != nil {
|
||||
t.Fatalf("marshal hello: %v", err)
|
||||
}
|
||||
text := string(encoded)
|
||||
if !strings.Contains(text, `"peripherals":[{"id":"firmata-0","name":"Camera arm","controls":[{"id":"position","type":"slider","name":"Position","min":0,"max":180}]`) {
|
||||
t.Fatalf("hello is missing ordered peripheral metadata: %s", text)
|
||||
}
|
||||
var envelope map[string]json.RawMessage
|
||||
if err := json.Unmarshal(encoded, &envelope); err != nil {
|
||||
t.Fatalf("unmarshal hello envelope: %v", err)
|
||||
}
|
||||
peripheralJSON := string(envelope["peripherals"])
|
||||
if strings.Contains(peripheralJSON, `"pin"`) || strings.Contains(peripheralJSON, `"output"`) {
|
||||
t.Fatalf("hello exposed private Firmata routing: %s", peripheralJSON)
|
||||
}
|
||||
}
|
||||
|
||||
func TestInboundPeripheralCommandPreservesRawJSONValue(t *testing.T) {
|
||||
var message inboundMessage
|
||||
err := json.Unmarshal([]byte(`{
|
||||
"type":"peripheral",
|
||||
"id":"command-1",
|
||||
"peripheral":{"id":"firmata-0","control":"displayText","value":"hello rover"}
|
||||
}`), &message)
|
||||
if err != nil {
|
||||
t.Fatalf("unmarshal command: %v", err)
|
||||
}
|
||||
if message.Peripheral == nil || message.Peripheral.ID != "firmata-0" || message.Peripheral.Control != "displayText" {
|
||||
t.Fatalf("unexpected peripheral command: %#v", message.Peripheral)
|
||||
}
|
||||
if string(message.Peripheral.Value) != `"hello rover"` {
|
||||
t.Fatalf("raw value = %s", message.Peripheral.Value)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
//go:build dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"io"
|
||||
"log"
|
||||
"time"
|
||||
)
|
||||
|
||||
// DiscoverPeripheralManager remains inert in a dummy build. The dummy daemon is
|
||||
// specifically used without rover hardware and must not probe or reset serial
|
||||
// devices that happen to be attached to a developer's machine.
|
||||
func DiscoverPeripheralManager(ctx context.Context, excludedDevice string, logger *log.Logger) (*PeripheralManager, error) {
|
||||
dependencies := peripheralDiscoveryDependencies{
|
||||
listCandidates: func(string) ([]string, error) { return nil, nil },
|
||||
open: func(string) (io.ReadWriteCloser, error) { return nil, nil },
|
||||
sleep: func(time.Duration) {},
|
||||
startupWait: 0,
|
||||
handshakeWait: 0,
|
||||
}
|
||||
return discoverPeripheralManager(ctx, excludedDevice, logger, dependencies)
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
//go:build !dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"io"
|
||||
"log"
|
||||
"time"
|
||||
|
||||
"github.com/tarm/serial"
|
||||
)
|
||||
|
||||
const (
|
||||
peripheralBaud = 115200
|
||||
peripheralReadTimeout = 100 * time.Millisecond
|
||||
)
|
||||
|
||||
// DiscoverPeripheralManager performs the one and only peripheral scan for this
|
||||
// roverd process. The Roomba Open Interface serial device is explicitly
|
||||
// excluded because it belongs to SerialAdapter and must never be probed as an
|
||||
// ESP32 peripheral.
|
||||
func DiscoverPeripheralManager(ctx context.Context, excludedDevice string, logger *log.Logger) (*PeripheralManager, error) {
|
||||
dependencies := peripheralDiscoveryDependencies{
|
||||
listCandidates: listPeripheralCandidates,
|
||||
open: func(devicePath string) (io.ReadWriteCloser, error) {
|
||||
return serial.OpenPort(&serial.Config{
|
||||
Name: devicePath,
|
||||
Baud: peripheralBaud,
|
||||
ReadTimeout: peripheralReadTimeout,
|
||||
})
|
||||
},
|
||||
sleep: time.Sleep,
|
||||
startupWait: peripheralStartupWait,
|
||||
handshakeWait: peripheralHandshakeTimeout,
|
||||
}
|
||||
return discoverPeripheralManager(ctx, excludedDevice, logger, dependencies)
|
||||
}
|
||||
@@ -0,0 +1,534 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"fmt"
|
||||
"io"
|
||||
"log"
|
||||
"path/filepath"
|
||||
"sort"
|
||||
"strings"
|
||||
"sync"
|
||||
"time"
|
||||
"unicode/utf8"
|
||||
)
|
||||
|
||||
const (
|
||||
peripheralStartupWait = 2 * time.Second
|
||||
peripheralHandshakeTimeout = 5 * time.Second
|
||||
peripheralFirmwareName = "RoverPeripheralFirmata"
|
||||
)
|
||||
|
||||
// RoverPeripheralMetadata is the part of a peripheral description that leaves
|
||||
// roverd. Pin numbers and output mappings intentionally remain private to the
|
||||
// rover process; the server and browser identify only the declared control.
|
||||
type RoverPeripheralMetadata struct {
|
||||
ID string `json:"id"`
|
||||
Name string `json:"name"`
|
||||
Controls []RoverPeripheralControl `json:"controls"`
|
||||
}
|
||||
|
||||
// RoverPeripheralControl contains only fields needed to render and operate one
|
||||
// of the four generic UI controls. Pointer fields preserve legitimate zero
|
||||
// bounds while still omitting properties that do not apply to a control type.
|
||||
type RoverPeripheralControl struct {
|
||||
ID string `json:"id"`
|
||||
Type string `json:"type"`
|
||||
Name string `json:"name"`
|
||||
Mode string `json:"mode,omitempty"`
|
||||
Minimum *int `json:"min,omitempty"`
|
||||
Maximum *int `json:"max,omitempty"`
|
||||
MaximumLength *int `json:"maxLength,omitempty"`
|
||||
}
|
||||
|
||||
type managedPeripheral struct {
|
||||
metadata RoverPeripheralMetadata
|
||||
description PeripheralDescription
|
||||
controls map[string]PeripheralControl
|
||||
client *FirmataClient
|
||||
connection io.ReadWriteCloser
|
||||
devicePath string
|
||||
capabilities [][]FirmataPinCapability
|
||||
}
|
||||
|
||||
// PeripheralManager owns the immutable boot-time inventory and every serial
|
||||
// connection behind it. The inventory never changes after discovery, even if a
|
||||
// USB device later disappears; a process restart is the only rescan mechanism.
|
||||
type PeripheralManager struct {
|
||||
mu sync.RWMutex
|
||||
peripherals []*managedPeripheral
|
||||
byID map[string]*managedPeripheral
|
||||
cancel context.CancelFunc
|
||||
closeOnce sync.Once
|
||||
logger *log.Logger
|
||||
}
|
||||
|
||||
type peripheralDiscoveryDependencies struct {
|
||||
listCandidates func(excludedDevice string) ([]string, error)
|
||||
open func(devicePath string) (io.ReadWriteCloser, error)
|
||||
sleep func(time.Duration)
|
||||
startupWait time.Duration
|
||||
handshakeWait time.Duration
|
||||
}
|
||||
|
||||
func discoverPeripheralManager(ctx context.Context, excludedDevice string, logger *log.Logger, dependencies peripheralDiscoveryDependencies) (*PeripheralManager, error) {
|
||||
managerContext, cancel := context.WithCancel(ctx)
|
||||
manager := &PeripheralManager{
|
||||
byID: make(map[string]*managedPeripheral),
|
||||
cancel: cancel,
|
||||
logger: logger,
|
||||
}
|
||||
|
||||
candidates, err := dependencies.listCandidates(excludedDevice)
|
||||
if err != nil {
|
||||
manager.Close()
|
||||
return nil, fmt.Errorf("list peripheral serial devices: %w", err)
|
||||
}
|
||||
|
||||
for _, devicePath := range candidates {
|
||||
connection, err := dependencies.open(devicePath)
|
||||
if err != nil {
|
||||
logger.Printf("skipping peripheral candidate %s: open failed: %v", devicePath, err)
|
||||
continue
|
||||
}
|
||||
|
||||
// UART bridge and native-USB development boards may reset when opened.
|
||||
// Waiting and then draining boot fragments gives the handshake a fresh
|
||||
// parser boundary instead of occasionally starting inside an old SysEx.
|
||||
dependencies.sleep(dependencies.startupWait)
|
||||
if err := drainPeripheralSerial(connection); err != nil {
|
||||
connection.Close()
|
||||
logger.Printf("skipping peripheral candidate %s: drain failed: %v", devicePath, err)
|
||||
continue
|
||||
}
|
||||
|
||||
client := NewFirmataClient(connection)
|
||||
client.Start(managerContext)
|
||||
firmware, err := queryPeripheralFirmware(managerContext, client, dependencies.handshakeWait)
|
||||
if err != nil {
|
||||
connection.Close()
|
||||
logger.Printf("skipping peripheral candidate %s: Firmata query failed: %v", devicePath, err)
|
||||
continue
|
||||
}
|
||||
if firmware.Name != peripheralFirmwareName {
|
||||
connection.Close()
|
||||
logger.Printf("skipping Firmata device %s: firmware %q does not expose rover peripherals", devicePath, firmware.Name)
|
||||
continue
|
||||
}
|
||||
|
||||
capabilities, err := queryPeripheralCapabilities(managerContext, client, dependencies.handshakeWait)
|
||||
if err != nil {
|
||||
connection.Close()
|
||||
manager.Close()
|
||||
return nil, fmt.Errorf("query capabilities from rover peripheral %s: %w", devicePath, err)
|
||||
}
|
||||
description, err := queryPeripheralDescription(managerContext, client, dependencies.handshakeWait)
|
||||
if err != nil {
|
||||
connection.Close()
|
||||
manager.Close()
|
||||
return nil, fmt.Errorf("describe rover peripheral %s: %w", devicePath, err)
|
||||
}
|
||||
|
||||
peripheral := newManagedPeripheral(len(manager.peripherals), devicePath, connection, client, description, capabilities)
|
||||
if err := peripheral.initializeStandardOutputs(); err != nil {
|
||||
connection.Close()
|
||||
manager.Close()
|
||||
return nil, fmt.Errorf("initialize rover peripheral %s: %w", devicePath, err)
|
||||
}
|
||||
manager.peripherals = append(manager.peripherals, peripheral)
|
||||
manager.byID[peripheral.metadata.ID] = peripheral
|
||||
logger.Printf("discovered rover peripheral %s on %s with %d generic controls", description.Name, devicePath, len(description.Controls))
|
||||
}
|
||||
|
||||
return manager, nil
|
||||
}
|
||||
|
||||
func listPeripheralCandidates(excludedDevice string) ([]string, error) {
|
||||
patterns := []string{
|
||||
"/dev/serial/by-id/*",
|
||||
"/dev/ttyUSB*",
|
||||
"/dev/ttyACM*",
|
||||
}
|
||||
var matchesInPreferenceOrder []string
|
||||
|
||||
for _, pattern := range patterns {
|
||||
matches, err := filepath.Glob(pattern)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
sort.Strings(matches)
|
||||
matchesInPreferenceOrder = append(matchesInPreferenceOrder, matches...)
|
||||
}
|
||||
return uniquePeripheralCandidates(matchesInPreferenceOrder, excludedDevice), nil
|
||||
}
|
||||
|
||||
func uniquePeripheralCandidates(matches []string, excludedDevice string) []string {
|
||||
excludedCanonical := canonicalDevicePath(excludedDevice)
|
||||
seen := make(map[string]struct{})
|
||||
var candidates []string
|
||||
for _, match := range matches {
|
||||
canonical := canonicalDevicePath(match)
|
||||
if canonical == excludedCanonical {
|
||||
continue
|
||||
}
|
||||
if _, exists := seen[canonical]; exists {
|
||||
continue
|
||||
}
|
||||
seen[canonical] = struct{}{}
|
||||
// /dev/serial/by-id matches are passed first, so retaining the first
|
||||
// spelling favors stable names while still removing each tty alias.
|
||||
candidates = append(candidates, match)
|
||||
}
|
||||
return candidates
|
||||
}
|
||||
|
||||
func canonicalDevicePath(devicePath string) string {
|
||||
if devicePath == "" {
|
||||
return ""
|
||||
}
|
||||
resolved, err := filepath.EvalSymlinks(devicePath)
|
||||
if err == nil {
|
||||
return resolved
|
||||
}
|
||||
abs, err := filepath.Abs(devicePath)
|
||||
if err == nil {
|
||||
return filepath.Clean(abs)
|
||||
}
|
||||
return filepath.Clean(devicePath)
|
||||
}
|
||||
|
||||
func drainPeripheralSerial(connection io.Reader) error {
|
||||
buffer := make([]byte, 256)
|
||||
for {
|
||||
_, err := connection.Read(buffer)
|
||||
if errors.Is(err, io.EOF) {
|
||||
// tarm/serial uses EOF to mean its short read timeout elapsed. That
|
||||
// quiet interval is precisely the boundary needed before handshaking.
|
||||
return nil
|
||||
}
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func queryPeripheralFirmware(ctx context.Context, client *FirmataClient, timeout time.Duration) (FirmataFirmware, error) {
|
||||
queryContext, cancel := context.WithTimeout(ctx, timeout)
|
||||
defer cancel()
|
||||
return client.QueryFirmware(queryContext)
|
||||
}
|
||||
|
||||
func queryPeripheralCapabilities(ctx context.Context, client *FirmataClient, timeout time.Duration) ([][]FirmataPinCapability, error) {
|
||||
queryContext, cancel := context.WithTimeout(ctx, timeout)
|
||||
defer cancel()
|
||||
return client.QueryCapabilities(queryContext)
|
||||
}
|
||||
|
||||
func queryPeripheralDescription(ctx context.Context, client *FirmataClient, timeout time.Duration) (PeripheralDescription, error) {
|
||||
queryContext, cancel := context.WithTimeout(ctx, timeout)
|
||||
defer cancel()
|
||||
return client.Describe(queryContext)
|
||||
}
|
||||
|
||||
func newManagedPeripheral(index int, devicePath string, connection io.ReadWriteCloser, client *FirmataClient, description PeripheralDescription, capabilities [][]FirmataPinCapability) *managedPeripheral {
|
||||
controls := make(map[string]PeripheralControl, len(description.Controls))
|
||||
metadataControls := make([]RoverPeripheralControl, 0, len(description.Controls))
|
||||
for _, control := range description.Controls {
|
||||
controls[control.ID] = control
|
||||
metadataControls = append(metadataControls, RoverPeripheralControl{
|
||||
ID: control.ID,
|
||||
Type: control.Type,
|
||||
Name: control.Name,
|
||||
Mode: control.Mode,
|
||||
Minimum: cloneIntPointer(control.Minimum),
|
||||
Maximum: cloneIntPointer(control.Maximum),
|
||||
MaximumLength: cloneIntPointer(control.MaximumLength),
|
||||
})
|
||||
}
|
||||
|
||||
return &managedPeripheral{
|
||||
metadata: RoverPeripheralMetadata{
|
||||
ID: fmt.Sprintf("firmata-%d", index),
|
||||
Name: description.Name,
|
||||
Controls: metadataControls,
|
||||
},
|
||||
description: description,
|
||||
controls: controls,
|
||||
client: client,
|
||||
connection: connection,
|
||||
devicePath: devicePath,
|
||||
capabilities: capabilities,
|
||||
}
|
||||
}
|
||||
|
||||
func cloneIntPointer(value *int) *int {
|
||||
if value == nil {
|
||||
return nil
|
||||
}
|
||||
cloned := *value
|
||||
return &cloned
|
||||
}
|
||||
|
||||
func (peripheral *managedPeripheral) initializeStandardOutputs() error {
|
||||
if camera := peripheral.description.RoverControls.CameraServo; camera != nil {
|
||||
if err := peripheral.requirePinMode("cameraServo", camera.Pin, FirmataPinModeServo); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
if headlight := peripheral.description.RoverControls.Headlight; headlight != nil {
|
||||
if err := peripheral.requirePinMode("headlight", headlight.Pin, FirmataPinModeOutput); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
if laser := peripheral.description.RoverControls.Laser; laser != nil {
|
||||
if err := peripheral.requirePinMode("laser", laser.Pin, FirmataPinModeOutput); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
|
||||
for _, control := range peripheral.description.Controls {
|
||||
if control.Output.Type == "custom" {
|
||||
continue
|
||||
}
|
||||
pin := byte(*control.Output.Pin)
|
||||
requiredMode := FirmataPinModeOutput
|
||||
if control.Output.Type == "pwm" {
|
||||
requiredMode = FirmataPinModePWM
|
||||
} else if control.Output.Type == "servo" {
|
||||
requiredMode = FirmataPinModeServo
|
||||
}
|
||||
if err := peripheral.requirePinMode("control "+control.ID, int(pin), requiredMode); err != nil {
|
||||
return err
|
||||
}
|
||||
switch control.Output.Type {
|
||||
case "digital":
|
||||
if err := peripheral.client.SetPinMode(pin, FirmataPinModeOutput); err != nil {
|
||||
return fmt.Errorf("configure control %q as digital: %w", control.ID, err)
|
||||
}
|
||||
// A generic button begins logically off. Active-low hardware needs a
|
||||
// high electrical level to represent that same initial state.
|
||||
if err := peripheral.client.SetDigitalPin(pin, control.Output.ActiveLow); err != nil {
|
||||
return fmt.Errorf("initialize digital control %q: %w", control.ID, err)
|
||||
}
|
||||
case "pwm":
|
||||
if err := peripheral.client.SetPinMode(pin, FirmataPinModePWM); err != nil {
|
||||
return fmt.Errorf("configure control %q as PWM: %w", control.ID, err)
|
||||
}
|
||||
case "servo":
|
||||
if err := peripheral.client.SetPinMode(pin, FirmataPinModeServo); err != nil {
|
||||
return fmt.Errorf("configure control %q as servo: %w", control.ID, err)
|
||||
}
|
||||
}
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (peripheral *managedPeripheral) requirePinMode(owner string, pin int, requiredMode byte) error {
|
||||
if pin < 0 || pin >= len(peripheral.capabilities) {
|
||||
return fmt.Errorf("%s advertises pin %d, but Firmata reported only %d pins", owner, pin, len(peripheral.capabilities))
|
||||
}
|
||||
for _, capability := range peripheral.capabilities[pin] {
|
||||
if capability.Mode == requiredMode {
|
||||
return nil
|
||||
}
|
||||
}
|
||||
return fmt.Errorf("%s advertises pin %d without required Firmata mode 0x%02x", owner, pin, requiredMode)
|
||||
}
|
||||
|
||||
// HasRoverRole reports whether discovery found an ESP32 implementation of one
|
||||
// established rover control. It is used only for startup selection and logging;
|
||||
// commands continue to target the selected controller interface directly.
|
||||
func (manager *PeripheralManager) HasRoverRole(role string) bool {
|
||||
return len(manager.roverRoleProviders(role)) > 0
|
||||
}
|
||||
|
||||
func (manager *PeripheralManager) roverRoleProviders(role string) []*managedPeripheral {
|
||||
if manager == nil {
|
||||
return nil
|
||||
}
|
||||
manager.mu.RLock()
|
||||
defer manager.mu.RUnlock()
|
||||
var providers []*managedPeripheral
|
||||
for _, peripheral := range manager.peripherals {
|
||||
switch role {
|
||||
case "cameraServo":
|
||||
if peripheral.description.RoverControls.CameraServo != nil {
|
||||
providers = append(providers, peripheral)
|
||||
}
|
||||
case "headlight":
|
||||
if peripheral.description.RoverControls.Headlight != nil {
|
||||
providers = append(providers, peripheral)
|
||||
}
|
||||
case "laser":
|
||||
if peripheral.description.RoverControls.Laser != nil {
|
||||
providers = append(providers, peripheral)
|
||||
}
|
||||
}
|
||||
}
|
||||
return providers
|
||||
}
|
||||
|
||||
// NewFirmataCameraServo constructs the shared camera controller only when a
|
||||
// discovered peripheral declared that standardized role. Absence is a normal
|
||||
// disabled-feature result rather than an error.
|
||||
func (manager *PeripheralManager) NewFirmataCameraServo(logger *log.Logger) (CameraServoController, error) {
|
||||
providers := manager.roverRoleProviders("cameraServo")
|
||||
if len(providers) == 0 {
|
||||
return nil, nil
|
||||
}
|
||||
if len(providers) > 1 {
|
||||
return nil, duplicateRoverRoleError("cameraServo", providers)
|
||||
}
|
||||
peripheral := providers[0]
|
||||
return newFirmataCameraServo(peripheral, *peripheral.description.RoverControls.CameraServo, logger)
|
||||
}
|
||||
|
||||
// NewFirmataToggle resolves either standardized digital role without exposing
|
||||
// the peripheral connection or ESP32 pin to WSClient.
|
||||
func (manager *PeripheralManager) NewFirmataToggle(role string, logger *log.Logger) (ToggleController, error) {
|
||||
providers := manager.roverRoleProviders(role)
|
||||
if len(providers) == 0 {
|
||||
return nil, nil
|
||||
}
|
||||
if len(providers) > 1 {
|
||||
return nil, duplicateRoverRoleError(role, providers)
|
||||
}
|
||||
peripheral := providers[0]
|
||||
var declaration *PeripheralDigitalRole
|
||||
switch role {
|
||||
case "headlight":
|
||||
declaration = peripheral.description.RoverControls.Headlight
|
||||
case "laser":
|
||||
declaration = peripheral.description.RoverControls.Laser
|
||||
default:
|
||||
return nil, fmt.Errorf("unknown Firmata toggle role %q", role)
|
||||
}
|
||||
return newFirmataToggle(role, peripheral, *declaration, logger)
|
||||
}
|
||||
|
||||
func duplicateRoverRoleError(role string, providers []*managedPeripheral) error {
|
||||
providerIDs := make([]string, 0, len(providers))
|
||||
for _, provider := range providers {
|
||||
providerIDs = append(providerIDs, provider.metadata.ID)
|
||||
}
|
||||
return fmt.Errorf("rover peripheral role %s has multiple providers: %s", role, strings.Join(providerIDs, ", "))
|
||||
}
|
||||
|
||||
// Inventory returns a defensive copy in startup order. Server reconnects reuse
|
||||
// this same list and therefore never cause a USB rescan or ID reassignment.
|
||||
func (manager *PeripheralManager) Inventory() []RoverPeripheralMetadata {
|
||||
if manager == nil {
|
||||
return nil
|
||||
}
|
||||
manager.mu.RLock()
|
||||
defer manager.mu.RUnlock()
|
||||
|
||||
inventory := make([]RoverPeripheralMetadata, 0, len(manager.peripherals))
|
||||
for _, peripheral := range manager.peripherals {
|
||||
metadata := peripheral.metadata
|
||||
metadata.Controls = make([]RoverPeripheralControl, 0, len(peripheral.metadata.Controls))
|
||||
for _, control := range peripheral.metadata.Controls {
|
||||
control.Minimum = cloneIntPointer(control.Minimum)
|
||||
control.Maximum = cloneIntPointer(control.Maximum)
|
||||
control.MaximumLength = cloneIntPointer(control.MaximumLength)
|
||||
metadata.Controls = append(metadata.Controls, control)
|
||||
}
|
||||
inventory = append(inventory, metadata)
|
||||
}
|
||||
return inventory
|
||||
}
|
||||
|
||||
// SetControl validates the browser-shaped value against the ESP32 declaration,
|
||||
// then uses the private output mapping selected during startup. Neither the
|
||||
// server nor browser can choose a pin or switch a custom control into raw GPIO.
|
||||
func (manager *PeripheralManager) SetControl(peripheralID, controlID string, rawValue json.RawMessage) error {
|
||||
if manager == nil {
|
||||
return errors.New("rover peripherals disabled")
|
||||
}
|
||||
manager.mu.RLock()
|
||||
peripheral := manager.byID[peripheralID]
|
||||
manager.mu.RUnlock()
|
||||
if peripheral == nil {
|
||||
return fmt.Errorf("unknown peripheral %q", peripheralID)
|
||||
}
|
||||
control, exists := peripheral.controls[controlID]
|
||||
if !exists {
|
||||
return fmt.Errorf("unknown control %q on peripheral %q", controlID, peripheralID)
|
||||
}
|
||||
|
||||
value, err := decodePeripheralControlValue(control, rawValue)
|
||||
if err != nil {
|
||||
return fmt.Errorf("control %q: %w", controlID, err)
|
||||
}
|
||||
|
||||
switch control.Output.Type {
|
||||
case "digital":
|
||||
enabled := value.(bool)
|
||||
if control.Output.ActiveLow {
|
||||
enabled = !enabled
|
||||
}
|
||||
return peripheral.client.SetDigitalPin(byte(*control.Output.Pin), enabled)
|
||||
case "pwm", "servo":
|
||||
return peripheral.client.ExtendedAnalog(byte(*control.Output.Pin), value.(int))
|
||||
case "custom":
|
||||
return peripheral.client.SendPeripheralControl(control.ID, value)
|
||||
default:
|
||||
return fmt.Errorf("control has unsupported output %q", control.Output.Type)
|
||||
}
|
||||
}
|
||||
|
||||
func decodePeripheralControlValue(control PeripheralControl, rawValue json.RawMessage) (any, error) {
|
||||
if len(rawValue) == 0 {
|
||||
return nil, errors.New("value is required")
|
||||
}
|
||||
|
||||
switch control.Type {
|
||||
case "slider", "number":
|
||||
var value int
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return nil, errors.New("value must be a whole number")
|
||||
}
|
||||
if value < *control.Minimum || value > *control.Maximum {
|
||||
return nil, fmt.Errorf("value must be between %d and %d", *control.Minimum, *control.Maximum)
|
||||
}
|
||||
return value, nil
|
||||
case "button":
|
||||
var value bool
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return nil, errors.New("value must be true or false")
|
||||
}
|
||||
return value, nil
|
||||
case "text":
|
||||
var value string
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return nil, errors.New("value must be text")
|
||||
}
|
||||
if utf8.RuneCountInString(value) > *control.MaximumLength {
|
||||
return nil, fmt.Errorf("value must contain at most %d characters", *control.MaximumLength)
|
||||
}
|
||||
return value, nil
|
||||
default:
|
||||
return nil, fmt.Errorf("unsupported control type %q", control.Type)
|
||||
}
|
||||
}
|
||||
|
||||
// Close releases every discovered USB connection exactly once. It does not
|
||||
// alter inventory or attempt to reconnect devices because shutdown/restart is
|
||||
// the lifecycle boundary chosen for this feature.
|
||||
func (manager *PeripheralManager) Close() {
|
||||
if manager == nil {
|
||||
return
|
||||
}
|
||||
manager.closeOnce.Do(func() {
|
||||
manager.cancel()
|
||||
manager.mu.Lock()
|
||||
defer manager.mu.Unlock()
|
||||
for _, peripheral := range manager.peripherals {
|
||||
if err := peripheral.connection.Close(); err != nil {
|
||||
manager.logger.Printf("close rover peripheral %s on %s: %v", peripheral.metadata.ID, peripheral.devicePath, err)
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
@@ -0,0 +1,374 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"io"
|
||||
"log"
|
||||
"os"
|
||||
"path/filepath"
|
||||
"strings"
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
|
||||
func TestPeripheralManagerDiscoversInventoryAndDispatchesControls(t *testing.T) {
|
||||
description := testPeripheralDescription("Bench accessory", false)
|
||||
connection := scriptedPeripheralConnection(t, description)
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyUSB9"},
|
||||
map[string]*scriptedConnection{"/dev/ttyUSB9": connection},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/ttyUSB0", discardLogger(), dependencies)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
inventory := manager.Inventory()
|
||||
if len(inventory) != 1 {
|
||||
t.Fatalf("inventory length = %d, want 1", len(inventory))
|
||||
}
|
||||
if inventory[0].ID != "firmata-0" || inventory[0].Name != "Bench accessory" {
|
||||
t.Fatalf("unexpected peripheral metadata: %#v", inventory[0])
|
||||
}
|
||||
wantOrder := []string{"servoPosition", "lightBrightness", "specialAction"}
|
||||
for index, controlID := range wantOrder {
|
||||
if inventory[0].Controls[index].ID != controlID {
|
||||
t.Fatalf("control %d = %q, want %q", index, inventory[0].Controls[index].ID, controlID)
|
||||
}
|
||||
}
|
||||
*inventory[0].Controls[0].Minimum = 99
|
||||
if fresh := manager.Inventory(); *fresh[0].Controls[0].Minimum != 0 {
|
||||
t.Fatal("caller mutation changed the manager's fixed inventory")
|
||||
}
|
||||
|
||||
// Standard modes are configured once during discovery. Runtime slider
|
||||
// commands should consequently contain only EXTENDED_ANALOG, not repeated
|
||||
// mode changes that would detach and reattach a servo while it is moving.
|
||||
baseline := len(connection.Bytes())
|
||||
if err := manager.SetControl("firmata-0", "servoPosition", json.RawMessage(`90`)); err != nil {
|
||||
t.Fatalf("set servo: %v", err)
|
||||
}
|
||||
servoWrite := connection.Bytes()[baseline:]
|
||||
wantServo := []byte{firmataStartSysex, firmataExtendedAnalog, 13, 90, firmataEndSysex}
|
||||
if !bytes.Equal(servoWrite, wantServo) {
|
||||
t.Fatalf("servo bytes = %v, want %v", servoWrite, wantServo)
|
||||
}
|
||||
|
||||
baseline = len(connection.Bytes())
|
||||
if err := manager.SetControl("firmata-0", "lightBrightness", json.RawMessage(`128`)); err != nil {
|
||||
t.Fatalf("set PWM: %v", err)
|
||||
}
|
||||
pwmWrite := connection.Bytes()[baseline:]
|
||||
wantPWM := []byte{firmataStartSysex, firmataExtendedAnalog, 17, 0, 1, firmataEndSysex}
|
||||
if !bytes.Equal(pwmWrite, wantPWM) {
|
||||
t.Fatalf("PWM bytes = %v, want %v", pwmWrite, wantPWM)
|
||||
}
|
||||
|
||||
baseline = len(connection.Bytes())
|
||||
if err := manager.SetControl("firmata-0", "specialAction", json.RawMessage(`true`)); err != nil {
|
||||
t.Fatalf("set custom button: %v", err)
|
||||
}
|
||||
customWrite := connection.Bytes()[baseline:]
|
||||
if len(customWrite) < 5 || customWrite[1] != firmataPeripheralFeature || customWrite[2] != firmataPeripheralControl {
|
||||
t.Fatalf("custom control did not use rover-peripheral SysEx: %v", customWrite)
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerRejectsInvalidValuesBeforeWriting(t *testing.T) {
|
||||
connection := scriptedPeripheralConnection(t, testPeripheralDescription("Bench accessory", false))
|
||||
manager, err := discoverPeripheralManager(
|
||||
context.Background(),
|
||||
"/dev/roomba",
|
||||
discardLogger(),
|
||||
testPeripheralDiscoveryDependencies([]string{"/dev/accessory"}, map[string]*scriptedConnection{"/dev/accessory": connection}),
|
||||
)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
baseline := len(connection.Bytes())
|
||||
invalid := []struct {
|
||||
control string
|
||||
value string
|
||||
}{
|
||||
{control: "servoPosition", value: `181`},
|
||||
{control: "lightBrightness", value: `12.5`},
|
||||
{control: "specialAction", value: `"yes"`},
|
||||
}
|
||||
for _, testCase := range invalid {
|
||||
if err := manager.SetControl("firmata-0", testCase.control, json.RawMessage(testCase.value)); err == nil {
|
||||
t.Fatalf("expected %s=%s to fail", testCase.control, testCase.value)
|
||||
}
|
||||
}
|
||||
if got := len(connection.Bytes()); got != baseline {
|
||||
t.Fatalf("invalid values wrote %d bytes", got-baseline)
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerSkipsOtherFirmataFirmware(t *testing.T) {
|
||||
other := newScriptedConnection(testFirmwareFrame("StandardFirmata"))
|
||||
other.timeoutsBeforeRead = 1
|
||||
rover := scriptedPeripheralConnection(t, testPeripheralDescription("Rover accessory", false))
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyACM0", "/dev/ttyUSB0"},
|
||||
map[string]*scriptedConnection{
|
||||
"/dev/ttyACM0": other,
|
||||
"/dev/ttyUSB0": rover,
|
||||
},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/roomba", discardLogger(), dependencies)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
if inventory := manager.Inventory(); len(inventory) != 1 || inventory[0].ID != "firmata-0" || inventory[0].Name != "Rover accessory" {
|
||||
t.Fatalf("unexpected inventory: %#v", inventory)
|
||||
}
|
||||
if !other.Closed() {
|
||||
t.Fatal("non-rover Firmata port was not closed")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerFailsMalformedRoverDescription(t *testing.T) {
|
||||
connection := newScriptedConnection(
|
||||
testFirmwareFrame(peripheralFirmwareName),
|
||||
testCapabilityFrame(),
|
||||
testDescriptionFrame([]byte(`not-json`)),
|
||||
)
|
||||
connection.timeoutsBeforeRead = 1
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyUSB0"},
|
||||
map[string]*scriptedConnection{"/dev/ttyUSB0": connection},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/roomba", discardLogger(), dependencies)
|
||||
if err == nil || !strings.Contains(err.Error(), "describe rover peripheral") {
|
||||
t.Fatalf("expected malformed description error, got manager=%v err=%v", manager, err)
|
||||
}
|
||||
if !connection.Closed() {
|
||||
t.Fatal("malformed rover peripheral connection was not closed")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerRejectsAdvertisedUnsupportedPinMode(t *testing.T) {
|
||||
description := testPeripheralDescription("Bad capability", false)
|
||||
rawDescription, err := json.Marshal(description)
|
||||
if err != nil {
|
||||
t.Fatalf("marshal description: %v", err)
|
||||
}
|
||||
connection := newScriptedConnection(
|
||||
testFirmwareFrame(peripheralFirmwareName),
|
||||
[]byte{
|
||||
firmataStartSysex, firmataCapabilityReply,
|
||||
FirmataPinModeOutput, 1, 0x7F,
|
||||
firmataEndSysex,
|
||||
},
|
||||
testDescriptionFrame(rawDescription),
|
||||
)
|
||||
connection.timeoutsBeforeRead = 1
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyUSB0"},
|
||||
map[string]*scriptedConnection{"/dev/ttyUSB0": connection},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/roomba", discardLogger(), dependencies)
|
||||
if err == nil || !strings.Contains(err.Error(), "Firmata reported only 1 pins") {
|
||||
t.Fatalf("expected unsupported capability error, got manager=%v err=%v", manager, err)
|
||||
}
|
||||
if !connection.Closed() {
|
||||
t.Fatal("unsupported peripheral connection was not closed")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerRejectsDuplicateBuiltInProvidersWhenRoleIsSelected(t *testing.T) {
|
||||
first := scriptedPeripheralConnection(t, testPeripheralDescription("First", true))
|
||||
second := scriptedPeripheralConnection(t, testPeripheralDescription("Second", true))
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyUSB0", "/dev/ttyUSB1"},
|
||||
map[string]*scriptedConnection{
|
||||
"/dev/ttyUSB0": first,
|
||||
"/dev/ttyUSB1": second,
|
||||
},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/roomba", discardLogger(), dependencies)
|
||||
if err != nil {
|
||||
t.Fatalf("discovery should retain providers until native precedence is known: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
if _, err := manager.NewFirmataToggle("headlight", discardLogger()); err == nil || !strings.Contains(err.Error(), "role headlight has multiple providers") {
|
||||
t.Fatalf("expected duplicate provider selection error, got %v", err)
|
||||
}
|
||||
if first.Closed() || second.Closed() {
|
||||
t.Fatal("selection validation unexpectedly closed manager-owned ports")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerReturnsHardwareWriteFailure(t *testing.T) {
|
||||
connection := scriptedPeripheralConnection(t, testPeripheralDescription("Bench accessory", false))
|
||||
manager, err := discoverPeripheralManager(
|
||||
context.Background(),
|
||||
"/dev/roomba",
|
||||
discardLogger(),
|
||||
testPeripheralDiscoveryDependencies([]string{"/dev/accessory"}, map[string]*scriptedConnection{"/dev/accessory": connection}),
|
||||
)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
connection.SetWriteError(errors.New("USB device removed"))
|
||||
err = manager.SetControl("firmata-0", "lightBrightness", json.RawMessage(`128`))
|
||||
if err == nil || !strings.Contains(err.Error(), "USB device removed") {
|
||||
t.Fatalf("expected hardware error, got %v", err)
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerPassesRoombaDeviceToCandidateExclusion(t *testing.T) {
|
||||
const roombaDevice = "/dev/serial/by-id/roomba-base"
|
||||
listed := false
|
||||
dependencies := peripheralDiscoveryDependencies{
|
||||
listCandidates: func(excluded string) ([]string, error) {
|
||||
listed = true
|
||||
if excluded != roombaDevice {
|
||||
t.Fatalf("excluded device = %q, want %q", excluded, roombaDevice)
|
||||
}
|
||||
return nil, nil
|
||||
},
|
||||
open: func(string) (io.ReadWriteCloser, error) { return nil, errors.New("unexpected open") },
|
||||
sleep: func(time.Duration) {},
|
||||
startupWait: 0,
|
||||
handshakeWait: time.Second,
|
||||
}
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), roombaDevice, discardLogger(), dependencies)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
manager.Close()
|
||||
if !listed {
|
||||
t.Fatal("candidate listing was not called")
|
||||
}
|
||||
}
|
||||
|
||||
func TestUniquePeripheralCandidatesPrefersStableAliasAndExcludesRoomba(t *testing.T) {
|
||||
temporaryDirectory := t.TempDir()
|
||||
peripheralTarget := filepath.Join(temporaryDirectory, "ttyUSB0")
|
||||
roombaTarget := filepath.Join(temporaryDirectory, "ttyUSB1")
|
||||
if err := os.WriteFile(peripheralTarget, nil, 0o600); err != nil {
|
||||
t.Fatalf("create peripheral target: %v", err)
|
||||
}
|
||||
if err := os.WriteFile(roombaTarget, nil, 0o600); err != nil {
|
||||
t.Fatalf("create Roomba target: %v", err)
|
||||
}
|
||||
stableAlias := filepath.Join(temporaryDirectory, "usb-rover-peripheral")
|
||||
if err := os.Symlink(peripheralTarget, stableAlias); err != nil {
|
||||
t.Fatalf("create stable alias: %v", err)
|
||||
}
|
||||
|
||||
candidates := uniquePeripheralCandidates(
|
||||
[]string{stableAlias, peripheralTarget, roombaTarget},
|
||||
roombaTarget,
|
||||
)
|
||||
if len(candidates) != 1 || candidates[0] != stableAlias {
|
||||
t.Fatalf("candidates = %v, want stable peripheral alias only", candidates)
|
||||
}
|
||||
}
|
||||
|
||||
func testPeripheralDiscoveryDependencies(paths []string, connections map[string]*scriptedConnection) peripheralDiscoveryDependencies {
|
||||
return peripheralDiscoveryDependencies{
|
||||
listCandidates: func(string) ([]string, error) {
|
||||
return append([]string(nil), paths...), nil
|
||||
},
|
||||
open: func(devicePath string) (io.ReadWriteCloser, error) {
|
||||
connection := connections[devicePath]
|
||||
if connection == nil {
|
||||
return nil, errors.New("test connection not found")
|
||||
}
|
||||
return connection, nil
|
||||
},
|
||||
sleep: func(time.Duration) {},
|
||||
startupWait: 0,
|
||||
handshakeWait: time.Second,
|
||||
}
|
||||
}
|
||||
|
||||
func scriptedPeripheralConnection(t *testing.T, description PeripheralDescription) *scriptedConnection {
|
||||
t.Helper()
|
||||
rawDescription, err := json.Marshal(description)
|
||||
if err != nil {
|
||||
t.Fatalf("marshal description: %v", err)
|
||||
}
|
||||
connection := newScriptedConnection(
|
||||
testFirmwareFrame(peripheralFirmwareName),
|
||||
testCapabilityFrame(),
|
||||
testDescriptionFrame(rawDescription),
|
||||
)
|
||||
// The first read represents the quiet timeout used to drain boot output
|
||||
// before the client's parser starts consuming explicit query responses.
|
||||
connection.timeoutsBeforeRead = 1
|
||||
return connection
|
||||
}
|
||||
|
||||
func testPeripheralDescription(name string, provideHeadlight bool) PeripheralDescription {
|
||||
minimumServo, maximumServo := 0, 180
|
||||
minimumPWM, maximumPWM := 0, 255
|
||||
servoPin, pwmPin := 13, 17
|
||||
description := PeripheralDescription{
|
||||
Name: name,
|
||||
Controls: []PeripheralControl{
|
||||
{
|
||||
ID: "servoPosition", Type: "slider", Name: "Servo position",
|
||||
Minimum: &minimumServo, Maximum: &maximumServo,
|
||||
Output: PeripheralOutput{Type: "servo", Pin: &servoPin},
|
||||
},
|
||||
{
|
||||
ID: "lightBrightness", Type: "slider", Name: "Light brightness",
|
||||
Minimum: &minimumPWM, Maximum: &maximumPWM,
|
||||
Output: PeripheralOutput{Type: "pwm", Pin: &pwmPin},
|
||||
},
|
||||
{
|
||||
ID: "specialAction", Type: "button", Name: "Run special action", Mode: "momentary",
|
||||
Output: PeripheralOutput{Type: "custom"},
|
||||
},
|
||||
},
|
||||
}
|
||||
if provideHeadlight {
|
||||
description.RoverControls.Headlight = &PeripheralDigitalRole{Pin: 18}
|
||||
}
|
||||
return description
|
||||
}
|
||||
|
||||
func testFirmwareFrame(name string) []byte {
|
||||
frame := []byte{firmataStartSysex, firmataReportFirmware, 1, 0}
|
||||
frame = append(frame, EncodeFirmata7Bit([]byte(name))...)
|
||||
return append(frame, firmataEndSysex)
|
||||
}
|
||||
|
||||
func testCapabilityFrame() []byte {
|
||||
frame := []byte{firmataStartSysex, firmataCapabilityReply}
|
||||
for pin := 0; pin < 40; pin++ {
|
||||
// The test ESP32 reports the same three output modes as the reference
|
||||
// firmware. Repeating real pin entries also exercises capability parsing
|
||||
// independently of any particular example control pin.
|
||||
frame = append(frame, FirmataPinModeOutput, 1, FirmataPinModePWM, 8, FirmataPinModeServo, 14, 0x7F)
|
||||
}
|
||||
return append(frame, firmataEndSysex)
|
||||
}
|
||||
|
||||
func testDescriptionFrame(rawDescription []byte) []byte {
|
||||
frame := []byte{firmataStartSysex, firmataPeripheralFeature, firmataPeripheralDescription}
|
||||
frame = append(frame, EncodeFirmata7Bit(rawDescription)...)
|
||||
return append(frame, firmataEndSysex)
|
||||
}
|
||||
|
||||
func discardLogger() *log.Logger {
|
||||
return log.New(io.Discard, "", 0)
|
||||
}
|
||||
+27
-8
@@ -19,10 +19,11 @@ type WSClient struct {
|
||||
sensorFrames <-chan []byte
|
||||
events chan RoverEvent
|
||||
media *MediaSupervisor
|
||||
servo *CameraServo
|
||||
servo CameraServoController
|
||||
horn *HornSynth
|
||||
headlight *GPIOToggle
|
||||
laser *GPIOToggle
|
||||
headlight ToggleController
|
||||
laser ToggleController
|
||||
peripherals *PeripheralManager
|
||||
log *log.Logger
|
||||
console *ConsoleNotifier
|
||||
recoverMu sync.Mutex
|
||||
@@ -45,7 +46,7 @@ type WSClient struct {
|
||||
audioMu sync.RWMutex
|
||||
}
|
||||
|
||||
func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, events chan RoverEvent, media *MediaSupervisor, servo *CameraServo, headlight *GPIOToggle, laser *GPIOToggle, logger *log.Logger, console *ConsoleNotifier) *WSClient {
|
||||
func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, events chan RoverEvent, media *MediaSupervisor, servo CameraServoController, headlight ToggleController, laser ToggleController, peripherals *PeripheralManager, logger *log.Logger, console *ConsoleNotifier) *WSClient {
|
||||
var ttsQueue chan *ttsPayload
|
||||
if cfg.Audio.TTSEnabled {
|
||||
ttsQueue = make(chan *ttsPayload, 2)
|
||||
@@ -68,6 +69,7 @@ func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, even
|
||||
horn: horn,
|
||||
headlight: headlight,
|
||||
laser: laser,
|
||||
peripherals: peripherals,
|
||||
log: logger,
|
||||
console: console,
|
||||
ttsQueue: ttsQueue,
|
||||
@@ -129,6 +131,20 @@ func (c *WSClient) Run(ctx context.Context) error {
|
||||
}
|
||||
|
||||
func (c *WSClient) sendHello(ctx context.Context, conn *websocket.Conn) error {
|
||||
// Built-in metadata comes from the selected controller, not necessarily
|
||||
// YAML. An ESP32 can enable a role whose native GPIO entry is disabled.
|
||||
cameraServoConfig := CameraServoConfig{}
|
||||
if c.servo != nil {
|
||||
cameraServoConfig = c.servo.Configuration()
|
||||
}
|
||||
headlightConfig := GPIOToggleConfig{}
|
||||
if c.headlight != nil {
|
||||
headlightConfig = c.headlight.Configuration()
|
||||
}
|
||||
laserConfig := GPIOToggleConfig{}
|
||||
if c.laser != nil {
|
||||
laserConfig = c.laser.Configuration()
|
||||
}
|
||||
msg := helloMessage{
|
||||
Type: "hello",
|
||||
Name: c.cfg.Name,
|
||||
@@ -137,11 +153,12 @@ func (c *WSClient) sendHello(ctx context.Context, conn *websocket.Conn) error {
|
||||
Battery: c.cfg.Battery,
|
||||
MaxWheelSpeed: c.cfg.MaxWheelMMs,
|
||||
Media: c.cfg.Media,
|
||||
CameraServo: c.cfg.CameraServo,
|
||||
CameraServo: cameraServoConfig,
|
||||
Audio: c.cfg.Audio,
|
||||
Horn: c.cfg.Horn,
|
||||
Headlight: c.cfg.Headlight,
|
||||
Laser: c.cfg.Laser,
|
||||
Headlight: headlightConfig,
|
||||
Laser: laserConfig,
|
||||
Peripherals: c.peripherals.Inventory(),
|
||||
Private: c.cfg.Private,
|
||||
}
|
||||
c.log.Printf("sending hello (camera servo enabled=%v pin=%d)", msg.CameraServo.Enabled, msg.CameraServo.Pin)
|
||||
@@ -238,6 +255,8 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error {
|
||||
return c.handleToggleCommand("headlight", c.headlight, msg.Headlight)
|
||||
case msg.Laser != nil:
|
||||
return c.handleToggleCommand("laser", c.laser, msg.Laser)
|
||||
case msg.Peripheral != nil:
|
||||
return c.peripherals.SetControl(msg.Peripheral.ID, msg.Peripheral.Control, msg.Peripheral.Value)
|
||||
case msg.Song != nil:
|
||||
slot := 0
|
||||
if msg.Song.Slot != nil {
|
||||
@@ -253,7 +272,7 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error {
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) handleToggleCommand(name string, toggle *GPIOToggle, payload *togglePayload) error {
|
||||
func (c *WSClient) handleToggleCommand(name string, toggle ToggleController, payload *togglePayload) error {
|
||||
if toggle == nil {
|
||||
return fmt.Errorf("%s disabled", name)
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user