02217443ea
Consolidating seb/saltylab into saltylab-firmware before deleting the seed repo. - 16 OpenSCAD CAD models → cad/ - Design docs (SALTYLAB.md, PLATFORM.md, AGENTS.md, board-viz.html) → docs/
421 lines
18 KiB
Markdown
421 lines
18 KiB
Markdown
# 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
|