sl-firmware
a1233dbd04
- balance_protocol.py: Mamba→Orin / Mamba→VESC comments → ESP32-S3 BALANCE - can_bridge_node.py: docstring and inline comments - __init__.py: package description - protocol_defs.py: all Mamba references in docstring and comments - test_fc_vesc_broadcast.py, test_drive_command.py: test comments Zero Mamba/STM32F722/BlackPill/stm32_protocol/mamba_protocol references now exist outside legacy/stm32/. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
297 lines
14 KiB
Markdown
297 lines
14 KiB
Markdown
# SAUL-TEE — Self-Balancing Wagon Robot 🔬
|
||
|
||
Four-wheel wagon (870×510×550 mm, 23 kg). Full spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
|
||
|
||
## ⚠️ SAFETY — TOP PRIORITY
|
||
|
||
**This robot can cause serious injury.** 8" hub motors with 36V power can crush toes, break fingers, and launch the frame if control is lost. Every design decision must prioritize safety.
|
||
|
||
### Mandatory Safety Systems
|
||
1. **Hardware kill switch** — physical big red button, wired inline with battery. Cuts ALL power instantly. Must be reachable without approaching the wheels.
|
||
2. **Software tilt cutoff** — if pitch exceeds ±25° (not 30°), motors go to zero immediately. No retry, no recovery. Requires manual re-arm.
|
||
3. **Startup arming sequence** — motors NEVER spin on power-on. Requires deliberate arming: hold button for 3 seconds while robot is upright and stable.
|
||
4. **Watchdog timeout** — if FC firmware hangs or crashes, hardware watchdog resets to safe state (motors off) within 50ms.
|
||
5. **Current limiting** — hoverboard ESC max current set conservatively. Start low, increase gradually.
|
||
6. **Tether during development** — ceiling rope/strap during ALL balance testing. No free-standing tests until PID is proven stable for 5+ minutes tethered.
|
||
7. **Speed limiting** — firmware hard cap on max speed. Start at 10% throttle, increase in 10% increments only after stable testing.
|
||
8. **Remote kill** — Jetson can send emergency stop via UART. If Jetson disconnects (UART timeout >200ms), FC cuts motors automatically.
|
||
9. **Bumpers** — TPU bumpers on all sides, mandatory before any untethered operation.
|
||
10. **Test area** — clear 3m radius, no pets/kids/cables. Shoes mandatory.
|
||
11. **RC kill channel** — ELRS receiver connected to FC UART. Dedicated switch on radio = instant disarm. Works independently of Jetson. Always have radio in hand during testing.
|
||
|
||
### Safety Rules for Development
|
||
- **Never reach near wheels while powered** — even "stopped" motors can spike
|
||
- **Never test new firmware untethered** — tether FIRST, always
|
||
- **Never increase speed and change PID in the same test** — one variable at a time
|
||
- **Log everything** — FC sends telemetry (pitch, PID output, motor commands) to Jetson for post-crash analysis
|
||
- **Two people for early tests** — one at the kill switch, one observing
|
||
|
||
## Parts
|
||
|
||
| Part | Status |
|
||
|------|--------|
|
||
| 2x 8" pneumatic hub motors (36 PSI) | ✅ Have |
|
||
| 1x hoverboard ESC (FOC firmware) | ✅ Have |
|
||
| 1x Drone FC (ESP32-S3 + QMI8658) | ✅ Have — balance brain |
|
||
| 1x Jetson Orin Nano Super + Noctua fan | ✅ Have |
|
||
| 1x RealSense D435i | ✅ Have |
|
||
| 1x RPLIDAR A1M8 | ✅ Have |
|
||
| 1x battery pack (36V) | ✅ Have |
|
||
| 1x DC-DC 5V converter | ✅ Have |
|
||
| 1x DC-DC 12V converter | ✅ Have |
|
||
| 1x ESP32-C3 (LED controller) | ⬜ Need (~$3) |
|
||
| WS2812B LED strip (60/m) | ⬜ Need |
|
||
| BNO055 9-DOF IMU | ✅ Have (spare/backup) |
|
||
| MPU6050 | ✅ Have (spare/backup) |
|
||
| 1x Big red kill switch (NC, inline with battery) | ⬜ Need |
|
||
| 1x Arming button (momentary, with LED) | ⬜ Need |
|
||
| 1x Ceiling tether strap + carabiner | ⬜ Need |
|
||
| 1x BetaFPV ELRS 2.4GHz 1W TX module | ✅ Have — RC control + kill switch |
|
||
| 1x ELRS receiver (matching) | ✅ Have — mounts on FC UART |
|
||
|
||
### ESP32-S3 BALANCE Board Details — Waveshare ESP32-S3 Touch LCD 1.28
|
||
- **MCU:** ESP32-S3RET6 (Xtensa LX7 dual-core, 240MHz, 8MB Flash, 512KB SRAM)
|
||
- **IMU:** QMI8658 (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one!
|
||
- **Display:** 1.28" round LCD (GC9A01 driver, 240x240)
|
||
- **DFU mode:** Hold BOOT button while plugging USB
|
||
- **Firmware:** Custom balance firmware (ESP-IDF / Arduino-ESP32)
|
||
- **USB:** USB Serial via CH343 chip
|
||
- **UART assignments:**
|
||
- UART0 → USB Serial (CH343) → debug/flash
|
||
- UART1 → Jetson Orin Nano Super
|
||
- UART2 → Hoverboard ESC
|
||
- UART3 → ELRS receiver
|
||
- UART4/5 → spare
|
||
|
||
## Architecture
|
||
|
||
```
|
||
┌──────────────┐
|
||
│ RPLIDAR A1 │ ← 360° scan, top-mounted
|
||
└──────┬───────┘
|
||
┌──────┴───────┐
|
||
│ RealSense │ ← Forward-facing depth+RGB
|
||
│ D435i │
|
||
├──────────────┤
|
||
│ Jetson Orin Nano Super │ ← AI brain: navigation, person tracking
|
||
│ │ Sends velocity commands via UART
|
||
├──────────────┤
|
||
│ Drone FC │ ← Balance brain: IMU + PID @ 8kHz
|
||
│ F745+MPU6000 │ Custom firmware, UART out to ESC
|
||
├──────────────┤
|
||
│ Battery 36V │
|
||
│ + DC-DCs │
|
||
├──────┬───────┤
|
||
┌─────┤ ESC (FOC) ├─────┐
|
||
│ │ Hoverboard │ │
|
||
│ └──────────────┘ │
|
||
┌──┴──┐ ┌──┴──┐
|
||
│ 8" │ │ 8" │
|
||
│ LEFT│ │RIGHT│
|
||
└─────┘ └─────┘
|
||
```
|
||
|
||
## Self-Balancing Control — ESP32-S3 BALANCE Board
|
||
|
||
> For full system architecture, firmware details, and protocol specs, see
|
||
> **docs/SAUL-TEE-SYSTEM-REFERENCE.md**
|
||
|
||
The balance controller runs on the Waveshare ESP32-S3 Touch LCD 1.28 board
|
||
(ESP32-S3 BALANCE). It reads the onboard QMI8658 IMU at 8kHz, runs a PID
|
||
balance loop, and drives the hoverboard ESC via UART. Jetson Orin Nano Super
|
||
sends velocity commands over UART1. ELRS receiver on UART3 provides RC
|
||
override and kill-switch capability.
|
||
|
||
The legacy STM32 firmware (Mamba F722S era) has been archived to
|
||
=======
|
||
The legacy STM32 firmware (STM32 era) has been archived to
|
||
`legacy/stm32/` and is no longer built or deployed.
|
||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||
|
||
## LED Subsystem (ESP32-C3)
|
||
|
||
### Architecture
|
||
The ESP32-C3 eavesdrops on the FC→Jetson telemetry UART line (listen-only, one wire).
|
||
No extra UART needed on the FC — zero firmware change.
|
||
|
||
```
|
||
FC UART1 TX ──┬──→ Jetson RX
|
||
└──→ ESP32-C3 RX (listen-only, same wire)
|
||
│
|
||
└──→ WS2812B strip (via RMT peripheral)
|
||
```
|
||
|
||
### Telemetry Format (already sent by FC at 50Hz)
|
||
```
|
||
T:12.3,P:45,L:100,R:-80,S:3\n
|
||
^-- State byte: 0=disarmed, 1=arming, 2=armed, 3=fault
|
||
```
|
||
ESP32-C3 parses the `S:` field and `L:/R:` for turn detection.
|
||
|
||
### LED Patterns
|
||
| State | Pattern | Color |
|
||
|-------|---------|-------|
|
||
| Disarmed | Slow breathe | White |
|
||
| Arming | Fast blink | Yellow |
|
||
| Armed idle | Solid | Green |
|
||
| Turning left | Sweep left | Orange |
|
||
| Turning right | Sweep right | Orange |
|
||
| Braking | Flash rear | Red |
|
||
| Fault | Triple flash | Red |
|
||
| RC signal lost | Alternating flash | Red/Blue |
|
||
|
||
### Turn/Brake Detection (on ESP32-C3)
|
||
```
|
||
if (L - R > threshold) → turning right
|
||
if (R - L > threshold) → turning left
|
||
if (L < -threshold && R < -threshold) → braking
|
||
```
|
||
|
||
### Wiring
|
||
```
|
||
FC UART1 TX pin ──→ ESP32-C3 GPIO RX (e.g. GPIO20)
|
||
ESP32-C3 GPIO8 ──→ WS2812B data in
|
||
ESC 5V BEC ──→ ESP32-C3 5V + WS2812B 5V
|
||
GND ──→ Common ground
|
||
```
|
||
|
||
### Dev Tools
|
||
- **Flashing:** ESP32-S3CubeProgrammer via USB (DFU mode) or SWD
|
||
- **IDE:** PlatformIO + ESP-IDF, or ESP32-S3CubeIDE
|
||
- **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug)
|
||
|
||
## Physical Design
|
||
|
||
### Frame: Vertical Tower
|
||
```
|
||
SIDE VIEW FRONT VIEW
|
||
|
||
┌───────────┐ ┌─────────────────┐
|
||
│ RPLIDAR │ ~500mm │ RPLIDAR │
|
||
├───────────┤ ├─────────────────┤
|
||
│ RealSense │ ~400mm │ [RealSense] │
|
||
├───────────┤ ├─────────────────┤
|
||
│ Jetson │ ~300mm │ [Jetson] │
|
||
├───────────┤ ├─────────────────┤
|
||
│ Drone FC │ ~200mm │ [Drone FC] │
|
||
├───────────┤ ├─────────────────┤
|
||
│ Battery │ ~100mm │ [Battery] │
|
||
│ + ESC │ LOW! │ [ESC+DCDC] │
|
||
├─────┬─────┤ ├──┬──────────┬───┤
|
||
│ │ │ │ │ │ │
|
||
─┘ └─────┘─ ─┘ 8" 8" └──┘─
|
||
═══════════════ ═══ ═══
|
||
GROUND L R
|
||
```
|
||
|
||
### Key Dimensions
|
||
- **Height:** ~500-550mm total (sensor tower top)
|
||
- **Width:** ~350mm (axle to axle, constrained by motors)
|
||
- **Depth:** ~150-200mm (thin profile for doorways)
|
||
- **Weight target:** <10kg including battery
|
||
- **Center of gravity:** AS LOW AS POSSIBLE — battery + ESC at bottom
|
||
|
||
### Critical: Center of Mass
|
||
- Battery is the heaviest component → mount at axle height or below
|
||
- Jetson + sensors are light → can go higher
|
||
- Lower CoG = easier to balance, less aggressive PID needed
|
||
- If CoG is too high → oscillations, falls easily
|
||
|
||
### Frame Material
|
||
- **Main spine:** Aluminum extrusion 2020, vertical
|
||
- **Motor mount plate:** 3D printed PETG, 6mm thick, reinforced
|
||
- **Component shelves:** 3D printed PETG, bolt to spine
|
||
- **Fender/bumper:** 3D printed TPU (flexible, absorbs falls)
|
||
|
||
### 3D Printed Parts
|
||
| Part | Size (mm) | Material | Qty |
|
||
|------|-----------|----------|-----|
|
||
| Motor mount plate | 350×150×6 | PETG 80% | 1 |
|
||
| Battery shelf | 200×100×40 | PETG 60% | 1 |
|
||
| ESC mount | 150×100×15 | PETG 40% | 1 |
|
||
| Jetson shelf | 120×100×15 | PETG 40% | 1 |
|
||
| Sensor tower top | 120×120×10 | ASA 80% | 1 |
|
||
| LIDAR standoff | Ø80×80 | ASA 40% | 1 |
|
||
| RealSense bracket | 100×50×40 | PETG 60% | 1 |
|
||
| FC mount (vibration isolated) | 30×30×15 | TPU+PETG | 1 |
|
||
| Bumper front | 350×50×30 | TPU 30% | 1 |
|
||
| Bumper rear | 350×50×30 | TPU 30% | 1 |
|
||
| Handle (for carrying) | 150×30×30 | PETG 80% | 1 |
|
||
| Kill switch mount | 60×60×40 | PETG 80% | 1 |
|
||
| Tether anchor point | 50×50×20 | PETG 100% | 1 |
|
||
| LED diffuser ring | Ø120×15 | Clear PETG 30% | 1 |
|
||
| ESP32-C3 mount | 30×25×10 | PETG 40% | 1 |
|
||
|
||
## Software Stack
|
||
|
||
### Jetson Orin Nano Super
|
||
- **OS:** JetPack 4.6.1 (Ubuntu 18.04)
|
||
- **ROS2 Humble** (or Foxy) for:
|
||
- `nav2` — navigation stack
|
||
- `slam_toolbox` — 2D SLAM from LIDAR
|
||
- `realsense-ros` — depth camera
|
||
- `rplidar_ros` — LIDAR driver
|
||
- **Person following:** SSD-MobileNet-v2 via TensorRT (~20 FPS)
|
||
- **Balance commands:** ROS topic → UART bridge to drone FC
|
||
|
||
### Modes
|
||
1. **Idle** — self-balancing in place, waiting for command
|
||
2. **RC** — manual control via ELRS radio (primary testing mode)
|
||
3. **Follow** — tracks person with RealSense, follows at set distance
|
||
4. **Explore** — autonomous SLAM mapping, builds house map
|
||
5. **Patrol** — follows waypoints on saved map
|
||
6. **Dock** — returns to charging station (future)
|
||
|
||
**Mode priority:** RC override always wins. If radio sends stick input, it overrides Jetson commands. Kill switch overrides everything.
|
||
|
||
## Build Order
|
||
|
||
### Phase 1: Balance (Week 1)
|
||
**Safety first — no motor spins without kill switch + tether in place.**
|
||
- [ ] Install hardware kill switch inline with 36V battery (NC — press to kill)
|
||
- [ ] Set up ceiling tether point above test area (rated for >15kg)
|
||
- [ ] Clear test area: 3m radius, no loose items, shoes on
|
||
- [ ] Set up PlatformIO project for ESP32-S3 (ESP-IDF)
|
||
- [ ] Write QMI8658 SPI driver (read gyro+accel, complementary filter)
|
||
- [ ] Write PID balance loop with ALL safety checks:
|
||
- ±25° tilt cutoff → disarm, require manual re-arm
|
||
- Watchdog timer (50ms hardware WDT)
|
||
- Speed limit at 10% (max_speed_limit = 100)
|
||
- Arming sequence (3s hold while upright)
|
||
- [ ] Write hoverboard ESC UART output (speed+steer protocol)
|
||
- [ ] Flash firmware via USB DFU (boot0 jumper on FC)
|
||
- [ ] Write ELRS CRSF receiver driver (UART3, parse channels + arm switch)
|
||
- [ ] Bind ELRS TX ↔ RX, verify channel data on serial monitor
|
||
- [ ] Map radio: CH1=steer, CH2=speed, CH5=arm/disarm switch
|
||
- [ ] **Bench test first** — FC powered but ESC disconnected, verify IMU reads + PID output + RC channels on serial monitor
|
||
- [ ] Wire FC UART2 → hoverboard ESC UART
|
||
- [ ] Build minimal frame: motor plate + battery + ESC + FC
|
||
- [ ] Power FC from ESC 5V BEC
|
||
- [ ] **First balance test — TETHERED, kill switch in hand, 10% speed limit**
|
||
- [ ] Tune PID at 10% speed until stable tethered for 5+ minutes
|
||
- [ ] Gradually increase speed limit (10% increments, 5 min stable each)
|
||
|
||
### Phase 2: Brain (Week 2)
|
||
- [ ] Mount Jetson + power (DC-DC 5V)
|
||
- [ ] Set up JetPack + ROS2
|
||
- [ ] Add Jetson UART RX to FC firmware (receive speed+steer commands)
|
||
- [ ] Wire Jetson UART1 → FC UART1
|
||
- [ ] Python serial bridge: send speed+steer, read telemetry
|
||
- [ ] Test: keyboard teleoperation while balancing
|
||
|
||
### Phase 3: Senses (Week 3)
|
||
- [ ] Mount RealSense + RPLIDAR
|
||
- [ ] SLAM mapping of a room
|
||
- [ ] Person detection + tracking (SSD-MobileNet-v2 via TensorRT)
|
||
- [ ] Follow mode: maintain 1.5m distance from person
|
||
|
||
### Phase 4: Polish (Week 4)
|
||
- [ ] Print proper enclosures, bumpers, diffuser ring
|
||
- [ ] Wire ESP32-C3 to FC telemetry TX line (listen-only tap)
|
||
- [ ] Flash ESP32-C3: parse telemetry, drive WS2812B via RMT
|
||
- [ ] Mount LED strip around frame with diffuser
|
||
- [ ] Test all LED patterns: disarmed/arming/armed/turning/fault
|
||
- [ ] Speaker for audio feedback
|
||
- [ ] WiFi status dashboard (ESP32-C3 can serve this too)
|
||
- [ ] Emergency stop button
|