# SaltyLab — Self-Balancing Indoor Bot 🔬

Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.

## ⚠️ 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 (STM32F745 + MPU-6000) | ✅ Have — balance brain |
| 1x Jetson Nano + 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 |

### Drone FC Details — GEPRC GEP-F7 AIO
- **MCU:** STM32F722RET6 (216MHz Cortex-M7, 512KB flash, 256KB RAM)
- **IMU:** TDK ICM-42688-P (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one!
- **Flash:** 8MB Winbond W25Q64 (blackbox, unused)
- **OSD:** AT7456E (unused)
- **4-in-1 ESC:** Built into AIO board (unused — we use hoverboard ESC)
- **DFU mode:** Hold yellow BOOT button while plugging USB
- **Firmware:** Custom balance firmware (PlatformIO + STM32 HAL)
- **UART pads (confirmed from silkscreen):**
  - T1/R1 (bottom) → USART1 (PA9/PA10) → Jetson
  - T2/R2 (right top) → USART2 (PA2/PA3) → Hoverboard ESC
  - T3/R3 (bottom) → USART3 (PB10/PB11) → ELRS receiver
  - T4/R4 (bottom) → UART4 → spare
  - T5/R5 (right bottom) → UART5 → spare

## Architecture

```
                    ┌──────────────┐
                    │  RPLIDAR A1  │ ← 360° scan, top-mounted
                    └──────┬───────┘
                    ┌──────┴───────┐
                    │ RealSense    │ ← Forward-facing depth+RGB
                    │ D435i        │
                    ├──────────────┤
                    │ Jetson Nano  │ ← 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 — Custom Firmware on Drone FC

### Why a Drone FC?
The F745 board is just a premium STM32 dev board with a high-quality IMU (MPU-6000) already soldered on, proper voltage regulation, and multiple UARTs broken out. We write a lean custom balance firmware (~50 lines of C).

### Architecture
```
Jetson (speed+steer via UART1)
    │
    ▼
Drone FC (F745 + MPU-6000)
    │  - Reads IMU @ 8kHz (SPI)
    │  - Runs PID balance loop
    │  - Mixes balance correction + Jetson commands
    │  - Outputs speed+steer via UART2
    ▼
Hoverboard ESC (FOC firmware)
    │  - Receives UART commands
    │  - Drives hub motors
    ▼
Left + Right wheels
```

- **No motor outputs used** — FC talks UART directly to hoverboard ESC
- **Custom firmware only** — no third-party flight software
- **Dead motor output irrelevant** — not using any PWM channels

### Wiring

```
Jetson UART1        Drone FC (UART1)
────────────        ────────────────
TX (Pin 8)   ──→   RX
RX (Pin 10)  ──→   TX
GND          ──→   GND

Drone FC (UART2)    Hoverboard ESC
────────────────    ──────────────
TX           ──→   RX (serial input)
GND          ──→   GND
5V (BEC)     ←──   ESC 5V out (powers FC)

ELRS Receiver       Drone FC (UART3)
─────────────       ────────────────
TX           ──→   RX
RX           ←──   TX (for telemetry/binding)
GND          ──→   GND
5V           ←──   5V
```

### Custom Firmware (STM32 C)

```c
// Core balance loop — runs in timer interrupt @ 1-8kHz
void balance_loop(void) {
    // 1. Read pitch angle from MPU-6000 (complementary filter)
    float pitch = get_pitch_angle();  // SPI read + filter
    
    // 2. Get velocity command from Jetson (updated async via UART1 RX)
    float target_speed = jetson_cmd.speed;   // -1000 to 1000
    float target_steer = jetson_cmd.steer;   // -1000 to 1000
    
    // 3. PID on pitch error
    //    Target angle shifts with speed command (lean forward = go forward)
    float target_angle = target_speed * SPEED_TO_ANGLE_FACTOR;
    float error = target_angle - pitch;
    
    integral += error * dt;
    integral = clamp(integral, -MAX_I, MAX_I);  // anti-windup
    float derivative = (error - prev_error) / dt;
    prev_error = error;
    
    float output = Kp * error + Ki * integral + Kd * derivative;
    
    // 4. Mix balance + steering → hoverboard ESC UART command
    int16_t left  = clamp(output + target_steer, -1000, 1000);
    int16_t right = clamp(output - target_steer, -1000, 1000);
    
    // 5. Send to hoverboard ESC via UART2
    send_hoverboard_cmd(left, right);
    
    // 6. Safety: kill motors if tipped beyond recovery
    if (fabs(pitch) > MAX_TILT_DEG) {
        send_hoverboard_cmd(0, 0);
        disarm();
    }
    
    // 7. Safety: RC kill switch (ELRS channel, checked every loop)
    if (rc_channels.arm_switch == DISARMED) {
        send_hoverboard_cmd(0, 0);
        disarm();
    }
    
    // 8. Safety: kill if Jetson UART heartbeat lost
    if (millis() - jetson_last_rx > JETSON_TIMEOUT_MS) {
        send_hoverboard_cmd(0, 0);
        disarm();
    }
    
    // 8. Safety: clamp output to max allowed speed
    left  = clamp(left,  -max_speed_limit, max_speed_limit);
    right = clamp(right, -max_speed_limit, max_speed_limit);
}
```

### Hoverboard ESC UART Protocol
```c
typedef struct {
    uint16_t start;    // 0xABCD
    int16_t  speed;    // -1000 to 1000 (left)
    int16_t  steer;    // -1000 to 1000 (right)
    uint16_t checksum; // XOR of all bytes
} HoverboardCmd;
// 115200 baud, send at loop rate
```

### Jetson → FC Protocol (simple custom)
```c
typedef struct {
    uint8_t  header;   // 0xAA
    int16_t  speed;    // -1000 to 1000
    int16_t  steer;    // -1000 to 1000
    uint8_t  mode;     // 0=idle, 1=balance, 2=follow, 3=RC
    uint8_t  checksum;
} JetsonCmd;
// 115200 baud, ~50Hz from Jetson is plenty
```

### PID Tuning
| Param | Starting Value | Notes |
|-------|---------------|-------|
| Kp | 30-50 | Main balance response |
| Ki | 0.5-2 | Drift correction |
| Kd | 0.5-2 | Damping oscillation |
| Loop rate | 1-8 kHz | Start at 1kHz, increase if needed |
| Max tilt | ±25° | Beyond this = cut motors, require re-arm |
| JETSON_TIMEOUT_MS | 200 | Kill motors if Jetson stops talking |
| max_speed_limit | 100 | Start at 10% (100/1000), increase gradually |
| SPEED_TO_ANGLE_FACTOR | 0.01-0.05 | How much lean per speed unit |

## 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:** STM32CubeProgrammer via USB (DFU mode) or SWD
- **IDE:** PlatformIO + STM32 HAL, or STM32CubeIDE
- **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 Nano
- **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 STM32F745 (STM32 HAL)
- [ ] Write MPU-6000 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