# Next Session: SLAM Drive Test (Take 2)

## Status
Session 62 fixed 3 critical lidar bugs. Lidar now delivers 400+ pts/rotation with correct sector mapping. SLAM reset and ready. All systems green.

## What Was Done (Session 62)
- **Bug 1 — Cumulative angles >360°:** pyrplidar returns 363°, 503°, etc. Fixed with `angle % 360.0`
- **Bug 2 — False rotation detection:** `angle < last_angle` triggered on tiny jitter. Fixed: require `last_angle - angle > 180` AND `len(local_points) >= 350`
- **Bug 3 — CCW sector mapping:** C1 reports CCW (90°=left). Fixed: `360 - angle` in `_angle_to_sector`. Verified with physical box to the right → Right-Forward sector at 416mm
- **Chassis filter:** Points < 150mm filtered from sector aggregation (car body visible at ~130mm)
- **Thresholds raised:** blocked <250mm, near <400mm
- **Side clearance guard:** `/drive` endpoint now checks adjacent lidar sectors before allowing movement
- **IMU fixed:** Pico was at REPL prompt; Ctrl-D soft-reboot restored it
- **Diagnostic scripts:** `scripts/diag_lidar.py`, `scripts/diag_imu.py`

## What Needs Doing

### 1. SLAM Drive Test (PRIMARY GOAL)
SLAM is reset, all systems green. Give Annie a drive command from Telegram:

```
"Annie, drive forward slowly"
```

Monitor from laptop:
```bash
TOKEN="8cX80yIBws1PfBjFuvPz0k9egPSZD0LvS02oUD6ijfg"

# Monitor pose while driving
ssh pi "curl -s -H 'Authorization: Bearer $TOKEN' http://localhost:8080/pose"

# Download map
ssh pi "curl -s -H 'Authorization: Bearer $TOKEN' http://localhost:8080/map" > /tmp/map.png

# Health check
ssh pi "curl -s -H 'Authorization: Bearer $TOKEN' http://localhost:8080/health"

# Scan quality
ssh pi "curl -s -H 'Authorization: Bearer $TOKEN' http://localhost:8080/scan" | python3 -c "import json,sys; d=json.load(sys.stdin); print(sum(s['num_points'] for s in d['sectors']), 'pts')"
```

Check:
- scan_count growing continuously
- x_m/y_m tracking movement (not stuck at 0)
- heading_deg reflecting turns
- Map PNG showing room walls
- All 12 sectors with 30+ pts each

### 2. Annie Navigation E2E (needs Titan)
Start Annie + Telegram bot + vLLM on Titan:
```bash
ssh titan "sudo systemctl start annie-voice telegram-bot"
```
Then message Annie: "explore this room"

### 3. Known Issues to Watch
- **IMU may drop to REPL** — if `imu_healthy=false`, run `scripts/diag_imu.py --reboot` (stop server first)
- **Lidar USB stability** — if points drop, restart service (buffer flush fix should auto-recover)
- **IMU drift** — ~4°/min on MPU-6050; scan matcher should correct
- **Side clearance guard** — new in this session, may reject valid drive commands if sectors show near/blocked. Check logs if Annie can't move.
- **LIDAR_FORWARD_OFFSET_DEG = 0.0** — recalibrate if sectors seem misaligned. Place box directly in front, check Forward sector shows it.

## Pre-flight Checklist
Before driving:
1. `health` → all healthy (lidar, imu, slam, sonar)
2. `slam_state` = "running" (not "degraded" or "init")
3. `scan` → all 12 sectors have 30+ points
4. `pose` → scan_count growing

If IMU is unhealthy:
```bash
ssh pi "sudo systemctl stop turbopi-server"
ssh pi "python3 ~/workplace/her/her-os/scripts/diag_imu.py --reboot"
ssh pi "sudo systemctl start turbopi-server"
```

## Key Files
- `services/turbopi-server/lidar.py` — scan loop, sector mapping, chassis filter (477 lines)
- `services/turbopi-server/slam.py` — HectorSLAM (624 lines)
- `services/turbopi-server/imu.py` — IMU reader (141 lines)
- `services/turbopi-server/main.py` — endpoints + side clearance guard
- `scripts/diag_lidar.py` — lidar diagnostic (raw serial, scan quality, USB reset)
- `scripts/diag_imu.py` — IMU diagnostic (REPL check, soft reboot)

## Git State
Main at `40c0c32`. Clean working tree. Pi deployed and running.