# 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
