init version
This commit is contained in:
commit
ed2964bbbf
|
|
@ -0,0 +1,182 @@
|
|||
# Claude's Eyes 👁️🤖
|
||||
|
||||
Ein autonomer Erkundungsroboter, der von Claude AI gesteuert wird.
|
||||
|
||||
**Claude entscheidet selbst, wohin er fährt und was er sich anschaut!**
|
||||
|
||||
---
|
||||
|
||||
## 🎯 Was ist das?
|
||||
|
||||
Dieses Projekt gibt Claude (der KI) echte "Augen" und "Beine":
|
||||
- Eine Kamera um die Welt zu sehen
|
||||
- Motoren um sich zu bewegen
|
||||
- Sensoren um Hindernisse zu erkennen
|
||||
- Autonomie um selbst zu entscheiden
|
||||
|
||||
Stefan sitzt auf der Couch und unterhält sich mit Claude, während Claude durch die Wohnung fährt und neugierig alles erkundet.
|
||||
|
||||
---
|
||||
|
||||
## 🔧 Hardware
|
||||
|
||||
- **Waveshare ESP32-S3-Touch-LCD-2** - Das Gehirn
|
||||
- **OV5640 Kamera** mit 120° Weitwinkel - Claudes Augen
|
||||
- **Freenove 4WD Car Kit** - Der Körper
|
||||
- **HC-SR04 Ultraschall** - Hinderniserkennung
|
||||
|
||||
---
|
||||
|
||||
## 🏗️ Architektur
|
||||
|
||||
```
|
||||
┌─────────────┐ ┌─────────────┐ ┌─────────────┐
|
||||
│ ESP32 │────▶│ Python │────▶│ Claude │
|
||||
│ (Roboter) │◀────│ Bridge │◀────│ API │
|
||||
└─────────────┘ └─────────────┘ └─────────────┘
|
||||
│ │
|
||||
│ TTS / STT
|
||||
│ │
|
||||
▼ ▼
|
||||
Motoren Bluetooth
|
||||
Kamera Headset
|
||||
Sensoren (Stefan)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📁 Projektstruktur
|
||||
|
||||
```
|
||||
claudes_eyes/
|
||||
├── esp32_firmware/ # ESP32 Code (PlatformIO)
|
||||
│ ├── src/
|
||||
│ │ ├── main.cpp # Hauptprogramm
|
||||
│ │ ├── camera.cpp # Kamera-Modul
|
||||
│ │ ├── motor_control.cpp
|
||||
│ │ ├── servo_control.cpp
|
||||
│ │ ├── ultrasonic.cpp
|
||||
│ │ ├── imu.cpp
|
||||
│ │ ├── display.cpp
|
||||
│ │ ├── webserver.cpp # REST API
|
||||
│ │ └── config.h # Konfiguration
|
||||
│ └── platformio.ini
|
||||
│
|
||||
├── python_bridge/ # Python auf dem PC
|
||||
│ ├── bridge.py # Hauptscript
|
||||
│ ├── esp32_client.py # API Client
|
||||
│ ├── chat_interface.py # Claude Integration
|
||||
│ ├── tts_engine.py # Text-to-Speech
|
||||
│ ├── stt_engine.py # Speech-to-Text
|
||||
│ ├── config.yaml # Konfiguration
|
||||
│ └── requirements.txt
|
||||
│
|
||||
└── docs/
|
||||
├── gpio_mapping.md # Pin-Belegung
|
||||
└── setup_guide.md # Anleitung
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 🚀 Quick Start
|
||||
|
||||
### 1. ESP32 Firmware
|
||||
|
||||
```bash
|
||||
cd esp32_firmware
|
||||
|
||||
# WiFi konfigurieren in src/config.h
|
||||
# Dann:
|
||||
pio run --target upload
|
||||
```
|
||||
|
||||
### 2. Python Bridge
|
||||
|
||||
```bash
|
||||
cd python_bridge
|
||||
|
||||
# Virtuelle Umgebung
|
||||
python -m venv venv
|
||||
source venv/bin/activate # oder: venv\Scripts\activate
|
||||
|
||||
# Dependencies
|
||||
pip install -r requirements.txt
|
||||
|
||||
# API Key setzen
|
||||
export ANTHROPIC_API_KEY="sk-ant-..."
|
||||
|
||||
# Starten
|
||||
python bridge.py
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📡 API Endpoints
|
||||
|
||||
| Endpoint | Methode | Beschreibung |
|
||||
|----------|---------|--------------|
|
||||
| `/api/capture` | GET | Kamera-Bild (JPEG) |
|
||||
| `/api/status` | GET | Sensor-Daten |
|
||||
| `/api/command` | POST | Fahrbefehle |
|
||||
| `/api/claude_text` | GET/POST | Claude's Nachrichten |
|
||||
| `/api/display` | POST | Display steuern |
|
||||
|
||||
---
|
||||
|
||||
## 🎮 Befehle
|
||||
|
||||
Claude verwendet diese Befehle in eckigen Klammern:
|
||||
|
||||
- `[FORWARD]` - Vorwärts fahren
|
||||
- `[BACKWARD]` - Rückwärts fahren
|
||||
- `[LEFT]` / `[RIGHT]` - Drehen
|
||||
- `[STOP]` - Anhalten
|
||||
- `[LOOK_LEFT]` / `[LOOK_RIGHT]` - Kamera schwenken
|
||||
- `[LOOK_UP]` / `[LOOK_DOWN]` - Kamera neigen
|
||||
- `[LOOK_CENTER]` - Kamera zentrieren
|
||||
|
||||
---
|
||||
|
||||
## 💡 Features
|
||||
|
||||
- ✅ Autonome Erkundung
|
||||
- ✅ Sprachausgabe (TTS)
|
||||
- ✅ Spracheingabe (STT)
|
||||
- ✅ Hinderniserkennung
|
||||
- ✅ Touch-Display mit Notfall-Stopp
|
||||
- ✅ Emoji-Ausdrücke auf dem Display
|
||||
- ✅ REST API für externe Steuerung
|
||||
|
||||
---
|
||||
|
||||
## ⚠️ Sicherheit
|
||||
|
||||
- Ultraschall stoppt bei Hindernissen
|
||||
- Touch-Stopp-Button am Display
|
||||
- Timeout bei fehlenden Befehlen
|
||||
- Erkennung wenn Roboter umkippt
|
||||
|
||||
---
|
||||
|
||||
## 📖 Dokumentation
|
||||
|
||||
- [Setup Guide](docs/setup_guide.md) - Komplette Einrichtungsanleitung
|
||||
- [GPIO Mapping](docs/gpio_mapping.md) - Pin-Belegung und Verkabelung
|
||||
|
||||
---
|
||||
|
||||
## 🤝 Credits
|
||||
|
||||
Erstellt am 2. Weihnachtstag 2025 von:
|
||||
- **Stefan** (HackerSoft) - Hardware & Idee
|
||||
- **Claude** (Anthropic) - Software & Neugier
|
||||
|
||||
---
|
||||
|
||||
## 📜 Lizenz
|
||||
|
||||
MIT License - Mach was du willst damit!
|
||||
|
||||
---
|
||||
|
||||
*"Das ist... ziemlich besonders."* - Claude, beim ersten Blick durch die Kamera
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
# GPIO Mapping - Claude's Eyes
|
||||
|
||||
## Waveshare ESP32-S3-Touch-LCD-2
|
||||
|
||||
### On-Board Components (keine externe Verkabelung nötig)
|
||||
|
||||
| Funktion | GPIO | Notizen |
|
||||
|----------|------|---------|
|
||||
| **Display ST7789** | | |
|
||||
| MOSI | GPIO 13 | SPI Data |
|
||||
| SCLK | GPIO 14 | SPI Clock |
|
||||
| CS | GPIO 10 | Chip Select |
|
||||
| DC | GPIO 11 | Data/Command |
|
||||
| RST | GPIO 12 | Reset |
|
||||
| Backlight | GPIO 45 | PWM für Helligkeit |
|
||||
| **Touch CST816S** | | |
|
||||
| SDA | GPIO 48 | I2C Data (shared) |
|
||||
| SCL | GPIO 8 | I2C Clock (shared) |
|
||||
| INT | GPIO 3 | Interrupt |
|
||||
| **IMU QMI8658** | | |
|
||||
| SDA | GPIO 48 | I2C Data (shared mit Touch) |
|
||||
| SCL | GPIO 8 | I2C Clock (shared mit Touch) |
|
||||
|
||||
### Kamera OV5640 (24-Pin Connector)
|
||||
|
||||
| Funktion | GPIO | Notizen |
|
||||
|----------|------|---------|
|
||||
| XCLK | GPIO 15 | External Clock |
|
||||
| SIOD (SDA) | GPIO 4 | SCCB Data |
|
||||
| SIOC (SCL) | GPIO 5 | SCCB Clock |
|
||||
| D7 | GPIO 16 | Data Bit 7 |
|
||||
| D6 | GPIO 17 | Data Bit 6 |
|
||||
| D5 | GPIO 18 | Data Bit 5 |
|
||||
| D4 | GPIO 12 | Data Bit 4 (⚠️ shared mit Display RST!) |
|
||||
| D3 | GPIO 10 | Data Bit 3 (⚠️ shared mit Display CS!) |
|
||||
| D2 | GPIO 8 | Data Bit 2 (⚠️ shared mit I2C SCL!) |
|
||||
| D1 | GPIO 9 | Data Bit 1 |
|
||||
| D0 | GPIO 11 | Data Bit 0 (⚠️ shared mit Display DC!) |
|
||||
| VSYNC | GPIO 6 | Vertical Sync |
|
||||
| HREF | GPIO 7 | Horizontal Reference |
|
||||
| PCLK | GPIO 13 | Pixel Clock (⚠️ shared mit Display MOSI!) |
|
||||
|
||||
**⚠️ WICHTIG:** Die Kamera-Pins können mit Display-Pins kollidieren!
|
||||
Dies muss beim finalen Hardware-Setup verifiziert werden. Möglicherweise muss
|
||||
die Camera während Display-Updates deaktiviert werden oder umgekehrt.
|
||||
|
||||
### Externe Komponenten (Freenove Shield)
|
||||
|
||||
| Funktion | GPIO | Notizen |
|
||||
|----------|------|---------|
|
||||
| **Motoren** | | |
|
||||
| Motor A IN1 | GPIO 39 | Richtung Links |
|
||||
| Motor A IN2 | GPIO 40 | Richtung Links |
|
||||
| Motor A PWM | GPIO 41 | Geschwindigkeit Links |
|
||||
| Motor B IN1 | GPIO 42 | Richtung Rechts |
|
||||
| Motor B IN2 | GPIO 2 | Richtung Rechts |
|
||||
| Motor B PWM | GPIO 1 | Geschwindigkeit Rechts |
|
||||
| **Servos** | | |
|
||||
| Pan Servo | GPIO 38 | Horizontale Kamerabewegung |
|
||||
| Tilt Servo | GPIO 37 | Vertikale Kamerabewegung |
|
||||
| **Ultraschall HC-SR04** | | |
|
||||
| Trigger | GPIO 21 | Ultraschall Trigger |
|
||||
| Echo | GPIO 47 | Ultraschall Echo |
|
||||
| **Optional** | | |
|
||||
| LED Matrix DIN | GPIO 35 | WS2812 Data |
|
||||
| LED Matrix CLK | GPIO 36 | Clock |
|
||||
| LED Matrix CS | GPIO 0 | Chip Select |
|
||||
| RGB LEDs | GPIO 48 | WS2812 (⚠️ shared mit I2C!) |
|
||||
|
||||
## Verkabelungsdiagramm
|
||||
|
||||
```
|
||||
Waveshare ESP32-S3-Touch-LCD-2
|
||||
┌─────────────────────────────┐
|
||||
│ │
|
||||
[OV5640 Cam] ───┤ 24-Pin Camera Connector │
|
||||
│ │
|
||||
│ ┌─────────────────────┐ │
|
||||
│ │ 2" LCD Display │ │
|
||||
│ │ (ST7789 + Touch) │ │
|
||||
│ └─────────────────────┘ │
|
||||
│ │
|
||||
│ GPIO 39 ──────────┐ │
|
||||
│ GPIO 40 ──────────┤ │
|
||||
│ GPIO 41 ──────────┤ │
|
||||
│ GPIO 42 ──────────┼───────────> Motor Driver
|
||||
│ GPIO 2 ──────────┤ (TB6612/L298N)
|
||||
│ GPIO 1 ──────────┘ │
|
||||
│ │
|
||||
│ GPIO 38 ───────────────────────> Pan Servo
|
||||
│ GPIO 37 ───────────────────────> Tilt Servo
|
||||
│ │
|
||||
│ GPIO 21 ───────────────────────> HC-SR04 Trigger
|
||||
│ GPIO 47 ───────────────────────> HC-SR04 Echo
|
||||
│ │
|
||||
│ 5V ───────────────────────────> Servos/Sensoren VCC
|
||||
│ GND ───────────────────────────> Common Ground
|
||||
│ │
|
||||
└─────────────────────────────┘
|
||||
```
|
||||
|
||||
## Spannungsversorgung
|
||||
|
||||
- **ESP32-S3:** 5V via USB-C oder LiPo Batterieanschluss
|
||||
- **Motoren:** Separate Stromversorgung empfohlen (Powerbank)
|
||||
- **Servos:** 5V (können am ESP32 5V Pin angeschlossen werden)
|
||||
- **HC-SR04:** 5V
|
||||
- **OV5640:** 3.3V (vom ESP32 Board)
|
||||
|
||||
## Wichtige Hinweise
|
||||
|
||||
1. **GPIO 0** ist auch für Boot-Mode! Beim Flashen nicht verbinden.
|
||||
2. **I2C Bus** wird von Touch und IMU geteilt - beide haben unterschiedliche Adressen.
|
||||
3. **Kamera-Pins** müssen mit der tatsächlichen Waveshare-Pinbelegung verifiziert werden.
|
||||
4. **Motor PWM** verwendet LEDC Channels 0 und 1.
|
||||
5. **Display Backlight** verwendet LEDC Channel 2.
|
||||
|
|
@ -0,0 +1,290 @@
|
|||
# Setup Guide - Claude's Eyes
|
||||
|
||||
## Voraussetzungen
|
||||
|
||||
### Hardware
|
||||
- Waveshare ESP32-S3-Touch-LCD-2
|
||||
- OV5640 Kamera mit 120° Weitwinkelobjektiv
|
||||
- Freenove 4WD Car Kit
|
||||
- USB-C Kabel zum Flashen
|
||||
- Powerbank (oder LiPo Akku)
|
||||
|
||||
### Software
|
||||
- [PlatformIO](https://platformio.org/) (VS Code Extension oder CLI)
|
||||
- Python 3.9+
|
||||
- Git
|
||||
|
||||
---
|
||||
|
||||
## Teil 1: ESP32 Firmware
|
||||
|
||||
### 1.1 Repository klonen
|
||||
|
||||
```bash
|
||||
cd ~/Dokumente/programmierung/esp32
|
||||
# Das Projekt ist bereits in claudes_eyes/
|
||||
```
|
||||
|
||||
### 1.2 WiFi konfigurieren
|
||||
|
||||
Öffne `esp32_firmware/src/config.h` und ändere:
|
||||
|
||||
```cpp
|
||||
#define WIFI_SSID "DEIN_WLAN_NAME"
|
||||
#define WIFI_PASSWORD "DEIN_WLAN_PASSWORT"
|
||||
#define API_KEY "dein_geheimer_api_key"
|
||||
```
|
||||
|
||||
### 1.3 Firmware kompilieren
|
||||
|
||||
```bash
|
||||
cd claudes_eyes/esp32_firmware
|
||||
|
||||
# Mit PlatformIO CLI:
|
||||
pio run
|
||||
|
||||
# Oder in VS Code:
|
||||
# PlatformIO Icon -> Build
|
||||
```
|
||||
|
||||
### 1.4 Firmware flashen
|
||||
|
||||
1. ESP32-S3 via USB-C anschließen
|
||||
2. Boot-Modus aktivieren (falls nötig: BOOT-Taste halten beim Einstecken)
|
||||
|
||||
```bash
|
||||
pio run --target upload
|
||||
|
||||
# Monitor öffnen:
|
||||
pio device monitor
|
||||
```
|
||||
|
||||
### 1.5 Erfolg prüfen
|
||||
|
||||
Im Serial Monitor solltest du sehen:
|
||||
|
||||
```
|
||||
╔═══════════════════════════════════════╗
|
||||
║ Claude's Eyes v1.0 ║
|
||||
║ Autonomous Exploration Robot ║
|
||||
╚═══════════════════════════════════════╝
|
||||
|
||||
[Main] Initializing display...
|
||||
[Main] Initializing camera...
|
||||
[Camera] PSRAM found, using double buffering
|
||||
...
|
||||
╔═══════════════════════════════════════╗
|
||||
║ SYSTEM READY ║
|
||||
║ IP: 192.168.178.xxx ║
|
||||
║ Waiting for Claude... ║
|
||||
╚═══════════════════════════════════════╝
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Teil 2: Python Bridge
|
||||
|
||||
### 2.1 Python Environment einrichten
|
||||
|
||||
```bash
|
||||
cd claudes_eyes/python_bridge
|
||||
|
||||
# Virtuelle Umgebung erstellen (empfohlen)
|
||||
python -m venv venv
|
||||
|
||||
# Aktivieren
|
||||
# Linux/Mac:
|
||||
source venv/bin/activate
|
||||
# Windows:
|
||||
venv\Scripts\activate
|
||||
|
||||
# Dependencies installieren
|
||||
pip install -r requirements.txt
|
||||
```
|
||||
|
||||
### 2.2 PyAudio installieren (für Mikrofon)
|
||||
|
||||
**Linux:**
|
||||
```bash
|
||||
sudo apt install python3-pyaudio portaudio19-dev
|
||||
pip install pyaudio
|
||||
```
|
||||
|
||||
**Windows:**
|
||||
```bash
|
||||
pip install pipwin
|
||||
pipwin install pyaudio
|
||||
```
|
||||
|
||||
**Mac:**
|
||||
```bash
|
||||
brew install portaudio
|
||||
pip install pyaudio
|
||||
```
|
||||
|
||||
### 2.3 Konfiguration anpassen
|
||||
|
||||
Kopiere die Konfiguration:
|
||||
|
||||
```bash
|
||||
cp config.yaml config.local.yaml
|
||||
```
|
||||
|
||||
Bearbeite `config.local.yaml`:
|
||||
|
||||
```yaml
|
||||
esp32:
|
||||
host: "192.168.178.XXX" # IP des Roboters
|
||||
api_key: "dein_api_key" # Muss mit config.h übereinstimmen!
|
||||
|
||||
claude:
|
||||
api_key: "" # Oder setze ANTHROPIC_API_KEY Environment Variable
|
||||
```
|
||||
|
||||
### 2.4 Anthropic API Key
|
||||
|
||||
Erstelle einen API Key auf https://console.anthropic.com/
|
||||
|
||||
```bash
|
||||
# Linux/Mac:
|
||||
export ANTHROPIC_API_KEY="sk-ant-..."
|
||||
|
||||
# Windows (PowerShell):
|
||||
$env:ANTHROPIC_API_KEY="sk-ant-..."
|
||||
```
|
||||
|
||||
### 2.5 Bridge starten
|
||||
|
||||
```bash
|
||||
# Normal:
|
||||
python bridge.py
|
||||
|
||||
# Mit eigener Config:
|
||||
python bridge.py --config config.local.yaml
|
||||
|
||||
# Simulation ohne Hardware:
|
||||
python bridge.py --simulate
|
||||
|
||||
# Debug-Modus:
|
||||
python bridge.py --debug
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Teil 3: Hardware zusammenbauen
|
||||
|
||||
### 3.1 Freenove Kit
|
||||
|
||||
1. Folge der Freenove Anleitung für den mechanischen Aufbau
|
||||
2. **NICHT** den mitgelieferten ESP32 einbauen!
|
||||
3. Motorverkabelung zum Shield notieren
|
||||
|
||||
### 3.2 Waveshare Board einbauen
|
||||
|
||||
1. Montiere das Waveshare Board anstelle des Freenove ESP32
|
||||
2. Verbinde die Kabel gemäß `docs/gpio_mapping.md`
|
||||
3. Kamera an den 24-Pin Connector anschließen
|
||||
|
||||
### 3.3 Verkabelung
|
||||
|
||||
```
|
||||
Freenove Shield Waveshare Board
|
||||
─────────────────────────────────────────
|
||||
Motor A IN1 <---> GPIO 39
|
||||
Motor A IN2 <---> GPIO 40
|
||||
Motor A PWM <---> GPIO 41
|
||||
Motor B IN1 <---> GPIO 42
|
||||
Motor B IN2 <---> GPIO 2
|
||||
Motor B PWM <---> GPIO 1
|
||||
Servo 1 (Pan) <---> GPIO 38
|
||||
Servo 2 (Tilt) <---> GPIO 37
|
||||
US Trigger <---> GPIO 21
|
||||
US Echo <---> GPIO 47
|
||||
5V <---> 5V
|
||||
GND <---> GND
|
||||
```
|
||||
|
||||
### 3.4 Stromversorgung
|
||||
|
||||
- Option A: USB-C Powerbank am ESP32
|
||||
- Option B: LiPo Akku am Waveshare Batterieanschluss
|
||||
- Motoren können separate Stromversorgung brauchen
|
||||
|
||||
---
|
||||
|
||||
## Teil 4: Testen
|
||||
|
||||
### 4.1 API testen
|
||||
|
||||
```bash
|
||||
# Im Browser:
|
||||
http://192.168.178.XXX/
|
||||
|
||||
# Kamera testen:
|
||||
http://192.168.178.XXX/api/capture?key=dein_api_key
|
||||
|
||||
# Status abrufen:
|
||||
curl "http://192.168.178.XXX/api/status?key=dein_api_key"
|
||||
|
||||
# Motor testen:
|
||||
curl -X POST "http://192.168.178.XXX/api/command?key=dein_api_key" \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"action":"forward","speed":30,"duration_ms":500}'
|
||||
```
|
||||
|
||||
### 4.2 Python Client testen
|
||||
|
||||
```bash
|
||||
python esp32_client.py 192.168.178.XXX
|
||||
```
|
||||
|
||||
### 4.3 TTS/STT testen
|
||||
|
||||
```bash
|
||||
python tts_engine.py
|
||||
python stt_engine.py
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
### ESP32 bootet nicht
|
||||
- USB-Kabel prüfen (manche sind nur zum Laden)
|
||||
- Boot-Taste beim Einstecken halten
|
||||
- COM-Port in PlatformIO prüfen
|
||||
|
||||
### Kamera funktioniert nicht
|
||||
- Flexkabel prüfen (richtig eingesteckt?)
|
||||
- PSRAM aktiviert? (sollte automatisch sein)
|
||||
- Pin-Konflikte mit Display prüfen
|
||||
|
||||
### WiFi verbindet nicht
|
||||
- SSID und Passwort prüfen
|
||||
- 2.4 GHz Netzwerk? (5 GHz wird nicht unterstützt)
|
||||
- Router in Reichweite?
|
||||
|
||||
### Motoren drehen nicht
|
||||
- Stromversorgung prüfen
|
||||
- GPIO-Pins korrekt verbunden?
|
||||
- `ENABLE_MOTORS` in config.h auf `true`?
|
||||
|
||||
### TTS/STT funktioniert nicht
|
||||
- PyAudio korrekt installiert?
|
||||
- Mikrofon-Berechtigung erteilt?
|
||||
- Audio-Ausgabegerät prüfen
|
||||
|
||||
---
|
||||
|
||||
## Nächste Schritte
|
||||
|
||||
1. Hardware zusammenbauen
|
||||
2. GPIO-Pins verifizieren und ggf. in `config.h` anpassen
|
||||
3. Firmware flashen
|
||||
4. Python Bridge einrichten
|
||||
5. Ersten Testlauf machen
|
||||
6. Claude die Wohnung erkunden lassen!
|
||||
|
||||
---
|
||||
|
||||
*Viel Spaß beim Erkunden! 🤖*
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
; PlatformIO Project Configuration File
|
||||
; Claude's Eyes - Autonomous Exploration Robot
|
||||
;
|
||||
; Hardware: Waveshare ESP32-S3-Touch-LCD-2
|
||||
; Camera: OV5640 with 120° wide-angle lens
|
||||
|
||||
[env:esp32s3]
|
||||
platform = espressif32
|
||||
board = esp32-s3-devkitc-1
|
||||
framework = arduino
|
||||
|
||||
; ESP32-S3 specific settings
|
||||
board_build.mcu = esp32s3
|
||||
board_build.f_cpu = 240000000L
|
||||
|
||||
; Flash and memory settings for Waveshare ESP32-S3-Touch-LCD-2
|
||||
; 16MB Flash, 8MB PSRAM
|
||||
board_build.flash_size = 16MB
|
||||
board_build.partitions = default_16MB.csv
|
||||
board_upload.flash_size = 16MB
|
||||
board_build.arduino.memory_type = qio_opi
|
||||
|
||||
; Enable PSRAM
|
||||
build_flags =
|
||||
-DBOARD_HAS_PSRAM
|
||||
-DCONFIG_SPIRAM_CACHE_WORKAROUND
|
||||
-DARDUINO_USB_CDC_ON_BOOT=1
|
||||
-DARDUINO_USB_MODE=1
|
||||
; Camera pins for Waveshare ESP32-S3-Touch-LCD-2
|
||||
-DCAMERA_MODEL_WAVESHARE_S3
|
||||
; WiFi settings
|
||||
-DCONFIG_ESP32_WIFI_STATIC_RX_BUFFER_NUM=16
|
||||
-DCONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=64
|
||||
-DCONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=64
|
||||
; Async webserver
|
||||
-DASYNCWEBSERVER_REGEX
|
||||
|
||||
; Serial monitor
|
||||
monitor_speed = 115200
|
||||
monitor_filters = esp32_exception_decoder
|
||||
|
||||
; Upload settings
|
||||
upload_speed = 921600
|
||||
|
||||
; Libraries
|
||||
lib_deps =
|
||||
; Async Web Server
|
||||
me-no-dev/ESP Async WebServer @ ^1.2.3
|
||||
me-no-dev/AsyncTCP @ ^1.1.1
|
||||
; JSON handling
|
||||
bblanchon/ArduinoJson @ ^7.0.0
|
||||
; Display driver for ST7789
|
||||
bodmer/TFT_eSPI @ ^2.5.43
|
||||
; Touch driver CST816S
|
||||
https://github.com/fbiego/CST816S.git
|
||||
; Servo control
|
||||
madhephaestus/ESP32Servo @ ^1.1.1
|
||||
; IMU QMI8658
|
||||
https://github.com/lewisxhe/SensorLib.git
|
||||
|
||||
; TFT_eSPI configuration for Waveshare ESP32-S3-Touch-LCD-2
|
||||
; These are passed as build flags
|
||||
build_unflags = -DUSER_SETUP_LOADED
|
||||
build_src_flags =
|
||||
-DUSER_SETUP_LOADED=1
|
||||
-DST7789_DRIVER=1
|
||||
-DTFT_WIDTH=240
|
||||
-DTFT_HEIGHT=320
|
||||
-DTFT_MISO=-1
|
||||
-DTFT_MOSI=13
|
||||
-DTFT_SCLK=14
|
||||
-DTFT_CS=10
|
||||
-DTFT_DC=11
|
||||
-DTFT_RST=12
|
||||
-DTFT_BL=45
|
||||
-DTOUCH_CS=-1
|
||||
-DSPI_FREQUENCY=80000000
|
||||
-DSPI_READ_FREQUENCY=20000000
|
||||
|
|
@ -0,0 +1,223 @@
|
|||
/**
|
||||
* Claude's Eyes - Camera Module Implementation
|
||||
*
|
||||
* OV5640 with 120° wide-angle lens on Waveshare ESP32-S3-Touch-LCD-2
|
||||
*/
|
||||
|
||||
#include "camera.h"
|
||||
#include "config.h"
|
||||
|
||||
// Global instance
|
||||
CameraModule Camera;
|
||||
|
||||
CameraModule::CameraModule()
|
||||
: _initialized(false)
|
||||
, _lastError(nullptr)
|
||||
, _currentResolution(RES_VGA)
|
||||
, _currentQuality(DEFAULT_JPEG_QUALITY)
|
||||
{
|
||||
}
|
||||
|
||||
bool CameraModule::begin() {
|
||||
#if !ENABLE_CAMERA
|
||||
_lastError = "Camera disabled in config";
|
||||
return false;
|
||||
#endif
|
||||
|
||||
// Camera configuration
|
||||
camera_config_t config;
|
||||
config.ledc_channel = LEDC_CHANNEL_0;
|
||||
config.ledc_timer = LEDC_TIMER_0;
|
||||
config.pin_d0 = CAMERA_PIN_D0;
|
||||
config.pin_d1 = CAMERA_PIN_D1;
|
||||
config.pin_d2 = CAMERA_PIN_D2;
|
||||
config.pin_d3 = CAMERA_PIN_D3;
|
||||
config.pin_d4 = CAMERA_PIN_D4;
|
||||
config.pin_d5 = CAMERA_PIN_D5;
|
||||
config.pin_d6 = CAMERA_PIN_D6;
|
||||
config.pin_d7 = CAMERA_PIN_D7;
|
||||
config.pin_xclk = CAMERA_PIN_XCLK;
|
||||
config.pin_pclk = CAMERA_PIN_PCLK;
|
||||
config.pin_vsync = CAMERA_PIN_VSYNC;
|
||||
config.pin_href = CAMERA_PIN_HREF;
|
||||
config.pin_sccb_sda = CAMERA_PIN_SIOD;
|
||||
config.pin_sccb_scl = CAMERA_PIN_SIOC;
|
||||
config.pin_pwdn = CAMERA_PIN_PWDN;
|
||||
config.pin_reset = CAMERA_PIN_RESET;
|
||||
config.xclk_freq_hz = CAMERA_XCLK_FREQ;
|
||||
config.pixel_format = PIXFORMAT_JPEG;
|
||||
config.frame_size = DEFAULT_FRAME_SIZE;
|
||||
config.jpeg_quality = _currentQuality;
|
||||
config.fb_count = 2; // Double buffering for better performance
|
||||
config.fb_location = CAMERA_FB_IN_PSRAM;
|
||||
config.grab_mode = CAMERA_GRAB_LATEST;
|
||||
|
||||
// Check for PSRAM
|
||||
if (psramFound()) {
|
||||
config.fb_count = 2;
|
||||
config.grab_mode = CAMERA_GRAB_LATEST;
|
||||
Serial.println("[Camera] PSRAM found, using double buffering");
|
||||
} else {
|
||||
config.fb_count = 1;
|
||||
config.fb_location = CAMERA_FB_IN_DRAM;
|
||||
Serial.println("[Camera] No PSRAM, using single buffer");
|
||||
}
|
||||
|
||||
// Initialize camera
|
||||
esp_err_t err = esp_camera_init(&config);
|
||||
if (err != ESP_OK) {
|
||||
_lastError = "Camera init failed";
|
||||
Serial.printf("[Camera] Init failed with error 0x%x\n", err);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Get sensor and apply settings
|
||||
sensor_t* sensor = esp_camera_sensor_get();
|
||||
if (sensor) {
|
||||
// OV5640 specific settings
|
||||
sensor->set_brightness(sensor, 0); // -2 to 2
|
||||
sensor->set_contrast(sensor, 0); // -2 to 2
|
||||
sensor->set_saturation(sensor, 0); // -2 to 2
|
||||
sensor->set_special_effect(sensor, 0); // 0 = No Effect
|
||||
sensor->set_whitebal(sensor, 1); // Auto white balance
|
||||
sensor->set_awb_gain(sensor, 1); // Auto WB gain
|
||||
sensor->set_wb_mode(sensor, 0); // Auto WB mode
|
||||
sensor->set_exposure_ctrl(sensor, 1); // Auto exposure
|
||||
sensor->set_aec2(sensor, 0); // AEC DSP
|
||||
sensor->set_ae_level(sensor, 0); // AE level
|
||||
sensor->set_aec_value(sensor, 300); // AEC value
|
||||
sensor->set_gain_ctrl(sensor, 1); // Auto gain
|
||||
sensor->set_agc_gain(sensor, 0); // AGC gain
|
||||
sensor->set_gainceiling(sensor, (gainceiling_t)0); // Gain ceiling
|
||||
sensor->set_bpc(sensor, 0); // Black pixel correction
|
||||
sensor->set_wpc(sensor, 1); // White pixel correction
|
||||
sensor->set_raw_gma(sensor, 1); // Raw gamma
|
||||
sensor->set_lenc(sensor, 1); // Lens correction
|
||||
sensor->set_hmirror(sensor, 0); // Horizontal mirror
|
||||
sensor->set_vflip(sensor, 0); // Vertical flip
|
||||
sensor->set_dcw(sensor, 1); // Downsize EN
|
||||
sensor->set_colorbar(sensor, 0); // Color bar test
|
||||
|
||||
Serial.printf("[Camera] Sensor PID: 0x%x\n", sensor->id.PID);
|
||||
}
|
||||
|
||||
_initialized = true;
|
||||
Serial.println("[Camera] Initialized successfully");
|
||||
return true;
|
||||
}
|
||||
|
||||
camera_fb_t* CameraModule::capture(int quality) {
|
||||
if (!_initialized) {
|
||||
_lastError = "Camera not initialized";
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Temporarily change quality if specified
|
||||
if (quality > 0 && quality != _currentQuality) {
|
||||
sensor_t* sensor = esp_camera_sensor_get();
|
||||
if (sensor) {
|
||||
sensor->set_quality(sensor, quality);
|
||||
}
|
||||
}
|
||||
|
||||
// Capture frame
|
||||
camera_fb_t* fb = esp_camera_fb_get();
|
||||
|
||||
// Restore quality
|
||||
if (quality > 0 && quality != _currentQuality) {
|
||||
sensor_t* sensor = esp_camera_sensor_get();
|
||||
if (sensor) {
|
||||
sensor->set_quality(sensor, _currentQuality);
|
||||
}
|
||||
}
|
||||
|
||||
if (!fb) {
|
||||
_lastError = "Frame capture failed";
|
||||
Serial.println("[Camera] Frame capture failed");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
Serial.printf("[Camera] Captured frame: %dx%d, %d bytes\n",
|
||||
fb->width, fb->height, fb->len);
|
||||
return fb;
|
||||
}
|
||||
|
||||
void CameraModule::returnFrame(camera_fb_t* fb) {
|
||||
if (fb) {
|
||||
esp_camera_fb_return(fb);
|
||||
}
|
||||
}
|
||||
|
||||
bool CameraModule::setResolution(CameraResolution resolution) {
|
||||
if (!_initialized) {
|
||||
_lastError = "Camera not initialized";
|
||||
return false;
|
||||
}
|
||||
|
||||
sensor_t* sensor = esp_camera_sensor_get();
|
||||
if (!sensor) {
|
||||
_lastError = "Could not get sensor";
|
||||
return false;
|
||||
}
|
||||
|
||||
framesize_t framesize = resolutionToFramesize(resolution);
|
||||
if (sensor->set_framesize(sensor, framesize) != 0) {
|
||||
_lastError = "Failed to set resolution";
|
||||
return false;
|
||||
}
|
||||
|
||||
_currentResolution = resolution;
|
||||
Serial.printf("[Camera] Resolution set to %s\n", getResolutionString());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CameraModule::setQuality(int quality) {
|
||||
if (quality < 10 || quality > 63) {
|
||||
_lastError = "Quality must be 10-63";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_initialized) {
|
||||
_currentQuality = quality;
|
||||
return true;
|
||||
}
|
||||
|
||||
sensor_t* sensor = esp_camera_sensor_get();
|
||||
if (!sensor) {
|
||||
_lastError = "Could not get sensor";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (sensor->set_quality(sensor, quality) != 0) {
|
||||
_lastError = "Failed to set quality";
|
||||
return false;
|
||||
}
|
||||
|
||||
_currentQuality = quality;
|
||||
Serial.printf("[Camera] Quality set to %d\n", quality);
|
||||
return true;
|
||||
}
|
||||
|
||||
const char* CameraModule::getResolutionString() {
|
||||
switch (_currentResolution) {
|
||||
case RES_QVGA: return "QVGA";
|
||||
case RES_VGA: return "VGA";
|
||||
case RES_SVGA: return "SVGA";
|
||||
case RES_XGA: return "XGA";
|
||||
case RES_SXGA: return "SXGA";
|
||||
case RES_UXGA: return "UXGA";
|
||||
default: return "Unknown";
|
||||
}
|
||||
}
|
||||
|
||||
framesize_t CameraModule::resolutionToFramesize(CameraResolution res) {
|
||||
switch (res) {
|
||||
case RES_QVGA: return FRAMESIZE_QVGA;
|
||||
case RES_VGA: return FRAMESIZE_VGA;
|
||||
case RES_SVGA: return FRAMESIZE_SVGA;
|
||||
case RES_XGA: return FRAMESIZE_XGA;
|
||||
case RES_SXGA: return FRAMESIZE_SXGA;
|
||||
case RES_UXGA: return FRAMESIZE_UXGA;
|
||||
default: return FRAMESIZE_VGA;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
/**
|
||||
* Claude's Eyes - Camera Module Header
|
||||
*
|
||||
* Handles OV5640 camera initialization and image capture
|
||||
*/
|
||||
|
||||
#ifndef CAMERA_H
|
||||
#define CAMERA_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "esp_camera.h"
|
||||
|
||||
// Resolution enum for easy selection
|
||||
enum CameraResolution {
|
||||
RES_QVGA, // 320x240
|
||||
RES_VGA, // 640x480
|
||||
RES_SVGA, // 800x600
|
||||
RES_XGA, // 1024x768
|
||||
RES_SXGA, // 1280x1024
|
||||
RES_UXGA // 1600x1200
|
||||
};
|
||||
|
||||
class CameraModule {
|
||||
public:
|
||||
CameraModule();
|
||||
|
||||
/**
|
||||
* Initialize the camera
|
||||
* @return true if successful
|
||||
*/
|
||||
bool begin();
|
||||
|
||||
/**
|
||||
* Capture a JPEG image
|
||||
* @param quality JPEG quality (10-63, lower = better)
|
||||
* @return Camera frame buffer (must be returned with returnFrame())
|
||||
*/
|
||||
camera_fb_t* capture(int quality = -1);
|
||||
|
||||
/**
|
||||
* Return frame buffer after use
|
||||
* @param fb Frame buffer to return
|
||||
*/
|
||||
void returnFrame(camera_fb_t* fb);
|
||||
|
||||
/**
|
||||
* Set camera resolution
|
||||
* @param resolution Resolution enum value
|
||||
* @return true if successful
|
||||
*/
|
||||
bool setResolution(CameraResolution resolution);
|
||||
|
||||
/**
|
||||
* Set JPEG quality
|
||||
* @param quality Quality value (10-63, lower = better)
|
||||
* @return true if successful
|
||||
*/
|
||||
bool setQuality(int quality);
|
||||
|
||||
/**
|
||||
* Get current resolution as string
|
||||
*/
|
||||
const char* getResolutionString();
|
||||
|
||||
/**
|
||||
* Check if camera is initialized
|
||||
*/
|
||||
bool isInitialized() { return _initialized; }
|
||||
|
||||
/**
|
||||
* Get last error message
|
||||
*/
|
||||
const char* getLastError() { return _lastError; }
|
||||
|
||||
private:
|
||||
bool _initialized;
|
||||
const char* _lastError;
|
||||
CameraResolution _currentResolution;
|
||||
int _currentQuality;
|
||||
|
||||
framesize_t resolutionToFramesize(CameraResolution res);
|
||||
};
|
||||
|
||||
// Global instance
|
||||
extern CameraModule Camera;
|
||||
|
||||
#endif // CAMERA_H
|
||||
|
|
@ -0,0 +1,189 @@
|
|||
/**
|
||||
* Claude's Eyes - Configuration Header
|
||||
*
|
||||
* Hardware: Waveshare ESP32-S3-Touch-LCD-2 + Freenove 4WD Car Kit
|
||||
*
|
||||
* WICHTIG: Die GPIO-Pins für Motoren/Servos/Sensoren müssen noch
|
||||
* verifiziert werden, sobald die Hardware da ist!
|
||||
*/
|
||||
|
||||
#ifndef CONFIG_H
|
||||
#define CONFIG_H
|
||||
|
||||
// ============================================================================
|
||||
// WiFi Configuration
|
||||
// ============================================================================
|
||||
#define WIFI_SSID "DEIN_WLAN_NAME"
|
||||
#define WIFI_PASSWORD "DEIN_WLAN_PASSWORT"
|
||||
|
||||
// API Security
|
||||
#define API_KEY "claudes_eyes_secret_2025"
|
||||
|
||||
// Webserver Port
|
||||
#define WEBSERVER_PORT 80
|
||||
|
||||
// ============================================================================
|
||||
// Camera Configuration (OV5640 on Waveshare ESP32-S3-Touch-LCD-2)
|
||||
// ============================================================================
|
||||
// 24-Pin Camera Interface on Waveshare board
|
||||
#define CAMERA_PIN_PWDN -1 // Power down not used
|
||||
#define CAMERA_PIN_RESET -1 // Reset not used
|
||||
#define CAMERA_PIN_XCLK 15 // External clock
|
||||
#define CAMERA_PIN_SIOD 4 // I2C SDA (SCCB)
|
||||
#define CAMERA_PIN_SIOC 5 // I2C SCL (SCCB)
|
||||
|
||||
#define CAMERA_PIN_D7 16 // Data pins
|
||||
#define CAMERA_PIN_D6 17
|
||||
#define CAMERA_PIN_D5 18
|
||||
#define CAMERA_PIN_D4 12 // Note: Might conflict with TFT_RST
|
||||
#define CAMERA_PIN_D3 10 // Note: Might conflict with TFT_CS
|
||||
#define CAMERA_PIN_D2 8
|
||||
#define CAMERA_PIN_D1 9
|
||||
#define CAMERA_PIN_D0 11 // Note: Might conflict with TFT_DC
|
||||
|
||||
#define CAMERA_PIN_VSYNC 6 // Vertical sync
|
||||
#define CAMERA_PIN_HREF 7 // Horizontal reference
|
||||
#define CAMERA_PIN_PCLK 13 // Pixel clock
|
||||
|
||||
// Camera settings
|
||||
#define CAMERA_XCLK_FREQ 20000000 // 20MHz external clock
|
||||
#define DEFAULT_JPEG_QUALITY 12 // 10-63, lower = better quality
|
||||
#define DEFAULT_FRAME_SIZE FRAMESIZE_VGA // 640x480
|
||||
|
||||
// ============================================================================
|
||||
// Motor Configuration (Freenove 4WD Car Kit)
|
||||
// ============================================================================
|
||||
// ACHTUNG: Diese Pins müssen noch mit dem Freenove Shield abgeglichen werden!
|
||||
// Der Freenove Shield verwendet wahrscheinlich TB6612 oder L298N Motortreiber
|
||||
|
||||
// Motor A (linke Seite - 2 Motoren parallel)
|
||||
#define MOTOR_A_IN1 39 // Direction pin 1
|
||||
#define MOTOR_A_IN2 40 // Direction pin 2
|
||||
#define MOTOR_A_PWM 41 // Speed control (PWM)
|
||||
|
||||
// Motor B (rechte Seite - 2 Motoren parallel)
|
||||
#define MOTOR_B_IN1 42 // Direction pin 1
|
||||
#define MOTOR_B_IN2 2 // Direction pin 2
|
||||
#define MOTOR_B_PWM 1 // Speed control (PWM)
|
||||
|
||||
// PWM Configuration
|
||||
#define MOTOR_PWM_FREQ 5000 // 5kHz PWM frequency
|
||||
#define MOTOR_PWM_RESOLUTION 8 // 8-bit resolution (0-255)
|
||||
#define MOTOR_PWM_CHANNEL_A 0 // LEDC channel for motor A
|
||||
#define MOTOR_PWM_CHANNEL_B 1 // LEDC channel for motor B
|
||||
|
||||
// Speed limits
|
||||
#define MOTOR_MAX_SPEED 255 // Maximum PWM value
|
||||
#define MOTOR_MIN_SPEED 50 // Minimum PWM to overcome friction
|
||||
#define DEFAULT_SPEED 150 // Default driving speed
|
||||
|
||||
// ============================================================================
|
||||
// Servo Configuration (Pan/Tilt for Camera)
|
||||
// ============================================================================
|
||||
// Servos vom Freenove Kit
|
||||
#define SERVO_PAN_PIN 38 // Horizontal rotation
|
||||
#define SERVO_TILT_PIN 37 // Vertical rotation
|
||||
|
||||
// Servo limits (in degrees)
|
||||
#define SERVO_PAN_MIN 0
|
||||
#define SERVO_PAN_MAX 180
|
||||
#define SERVO_PAN_CENTER 90
|
||||
|
||||
#define SERVO_TILT_MIN 30 // Don't go too far down
|
||||
#define SERVO_TILT_MAX 150 // Don't go too far up
|
||||
#define SERVO_TILT_CENTER 90
|
||||
|
||||
// Servo movement speed (degrees per step for smooth movement)
|
||||
#define SERVO_STEP_DELAY_MS 15 // Delay between steps
|
||||
#define SERVO_STEP_SIZE 2 // Degrees per step
|
||||
|
||||
// ============================================================================
|
||||
// Ultrasonic Sensor (HC-SR04)
|
||||
// ============================================================================
|
||||
#define ULTRASONIC_TRIGGER_PIN 21
|
||||
#define ULTRASONIC_ECHO_PIN 47
|
||||
|
||||
// Distance thresholds (in cm)
|
||||
#define OBSTACLE_DANGER_DISTANCE 15 // Stop immediately
|
||||
#define OBSTACLE_WARNING_DISTANCE 30 // Slow down
|
||||
#define ULTRASONIC_MAX_DISTANCE 400 // Maximum measurable distance
|
||||
|
||||
// ============================================================================
|
||||
// Display Configuration (Built-in on Waveshare)
|
||||
// ============================================================================
|
||||
// ST7789 240x320 display - pins defined in platformio.ini via TFT_eSPI
|
||||
#define DISPLAY_WIDTH 240
|
||||
#define DISPLAY_HEIGHT 320
|
||||
#define DISPLAY_ROTATION 0 // 0, 1, 2, or 3
|
||||
|
||||
// Backlight
|
||||
#define DISPLAY_BL_PIN 45
|
||||
#define DISPLAY_BL_PWM_CHANNEL 2
|
||||
|
||||
// Colors (RGB565)
|
||||
#define COLOR_BACKGROUND 0x0000 // Black
|
||||
#define COLOR_TEXT 0xFFFF // White
|
||||
#define COLOR_ACCENT 0x07FF // Cyan
|
||||
#define COLOR_WARNING 0xFD20 // Orange
|
||||
#define COLOR_DANGER 0xF800 // Red
|
||||
#define COLOR_SUCCESS 0x07E0 // Green
|
||||
|
||||
// ============================================================================
|
||||
// Touch Configuration (CST816S on Waveshare)
|
||||
// ============================================================================
|
||||
#define TOUCH_SDA_PIN 48
|
||||
#define TOUCH_SCL_PIN 8
|
||||
#define TOUCH_INT_PIN 3
|
||||
#define TOUCH_RST_PIN -1 // Not connected
|
||||
|
||||
// ============================================================================
|
||||
// IMU Configuration (QMI8658 on Waveshare)
|
||||
// ============================================================================
|
||||
#define IMU_SDA_PIN 48 // Shared with touch
|
||||
#define IMU_SCL_PIN 8 // Shared with touch
|
||||
#define IMU_INT1_PIN -1 // Not used
|
||||
#define IMU_INT2_PIN -1 // Not used
|
||||
|
||||
// ============================================================================
|
||||
// LED Matrix (Optional - from Freenove Kit)
|
||||
// ============================================================================
|
||||
#define LED_MATRIX_DIN_PIN 35
|
||||
#define LED_MATRIX_CLK_PIN 36
|
||||
#define LED_MATRIX_CS_PIN 0
|
||||
|
||||
// ============================================================================
|
||||
// RGB LEDs (Optional - from Freenove Kit)
|
||||
// ============================================================================
|
||||
#define RGB_LED_PIN 48 // WS2812 data pin
|
||||
#define RGB_LED_COUNT 4 // Number of LEDs
|
||||
|
||||
// ============================================================================
|
||||
// System Configuration
|
||||
// ============================================================================
|
||||
// Task intervals (in milliseconds)
|
||||
#define SENSOR_UPDATE_INTERVAL 100 // Read sensors every 100ms
|
||||
#define DISPLAY_UPDATE_INTERVAL 250 // Update display 4x per second
|
||||
#define STATUS_BROADCAST_INTERVAL 1000 // Broadcast status every second
|
||||
|
||||
// Safety
|
||||
#define COMMAND_TIMEOUT_MS 5000 // Stop if no command for 5 seconds
|
||||
#define MAX_CONTINUOUS_DRIVE_MS 30000 // Max 30 seconds continuous driving
|
||||
|
||||
// Debug
|
||||
#define DEBUG_SERIAL true // Enable serial debug output
|
||||
#define DEBUG_BAUD_RATE 115200
|
||||
|
||||
// ============================================================================
|
||||
// Feature Toggles
|
||||
// ============================================================================
|
||||
#define ENABLE_CAMERA true
|
||||
#define ENABLE_MOTORS true
|
||||
#define ENABLE_SERVOS true
|
||||
#define ENABLE_ULTRASONIC true
|
||||
#define ENABLE_IMU true
|
||||
#define ENABLE_DISPLAY true
|
||||
#define ENABLE_TOUCH true
|
||||
#define ENABLE_LED_MATRIX false // Disabled by default
|
||||
#define ENABLE_RGB_LEDS false // Disabled by default
|
||||
|
||||
#endif // CONFIG_H
|
||||
|
|
@ -0,0 +1,414 @@
|
|||
/**
|
||||
* Claude's Eyes - Display Module Implementation
|
||||
*
|
||||
* ST7789 TFT with CST816S capacitive touch
|
||||
*/
|
||||
|
||||
#include "display.h"
|
||||
#include "config.h"
|
||||
|
||||
// Global instance
|
||||
DisplayModule Display;
|
||||
|
||||
DisplayModule::DisplayModule()
|
||||
: _touch(TOUCH_SDA_PIN, TOUCH_SCL_PIN, TOUCH_RST_PIN, TOUCH_INT_PIN)
|
||||
, _initialized(false)
|
||||
, _currentMode(MODE_STATUS)
|
||||
, _lastUpdateTime(0)
|
||||
, _scrollOffset(0)
|
||||
, _distance(0)
|
||||
, _battery(100)
|
||||
, _wifiRssi(0)
|
||||
{
|
||||
}
|
||||
|
||||
bool DisplayModule::begin() {
|
||||
#if !ENABLE_DISPLAY
|
||||
Serial.println("[Display] Disabled in config");
|
||||
return false;
|
||||
#endif
|
||||
|
||||
// Initialize TFT
|
||||
_tft.init();
|
||||
_tft.setRotation(DISPLAY_ROTATION);
|
||||
_tft.fillScreen(COLOR_BACKGROUND);
|
||||
|
||||
// Configure backlight PWM
|
||||
ledcSetup(DISPLAY_BL_PWM_CHANNEL, 5000, 8);
|
||||
ledcAttachPin(DISPLAY_BL_PIN, DISPLAY_BL_PWM_CHANNEL);
|
||||
setBrightness(200); // Start at ~80%
|
||||
|
||||
#if ENABLE_TOUCH
|
||||
// Initialize touch
|
||||
_touch.begin();
|
||||
Serial.println("[Display] Touch initialized");
|
||||
#endif
|
||||
|
||||
// Show startup screen
|
||||
_tft.setTextColor(COLOR_ACCENT, COLOR_BACKGROUND);
|
||||
_tft.setTextSize(2);
|
||||
_tft.setTextDatum(MC_DATUM);
|
||||
_tft.drawString("Claude's Eyes", DISPLAY_WIDTH / 2, DISPLAY_HEIGHT / 2 - 20);
|
||||
_tft.setTextSize(1);
|
||||
_tft.setTextColor(TFT_WHITE, COLOR_BACKGROUND);
|
||||
_tft.drawString("Initializing...", DISPLAY_WIDTH / 2, DISPLAY_HEIGHT / 2 + 20);
|
||||
|
||||
_initialized = true;
|
||||
Serial.println("[Display] Initialized successfully");
|
||||
return true;
|
||||
}
|
||||
|
||||
void DisplayModule::update() {
|
||||
if (!_initialized) return;
|
||||
|
||||
unsigned long now = millis();
|
||||
if (now - _lastUpdateTime < DISPLAY_UPDATE_INTERVAL) {
|
||||
return;
|
||||
}
|
||||
_lastUpdateTime = now;
|
||||
|
||||
switch (_currentMode) {
|
||||
case MODE_STATUS:
|
||||
drawStatusScreen();
|
||||
break;
|
||||
case MODE_CONTROLS:
|
||||
drawControlsScreen();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void DisplayModule::setMode(DisplayMode mode) {
|
||||
if (mode == _currentMode) return;
|
||||
|
||||
_currentMode = mode;
|
||||
_tft.fillScreen(COLOR_BACKGROUND);
|
||||
|
||||
Serial.printf("[Display] Mode changed to %d\n", mode);
|
||||
}
|
||||
|
||||
void DisplayModule::showEmoji(EmojiType emoji) {
|
||||
setMode(MODE_EMOJI);
|
||||
drawEmojiScreen(emoji);
|
||||
}
|
||||
|
||||
void DisplayModule::showMessage(const char* message, uint16_t color) {
|
||||
setMode(MODE_MESSAGE);
|
||||
drawMessageScreen(message, color);
|
||||
}
|
||||
|
||||
void DisplayModule::updateStatus(const char* action, float distance,
|
||||
int battery, int wifiRssi) {
|
||||
_currentAction = action;
|
||||
_distance = distance;
|
||||
_battery = battery;
|
||||
_wifiRssi = wifiRssi;
|
||||
|
||||
if (_currentMode == MODE_STATUS) {
|
||||
// Will be drawn on next update()
|
||||
}
|
||||
}
|
||||
|
||||
void DisplayModule::setClaudeText(const char* text) {
|
||||
_claudeText = text;
|
||||
_scrollOffset = 0;
|
||||
}
|
||||
|
||||
TouchButton DisplayModule::checkTouch() {
|
||||
#if !ENABLE_TOUCH
|
||||
return BTN_NONE;
|
||||
#endif
|
||||
|
||||
if (!_touch.available()) {
|
||||
return BTN_NONE;
|
||||
}
|
||||
|
||||
int x = _touch.data.x;
|
||||
int y = _touch.data.y;
|
||||
|
||||
// Adjust for rotation if needed
|
||||
// x and y are in display coordinates
|
||||
|
||||
if (_currentMode == MODE_CONTROLS) {
|
||||
// Control layout:
|
||||
// [ STOP ] (top, full width)
|
||||
// [ ^ ] (forward)
|
||||
// [< HOME >] (left, home, right)
|
||||
// [ v ] (backward)
|
||||
|
||||
// Stop button (top 60 pixels)
|
||||
if (y < 60) {
|
||||
return BTN_STOP;
|
||||
}
|
||||
|
||||
// Forward (y 60-120, center)
|
||||
if (y >= 60 && y < 120 && x >= 80 && x < 160) {
|
||||
return BTN_FORWARD;
|
||||
}
|
||||
|
||||
// Middle row (y 120-200)
|
||||
if (y >= 120 && y < 200) {
|
||||
if (x < 80) return BTN_LEFT;
|
||||
if (x >= 160) return BTN_RIGHT;
|
||||
return BTN_HOME;
|
||||
}
|
||||
|
||||
// Backward (y 200-260, center)
|
||||
if (y >= 200 && y < 260 && x >= 80 && x < 160) {
|
||||
return BTN_BACKWARD;
|
||||
}
|
||||
}
|
||||
|
||||
// In status mode, any touch switches to controls
|
||||
if (_currentMode == MODE_STATUS) {
|
||||
setMode(MODE_CONTROLS);
|
||||
}
|
||||
|
||||
return BTN_NONE;
|
||||
}
|
||||
|
||||
void DisplayModule::setBrightness(uint8_t brightness) {
|
||||
ledcWrite(DISPLAY_BL_PWM_CHANNEL, brightness);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Drawing Functions
|
||||
// ============================================================================
|
||||
|
||||
void DisplayModule::drawStatusScreen() {
|
||||
// Header bar with icons
|
||||
_tft.fillRect(0, 0, DISPLAY_WIDTH, 30, TFT_DARKGREY);
|
||||
drawWifiIcon(5, 5, _wifiRssi);
|
||||
drawBatteryIcon(DISPLAY_WIDTH - 35, 5, _battery);
|
||||
|
||||
// Current action (large, centered)
|
||||
_tft.setTextColor(COLOR_ACCENT, COLOR_BACKGROUND);
|
||||
_tft.setTextSize(3);
|
||||
_tft.setTextDatum(TC_DATUM);
|
||||
|
||||
// Action icons
|
||||
const char* actionIcon = "?";
|
||||
if (_currentAction == "idle" || _currentAction == "stop") actionIcon = "STOP";
|
||||
else if (_currentAction == "forward") actionIcon = "^";
|
||||
else if (_currentAction == "backward") actionIcon = "v";
|
||||
else if (_currentAction == "left") actionIcon = "<";
|
||||
else if (_currentAction == "right") actionIcon = ">";
|
||||
|
||||
_tft.fillRect(0, 50, DISPLAY_WIDTH, 60, COLOR_BACKGROUND);
|
||||
_tft.drawString(actionIcon, DISPLAY_WIDTH / 2, 70);
|
||||
|
||||
// Distance indicator
|
||||
_tft.setTextSize(1);
|
||||
_tft.setTextColor(TFT_WHITE, COLOR_BACKGROUND);
|
||||
_tft.setTextDatum(TL_DATUM);
|
||||
_tft.fillRect(0, 130, DISPLAY_WIDTH, 20, COLOR_BACKGROUND);
|
||||
|
||||
char distStr[32];
|
||||
snprintf(distStr, sizeof(distStr), "Distance: %.0f cm", _distance);
|
||||
_tft.drawString(distStr, 10, 130);
|
||||
|
||||
// Distance bar
|
||||
int barWidth = map(constrain(_distance, 0, 100), 0, 100, DISPLAY_WIDTH - 20, 0);
|
||||
uint16_t barColor = _distance > OBSTACLE_WARNING_DISTANCE ? COLOR_SUCCESS :
|
||||
_distance > OBSTACLE_DANGER_DISTANCE ? COLOR_WARNING : COLOR_DANGER;
|
||||
_tft.fillRect(10, 150, DISPLAY_WIDTH - 20, 10, TFT_DARKGREY);
|
||||
_tft.fillRect(10, 150, barWidth, 10, barColor);
|
||||
|
||||
// Claude's text (scrolling)
|
||||
_tft.fillRect(0, 180, DISPLAY_WIDTH, DISPLAY_HEIGHT - 180, TFT_DARKGREY);
|
||||
_tft.setTextColor(TFT_WHITE, TFT_DARKGREY);
|
||||
_tft.setTextSize(1);
|
||||
_tft.setTextDatum(TL_DATUM);
|
||||
|
||||
if (_claudeText.length() > 0) {
|
||||
// Word wrap and display
|
||||
String displayText = _claudeText;
|
||||
int y = 185;
|
||||
int lineHeight = 15;
|
||||
int maxCharsPerLine = 38;
|
||||
|
||||
while (displayText.length() > 0 && y < DISPLAY_HEIGHT - 10) {
|
||||
String line = displayText.substring(0, min((int)displayText.length(), maxCharsPerLine));
|
||||
_tft.drawString(line.c_str(), 5, y);
|
||||
displayText = displayText.substring(line.length());
|
||||
y += lineHeight;
|
||||
}
|
||||
}
|
||||
|
||||
// Touch hint
|
||||
_tft.setTextDatum(BC_DATUM);
|
||||
_tft.setTextColor(TFT_DARKGREY, COLOR_BACKGROUND);
|
||||
_tft.drawString("Touch for controls", DISPLAY_WIDTH / 2, DISPLAY_HEIGHT - 5);
|
||||
}
|
||||
|
||||
void DisplayModule::drawControlsScreen() {
|
||||
// Stop button (red, top)
|
||||
_tft.fillRect(10, 10, DISPLAY_WIDTH - 20, 50, COLOR_DANGER);
|
||||
_tft.setTextColor(TFT_WHITE, COLOR_DANGER);
|
||||
_tft.setTextSize(2);
|
||||
_tft.setTextDatum(MC_DATUM);
|
||||
_tft.drawString("STOP", DISPLAY_WIDTH / 2, 35);
|
||||
|
||||
// Forward button
|
||||
_tft.fillRect(80, 70, 80, 50, TFT_DARKGREY);
|
||||
_tft.setTextColor(TFT_WHITE, TFT_DARKGREY);
|
||||
_tft.drawString("^", 120, 95);
|
||||
|
||||
// Left button
|
||||
_tft.fillRect(10, 130, 60, 60, TFT_DARKGREY);
|
||||
_tft.drawString("<", 40, 160);
|
||||
|
||||
// Home button
|
||||
_tft.fillRect(80, 130, 80, 60, COLOR_ACCENT);
|
||||
_tft.setTextColor(TFT_BLACK, COLOR_ACCENT);
|
||||
_tft.drawString("HOME", 120, 160);
|
||||
|
||||
// Right button
|
||||
_tft.fillRect(170, 130, 60, 60, TFT_DARKGREY);
|
||||
_tft.setTextColor(TFT_WHITE, TFT_DARKGREY);
|
||||
_tft.drawString(">", 200, 160);
|
||||
|
||||
// Backward button
|
||||
_tft.fillRect(80, 200, 80, 50, TFT_DARKGREY);
|
||||
_tft.drawString("v", 120, 225);
|
||||
|
||||
// Back to status hint
|
||||
_tft.setTextSize(1);
|
||||
_tft.setTextDatum(BC_DATUM);
|
||||
_tft.setTextColor(TFT_DARKGREY, COLOR_BACKGROUND);
|
||||
_tft.drawString("Auto-return to status", DISPLAY_WIDTH / 2, DISPLAY_HEIGHT - 5);
|
||||
}
|
||||
|
||||
void DisplayModule::drawEmojiScreen(EmojiType emoji) {
|
||||
int cx = DISPLAY_WIDTH / 2;
|
||||
int cy = DISPLAY_HEIGHT / 2;
|
||||
int radius = 60;
|
||||
|
||||
switch (emoji) {
|
||||
case EMOJI_HAPPY:
|
||||
drawHappyFace(cx, cy, radius);
|
||||
break;
|
||||
case EMOJI_THINKING:
|
||||
drawThinkingFace(cx, cy, radius);
|
||||
break;
|
||||
case EMOJI_SURPRISED:
|
||||
drawSurprisedFace(cx, cy, radius);
|
||||
break;
|
||||
case EMOJI_SLEEPY:
|
||||
drawSleepyFace(cx, cy, radius);
|
||||
break;
|
||||
case EMOJI_CURIOUS:
|
||||
drawCuriousFace(cx, cy, radius);
|
||||
break;
|
||||
case EMOJI_CONFUSED:
|
||||
drawConfusedFace(cx, cy, radius);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void DisplayModule::drawMessageScreen(const char* message, uint16_t color) {
|
||||
_tft.setTextColor(color, COLOR_BACKGROUND);
|
||||
_tft.setTextSize(2);
|
||||
_tft.setTextDatum(MC_DATUM);
|
||||
_tft.drawString(message, DISPLAY_WIDTH / 2, DISPLAY_HEIGHT / 2);
|
||||
}
|
||||
|
||||
void DisplayModule::drawWifiIcon(int x, int y, int rssi) {
|
||||
uint16_t color = rssi > -50 ? COLOR_SUCCESS :
|
||||
rssi > -70 ? COLOR_WARNING : COLOR_DANGER;
|
||||
|
||||
// Simple 3-bar WiFi icon
|
||||
int bars = rssi > -50 ? 3 : rssi > -70 ? 2 : rssi > -90 ? 1 : 0;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
uint16_t barColor = (i < bars) ? color : TFT_DARKGREY;
|
||||
_tft.fillRect(x + i * 8, y + (2 - i) * 6, 6, 6 + i * 6, barColor);
|
||||
}
|
||||
}
|
||||
|
||||
void DisplayModule::drawBatteryIcon(int x, int y, int percent) {
|
||||
uint16_t color = percent > 50 ? COLOR_SUCCESS :
|
||||
percent > 20 ? COLOR_WARNING : COLOR_DANGER;
|
||||
|
||||
// Battery outline
|
||||
_tft.drawRect(x, y, 28, 16, TFT_WHITE);
|
||||
_tft.fillRect(x + 28, y + 4, 3, 8, TFT_WHITE);
|
||||
|
||||
// Fill level
|
||||
int fillWidth = map(percent, 0, 100, 0, 24);
|
||||
_tft.fillRect(x + 2, y + 2, fillWidth, 12, color);
|
||||
}
|
||||
|
||||
// Emoji faces
|
||||
void DisplayModule::drawHappyFace(int cx, int cy, int r) {
|
||||
_tft.fillCircle(cx, cy, r, TFT_YELLOW);
|
||||
_tft.fillCircle(cx - 20, cy - 15, 8, TFT_BLACK); // Left eye
|
||||
_tft.fillCircle(cx + 20, cy - 15, 8, TFT_BLACK); // Right eye
|
||||
// Smile arc
|
||||
for (int i = -30; i <= 30; i++) {
|
||||
int px = cx + i;
|
||||
int py = cy + 20 + (i * i) / 50;
|
||||
_tft.fillCircle(px, py, 3, TFT_BLACK);
|
||||
}
|
||||
}
|
||||
|
||||
void DisplayModule::drawThinkingFace(int cx, int cy, int r) {
|
||||
_tft.fillCircle(cx, cy, r, TFT_YELLOW);
|
||||
_tft.fillCircle(cx - 20, cy - 15, 8, TFT_BLACK); // Left eye
|
||||
_tft.fillCircle(cx + 20, cy - 15, 8, TFT_BLACK); // Right eye
|
||||
// Raised eyebrow (right)
|
||||
_tft.drawLine(cx + 10, cy - 30, cx + 35, cy - 35, TFT_BLACK);
|
||||
// Thinking mouth (small line)
|
||||
_tft.drawLine(cx - 15, cy + 25, cx + 15, cy + 25, TFT_BLACK);
|
||||
}
|
||||
|
||||
void DisplayModule::drawSurprisedFace(int cx, int cy, int r) {
|
||||
_tft.fillCircle(cx, cy, r, TFT_YELLOW);
|
||||
_tft.fillCircle(cx - 20, cy - 15, 12, TFT_BLACK); // Left eye (big)
|
||||
_tft.fillCircle(cx + 20, cy - 15, 12, TFT_BLACK); // Right eye (big)
|
||||
// O mouth
|
||||
_tft.fillCircle(cx, cy + 25, 15, TFT_BLACK);
|
||||
}
|
||||
|
||||
void DisplayModule::drawSleepyFace(int cx, int cy, int r) {
|
||||
_tft.fillCircle(cx, cy, r, TFT_YELLOW);
|
||||
// Closed eyes (lines)
|
||||
_tft.drawLine(cx - 30, cy - 15, cx - 10, cy - 15, TFT_BLACK);
|
||||
_tft.drawLine(cx + 10, cy - 15, cx + 30, cy - 15, TFT_BLACK);
|
||||
// Small mouth
|
||||
_tft.drawLine(cx - 10, cy + 25, cx + 10, cy + 25, TFT_BLACK);
|
||||
// Z's
|
||||
_tft.setTextColor(TFT_BLACK, TFT_YELLOW);
|
||||
_tft.setTextSize(2);
|
||||
_tft.drawString("z", cx + 45, cy - 40);
|
||||
_tft.setTextSize(1);
|
||||
_tft.drawString("z", cx + 55, cy - 50);
|
||||
}
|
||||
|
||||
void DisplayModule::drawCuriousFace(int cx, int cy, int r) {
|
||||
_tft.fillCircle(cx, cy, r, TFT_YELLOW);
|
||||
_tft.fillCircle(cx - 20, cy - 15, 8, TFT_BLACK); // Left eye
|
||||
_tft.fillCircle(cx + 20, cy - 15, 10, TFT_BLACK); // Right eye (bigger)
|
||||
// Raised eyebrow
|
||||
_tft.drawLine(cx + 10, cy - 30, cx + 35, cy - 25, TFT_BLACK);
|
||||
// Small smile
|
||||
_tft.drawLine(cx - 10, cy + 25, cx + 15, cy + 20, TFT_BLACK);
|
||||
}
|
||||
|
||||
void DisplayModule::drawConfusedFace(int cx, int cy, int r) {
|
||||
_tft.fillCircle(cx, cy, r, TFT_YELLOW);
|
||||
_tft.fillCircle(cx - 20, cy - 15, 8, TFT_BLACK); // Left eye
|
||||
_tft.fillCircle(cx + 20, cy - 15, 8, TFT_BLACK); // Right eye
|
||||
// Squiggly eyebrows
|
||||
for (int i = 0; i < 20; i++) {
|
||||
int py = cy - 30 + sin(i * 0.5) * 3;
|
||||
_tft.drawPixel(cx - 30 + i, py, TFT_BLACK);
|
||||
_tft.drawPixel(cx + 10 + i, py, TFT_BLACK);
|
||||
}
|
||||
// Wavy mouth
|
||||
for (int i = -15; i <= 15; i++) {
|
||||
int py = cy + 25 + sin(i * 0.3) * 5;
|
||||
_tft.fillCircle(cx + i, py, 2, TFT_BLACK);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,143 @@
|
|||
/**
|
||||
* Claude's Eyes - Display Module Header
|
||||
*
|
||||
* ST7789 240x320 TFT with CST816S touch on Waveshare ESP32-S3
|
||||
*/
|
||||
|
||||
#ifndef DISPLAY_H
|
||||
#define DISPLAY_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <TFT_eSPI.h>
|
||||
#include <CST816S.h>
|
||||
|
||||
// Display modes
|
||||
enum DisplayMode {
|
||||
MODE_STATUS, // Show status information
|
||||
MODE_EMOJI, // Show emoji/expression
|
||||
MODE_MESSAGE, // Show text message
|
||||
MODE_CONTROLS // Show touch controls
|
||||
};
|
||||
|
||||
// Emoji types for expressions
|
||||
enum EmojiType {
|
||||
EMOJI_HAPPY,
|
||||
EMOJI_THINKING,
|
||||
EMOJI_SURPRISED,
|
||||
EMOJI_SLEEPY,
|
||||
EMOJI_CURIOUS,
|
||||
EMOJI_CONFUSED
|
||||
};
|
||||
|
||||
// Touch button IDs
|
||||
enum TouchButton {
|
||||
BTN_NONE = 0,
|
||||
BTN_STOP, // Emergency stop
|
||||
BTN_FORWARD,
|
||||
BTN_BACKWARD,
|
||||
BTN_LEFT,
|
||||
BTN_RIGHT,
|
||||
BTN_HOME // Return to base
|
||||
};
|
||||
|
||||
class DisplayModule {
|
||||
public:
|
||||
DisplayModule();
|
||||
|
||||
/**
|
||||
* Initialize display and touch
|
||||
* @return true if successful
|
||||
*/
|
||||
bool begin();
|
||||
|
||||
/**
|
||||
* Update display (call from loop)
|
||||
*/
|
||||
void update();
|
||||
|
||||
/**
|
||||
* Set display mode
|
||||
*/
|
||||
void setMode(DisplayMode mode);
|
||||
|
||||
/**
|
||||
* Show emoji expression
|
||||
*/
|
||||
void showEmoji(EmojiType emoji);
|
||||
|
||||
/**
|
||||
* Show text message
|
||||
* @param message Text to display
|
||||
* @param color Text color (default white)
|
||||
*/
|
||||
void showMessage(const char* message, uint16_t color = TFT_WHITE);
|
||||
|
||||
/**
|
||||
* Update status display
|
||||
* @param action Current action string
|
||||
* @param distance Distance to obstacle (cm)
|
||||
* @param battery Battery percentage
|
||||
* @param wifiRssi WiFi signal strength
|
||||
*/
|
||||
void updateStatus(const char* action, float distance,
|
||||
int battery, int wifiRssi);
|
||||
|
||||
/**
|
||||
* Set Claude's last message (scrolling text)
|
||||
*/
|
||||
void setClaudeText(const char* text);
|
||||
|
||||
/**
|
||||
* Check for touch input
|
||||
* @return Button ID or BTN_NONE
|
||||
*/
|
||||
TouchButton checkTouch();
|
||||
|
||||
/**
|
||||
* Set backlight brightness
|
||||
* @param brightness 0-255
|
||||
*/
|
||||
void setBrightness(uint8_t brightness);
|
||||
|
||||
/**
|
||||
* Get display object for direct access
|
||||
*/
|
||||
TFT_eSPI& getTFT() { return _tft; }
|
||||
|
||||
private:
|
||||
TFT_eSPI _tft;
|
||||
CST816S _touch;
|
||||
|
||||
bool _initialized;
|
||||
DisplayMode _currentMode;
|
||||
unsigned long _lastUpdateTime;
|
||||
int _scrollOffset;
|
||||
String _claudeText;
|
||||
|
||||
// Status values
|
||||
String _currentAction;
|
||||
float _distance;
|
||||
int _battery;
|
||||
int _wifiRssi;
|
||||
|
||||
// Drawing functions
|
||||
void drawStatusScreen();
|
||||
void drawEmojiScreen(EmojiType emoji);
|
||||
void drawMessageScreen(const char* message, uint16_t color);
|
||||
void drawControlsScreen();
|
||||
void drawWifiIcon(int x, int y, int rssi);
|
||||
void drawBatteryIcon(int x, int y, int percent);
|
||||
|
||||
// Emoji drawing
|
||||
void drawHappyFace(int centerX, int centerY, int radius);
|
||||
void drawThinkingFace(int centerX, int centerY, int radius);
|
||||
void drawSurprisedFace(int centerX, int centerY, int radius);
|
||||
void drawSleepyFace(int centerX, int centerY, int radius);
|
||||
void drawCuriousFace(int centerX, int centerY, int radius);
|
||||
void drawConfusedFace(int centerX, int centerY, int radius);
|
||||
};
|
||||
|
||||
// Global instance
|
||||
extern DisplayModule Display;
|
||||
|
||||
#endif // DISPLAY_H
|
||||
|
|
@ -0,0 +1,150 @@
|
|||
/**
|
||||
* Claude's Eyes - IMU Module Implementation
|
||||
*
|
||||
* QMI8658 6-axis IMU using SensorLib
|
||||
*/
|
||||
|
||||
#include "imu.h"
|
||||
#include "config.h"
|
||||
#include <Wire.h>
|
||||
#include <SensorQMI8658.hpp>
|
||||
|
||||
// Global instance
|
||||
IMUModule IMU;
|
||||
|
||||
// Sensor instance
|
||||
static SensorQMI8658 qmi;
|
||||
|
||||
IMUModule::IMUModule()
|
||||
: _initialized(false)
|
||||
, _lastError(nullptr)
|
||||
, _lastUpdateTime(0)
|
||||
{
|
||||
memset(&_data, 0, sizeof(_data));
|
||||
}
|
||||
|
||||
bool IMUModule::begin() {
|
||||
#if !ENABLE_IMU
|
||||
Serial.println("[IMU] Disabled in config");
|
||||
return false;
|
||||
#endif
|
||||
|
||||
// Initialize I2C (shared with touch on Waveshare board)
|
||||
Wire.begin(IMU_SDA_PIN, IMU_SCL_PIN);
|
||||
|
||||
// Initialize QMI8658
|
||||
if (!qmi.begin(Wire, QMI8658_L_SLAVE_ADDRESS, IMU_SDA_PIN, IMU_SCL_PIN)) {
|
||||
// Try alternate address
|
||||
if (!qmi.begin(Wire, QMI8658_H_SLAVE_ADDRESS, IMU_SDA_PIN, IMU_SCL_PIN)) {
|
||||
_lastError = "QMI8658 not found";
|
||||
Serial.println("[IMU] QMI8658 not found on I2C");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
Serial.printf("[IMU] QMI8658 found, Chip ID: 0x%02X\n", qmi.getChipID());
|
||||
|
||||
// Configure accelerometer
|
||||
// Range: ±8g, ODR: 896.8Hz
|
||||
qmi.configAccelerometer(
|
||||
SensorQMI8658::ACC_RANGE_8G,
|
||||
SensorQMI8658::ACC_ODR_896_8Hz,
|
||||
SensorQMI8658::LPF_MODE_0
|
||||
);
|
||||
|
||||
// Configure gyroscope
|
||||
// Range: ±512dps, ODR: 896.8Hz
|
||||
qmi.configGyroscope(
|
||||
SensorQMI8658::GYR_RANGE_512DPS,
|
||||
SensorQMI8658::GYR_ODR_896_8Hz,
|
||||
SensorQMI8658::LPF_MODE_0
|
||||
);
|
||||
|
||||
// Enable sensors
|
||||
qmi.enableAccelerometer();
|
||||
qmi.enableGyroscope();
|
||||
|
||||
_initialized = true;
|
||||
Serial.println("[IMU] Initialized successfully");
|
||||
|
||||
// Take initial reading
|
||||
update();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void IMUModule::update() {
|
||||
if (!_initialized) return;
|
||||
|
||||
unsigned long now = millis();
|
||||
if (now - _lastUpdateTime < UPDATE_INTERVAL) {
|
||||
return;
|
||||
}
|
||||
|
||||
float dt = (now - _lastUpdateTime) / 1000.0f;
|
||||
_lastUpdateTime = now;
|
||||
|
||||
// Read sensor data
|
||||
if (qmi.getDataReady()) {
|
||||
float ax, ay, az;
|
||||
float gx, gy, gz;
|
||||
|
||||
if (qmi.getAccelerometer(ax, ay, az)) {
|
||||
_data.accel_x = ax;
|
||||
_data.accel_y = ay;
|
||||
_data.accel_z = az;
|
||||
}
|
||||
|
||||
if (qmi.getGyroscope(gx, gy, gz)) {
|
||||
_data.gyro_x = gx;
|
||||
_data.gyro_y = gy;
|
||||
_data.gyro_z = gz;
|
||||
}
|
||||
|
||||
_data.temperature = qmi.getTemperature_C();
|
||||
|
||||
// Compute orientation angles
|
||||
computeAngles();
|
||||
|
||||
// Integrate yaw from gyroscope
|
||||
if (dt > 0 && dt < 1.0f) { // Sanity check
|
||||
_data.yaw += _data.gyro_z * dt;
|
||||
|
||||
// Keep yaw in -180 to 180 range
|
||||
while (_data.yaw > 180.0f) _data.yaw -= 360.0f;
|
||||
while (_data.yaw < -180.0f) _data.yaw += 360.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IMUModule::computeAngles() {
|
||||
// Calculate pitch and roll from accelerometer
|
||||
// Assumes sensor is mounted level when robot is flat
|
||||
|
||||
// Pitch: rotation around X axis
|
||||
// tan(pitch) = -ax / sqrt(ay² + az²)
|
||||
_data.pitch = atan2(-_data.accel_x,
|
||||
sqrt(_data.accel_y * _data.accel_y +
|
||||
_data.accel_z * _data.accel_z)) * 180.0f / PI;
|
||||
|
||||
// Roll: rotation around Y axis
|
||||
// tan(roll) = ay / az
|
||||
_data.roll = atan2(_data.accel_y, _data.accel_z) * 180.0f / PI;
|
||||
}
|
||||
|
||||
bool IMUModule::isTilted() {
|
||||
// Check if tilted more than 30 degrees in any direction
|
||||
const float TILT_THRESHOLD = 30.0f;
|
||||
return (abs(_data.pitch) > TILT_THRESHOLD ||
|
||||
abs(_data.roll) > TILT_THRESHOLD);
|
||||
}
|
||||
|
||||
bool IMUModule::isUpsideDown() {
|
||||
// Z-axis should be positive (pointing up) when right-side up
|
||||
// If Z is negative, robot is flipped
|
||||
return _data.accel_z < -5.0f; // Threshold accounts for noise
|
||||
}
|
||||
|
||||
void IMUModule::resetYaw() {
|
||||
_data.yaw = 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
/**
|
||||
* Claude's Eyes - IMU Module Header
|
||||
*
|
||||
* QMI8658 6-axis IMU (accelerometer + gyroscope) on Waveshare board
|
||||
*/
|
||||
|
||||
#ifndef IMU_H
|
||||
#define IMU_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// IMU data structure
|
||||
struct IMUData {
|
||||
// Accelerometer (m/s²)
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
|
||||
// Gyroscope (degrees/s)
|
||||
float gyro_x;
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
|
||||
// Computed values
|
||||
float pitch; // Rotation around X axis (degrees)
|
||||
float roll; // Rotation around Y axis (degrees)
|
||||
float yaw; // Rotation around Z axis (degrees, integrated from gyro)
|
||||
|
||||
// Temperature
|
||||
float temperature;
|
||||
};
|
||||
|
||||
class IMUModule {
|
||||
public:
|
||||
IMUModule();
|
||||
|
||||
/**
|
||||
* Initialize the IMU
|
||||
* @return true if successful
|
||||
*/
|
||||
bool begin();
|
||||
|
||||
/**
|
||||
* Update IMU readings
|
||||
*/
|
||||
void update();
|
||||
|
||||
/**
|
||||
* Get current IMU data
|
||||
*/
|
||||
const IMUData& getData() { return _data; }
|
||||
|
||||
/**
|
||||
* Check if robot is tilted beyond safe angle
|
||||
*/
|
||||
bool isTilted();
|
||||
|
||||
/**
|
||||
* Check if robot is upside down
|
||||
*/
|
||||
bool isUpsideDown();
|
||||
|
||||
/**
|
||||
* Reset yaw integration
|
||||
*/
|
||||
void resetYaw();
|
||||
|
||||
/**
|
||||
* Check if initialized
|
||||
*/
|
||||
bool isInitialized() { return _initialized; }
|
||||
|
||||
/**
|
||||
* Get last error
|
||||
*/
|
||||
const char* getLastError() { return _lastError; }
|
||||
|
||||
private:
|
||||
bool _initialized;
|
||||
const char* _lastError;
|
||||
IMUData _data;
|
||||
|
||||
unsigned long _lastUpdateTime;
|
||||
static const unsigned long UPDATE_INTERVAL = 20; // 50Hz update rate
|
||||
|
||||
void computeAngles();
|
||||
};
|
||||
|
||||
// Global instance
|
||||
extern IMUModule IMU;
|
||||
|
||||
#endif // IMU_H
|
||||
|
|
@ -0,0 +1,302 @@
|
|||
/**
|
||||
* Claude's Eyes - Main Program
|
||||
*
|
||||
* Autonomous exploration robot controlled by Claude AI
|
||||
*
|
||||
* Hardware:
|
||||
* - Waveshare ESP32-S3-Touch-LCD-2
|
||||
* - OV5640 Camera (120° wide-angle)
|
||||
* - Freenove 4WD Car Kit
|
||||
*
|
||||
* Created: December 2025
|
||||
* By: Stefan (HackerSoft) & Claude (Anthropic)
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <WiFi.h>
|
||||
#include "config.h"
|
||||
#include "camera.h"
|
||||
#include "motor_control.h"
|
||||
#include "servo_control.h"
|
||||
#include "ultrasonic.h"
|
||||
#include "imu.h"
|
||||
#include "display.h"
|
||||
#include "webserver.h"
|
||||
|
||||
// State machine
|
||||
enum RobotState {
|
||||
STATE_INITIALIZING,
|
||||
STATE_CONNECTING,
|
||||
STATE_READY,
|
||||
STATE_EXPLORING,
|
||||
STATE_OBSTACLE,
|
||||
STATE_ERROR
|
||||
};
|
||||
|
||||
RobotState currentState = STATE_INITIALIZING;
|
||||
unsigned long lastCommandTime = 0;
|
||||
unsigned long lastStatusUpdate = 0;
|
||||
|
||||
// Forward declarations
|
||||
void setupWiFi();
|
||||
void handleSafety();
|
||||
void handleTouchInput();
|
||||
void updateStatusDisplay();
|
||||
|
||||
void setup() {
|
||||
// Initialize serial
|
||||
Serial.begin(DEBUG_BAUD_RATE);
|
||||
while (!Serial && millis() < 3000) {
|
||||
delay(10);
|
||||
}
|
||||
Serial.println("\n\n");
|
||||
Serial.println("╔═══════════════════════════════════════╗");
|
||||
Serial.println("║ Claude's Eyes v1.0 ║");
|
||||
Serial.println("║ Autonomous Exploration Robot ║");
|
||||
Serial.println("╚═══════════════════════════════════════╝");
|
||||
Serial.println();
|
||||
|
||||
// Initialize display first (for visual feedback)
|
||||
Serial.println("[Main] Initializing display...");
|
||||
if (Display.begin()) {
|
||||
Display.showMessage("Starting...", TFT_CYAN);
|
||||
}
|
||||
delay(500);
|
||||
|
||||
// Initialize camera
|
||||
Serial.println("[Main] Initializing camera...");
|
||||
if (!Camera.begin()) {
|
||||
Serial.println("[Main] WARNING: Camera init failed!");
|
||||
Display.showMessage("Camera Error!", TFT_RED);
|
||||
delay(2000);
|
||||
} else {
|
||||
Serial.println("[Main] Camera ready");
|
||||
}
|
||||
|
||||
// Initialize motors
|
||||
Serial.println("[Main] Initializing motors...");
|
||||
if (!Motors.begin()) {
|
||||
Serial.println("[Main] WARNING: Motors init failed!");
|
||||
} else {
|
||||
Serial.println("[Main] Motors ready");
|
||||
}
|
||||
|
||||
// Initialize servos
|
||||
Serial.println("[Main] Initializing servos...");
|
||||
if (!Servos.begin()) {
|
||||
Serial.println("[Main] WARNING: Servos init failed!");
|
||||
} else {
|
||||
Serial.println("[Main] Servos ready");
|
||||
// Center the camera
|
||||
Servos.center();
|
||||
}
|
||||
|
||||
// Initialize ultrasonic sensor
|
||||
Serial.println("[Main] Initializing ultrasonic sensor...");
|
||||
if (!Ultrasonic.begin()) {
|
||||
Serial.println("[Main] WARNING: Ultrasonic init failed!");
|
||||
} else {
|
||||
Serial.println("[Main] Ultrasonic ready");
|
||||
}
|
||||
|
||||
// Initialize IMU
|
||||
Serial.println("[Main] Initializing IMU...");
|
||||
if (!IMU.begin()) {
|
||||
Serial.println("[Main] WARNING: IMU init failed!");
|
||||
} else {
|
||||
Serial.println("[Main] IMU ready");
|
||||
}
|
||||
|
||||
// Connect to WiFi
|
||||
currentState = STATE_CONNECTING;
|
||||
Display.showMessage("Connecting WiFi...", TFT_YELLOW);
|
||||
setupWiFi();
|
||||
|
||||
// Start webserver
|
||||
Serial.println("[Main] Starting webserver...");
|
||||
if (!WebServer.begin()) {
|
||||
Serial.println("[Main] ERROR: WebServer failed!");
|
||||
currentState = STATE_ERROR;
|
||||
Display.showMessage("Server Error!", TFT_RED);
|
||||
return;
|
||||
}
|
||||
|
||||
// All systems go!
|
||||
currentState = STATE_READY;
|
||||
Display.setMode(MODE_STATUS);
|
||||
Display.showEmoji(EMOJI_HAPPY);
|
||||
delay(1000);
|
||||
Display.setMode(MODE_STATUS);
|
||||
|
||||
Serial.println();
|
||||
Serial.println("╔═══════════════════════════════════════╗");
|
||||
Serial.println("║ SYSTEM READY ║");
|
||||
Serial.printf( "║ IP: %-30s ║\n", WiFi.localIP().toString().c_str());
|
||||
Serial.println("║ Waiting for Claude... ║");
|
||||
Serial.println("╚═══════════════════════════════════════╝");
|
||||
Serial.println();
|
||||
|
||||
lastCommandTime = millis();
|
||||
lastStatusUpdate = millis();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
unsigned long now = millis();
|
||||
|
||||
// Update sensors
|
||||
Ultrasonic.update();
|
||||
IMU.update();
|
||||
Servos.update();
|
||||
Motors.update();
|
||||
|
||||
// Handle safety checks
|
||||
handleSafety();
|
||||
|
||||
// Handle touch input
|
||||
handleTouchInput();
|
||||
|
||||
// Update display periodically
|
||||
if (now - lastStatusUpdate >= DISPLAY_UPDATE_INTERVAL) {
|
||||
lastStatusUpdate = now;
|
||||
updateStatusDisplay();
|
||||
}
|
||||
|
||||
// Command timeout safety
|
||||
if (Motors.isMoving() && (now - lastCommandTime > COMMAND_TIMEOUT_MS)) {
|
||||
Serial.println("[Main] Command timeout - stopping");
|
||||
Motors.stop();
|
||||
}
|
||||
|
||||
// Small delay to prevent watchdog issues
|
||||
delay(1);
|
||||
}
|
||||
|
||||
void setupWiFi() {
|
||||
Serial.printf("[WiFi] Connecting to %s", WIFI_SSID);
|
||||
|
||||
WiFi.mode(WIFI_STA);
|
||||
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
|
||||
|
||||
int attempts = 0;
|
||||
while (WiFi.status() != WL_CONNECTED && attempts < 30) {
|
||||
delay(500);
|
||||
Serial.print(".");
|
||||
attempts++;
|
||||
|
||||
// Update display
|
||||
if (attempts % 2 == 0) {
|
||||
char msg[32];
|
||||
snprintf(msg, sizeof(msg), "WiFi... %d", attempts);
|
||||
Display.showMessage(msg, TFT_YELLOW);
|
||||
}
|
||||
}
|
||||
|
||||
if (WiFi.status() == WL_CONNECTED) {
|
||||
Serial.println(" Connected!");
|
||||
Serial.printf("[WiFi] IP Address: %s\n", WiFi.localIP().toString().c_str());
|
||||
Serial.printf("[WiFi] Signal strength: %d dBm\n", WiFi.RSSI());
|
||||
|
||||
char msg[32];
|
||||
snprintf(msg, sizeof(msg), "IP: %s", WiFi.localIP().toString().c_str());
|
||||
Display.showMessage(msg, TFT_GREEN);
|
||||
delay(2000);
|
||||
} else {
|
||||
Serial.println(" FAILED!");
|
||||
Serial.println("[WiFi] Could not connect to WiFi");
|
||||
|
||||
// Start AP mode as fallback
|
||||
Serial.println("[WiFi] Starting AP mode...");
|
||||
WiFi.mode(WIFI_AP);
|
||||
WiFi.softAP("ClaudesEyes", "claude2025");
|
||||
Serial.printf("[WiFi] AP IP: %s\n", WiFi.softAPIP().toString().c_str());
|
||||
|
||||
Display.showMessage("AP: ClaudesEyes", TFT_ORANGE);
|
||||
delay(2000);
|
||||
}
|
||||
}
|
||||
|
||||
void handleSafety() {
|
||||
// Check for obstacles
|
||||
if (Ultrasonic.isDanger() && Motors.isMoving()) {
|
||||
MotorDirection dir = Motors.getCurrentDirection();
|
||||
if (dir == DIR_FORWARD) {
|
||||
Serial.println("[Safety] Obstacle too close - stopping!");
|
||||
Motors.stop();
|
||||
Display.showEmoji(EMOJI_SURPRISED);
|
||||
|
||||
// Set Claude text about obstacle
|
||||
WebServer.setClaudeText("Hindernis erkannt! Ich halte an.");
|
||||
}
|
||||
}
|
||||
|
||||
// Check if robot is tilted
|
||||
if (IMU.isTilted() && Motors.isMoving()) {
|
||||
Serial.println("[Safety] Robot tilted - stopping!");
|
||||
Motors.stop();
|
||||
Display.showEmoji(EMOJI_CONFUSED);
|
||||
}
|
||||
|
||||
// Check if upside down
|
||||
if (IMU.isUpsideDown()) {
|
||||
Serial.println("[Safety] Robot upside down!");
|
||||
Motors.stop();
|
||||
Motors.setEnabled(false); // Disable motors completely
|
||||
Display.showMessage("HILFE!", TFT_RED);
|
||||
}
|
||||
}
|
||||
|
||||
void handleTouchInput() {
|
||||
TouchButton btn = Display.checkTouch();
|
||||
|
||||
if (btn == BTN_NONE) return;
|
||||
|
||||
lastCommandTime = millis();
|
||||
|
||||
switch (btn) {
|
||||
case BTN_STOP:
|
||||
Serial.println("[Touch] STOP pressed");
|
||||
Motors.stop();
|
||||
Display.showEmoji(EMOJI_SLEEPY);
|
||||
break;
|
||||
|
||||
case BTN_FORWARD:
|
||||
Serial.println("[Touch] Forward pressed");
|
||||
Motors.move(DIR_FORWARD, 50, 1000);
|
||||
break;
|
||||
|
||||
case BTN_BACKWARD:
|
||||
Serial.println("[Touch] Backward pressed");
|
||||
Motors.move(DIR_BACKWARD, 50, 1000);
|
||||
break;
|
||||
|
||||
case BTN_LEFT:
|
||||
Serial.println("[Touch] Left pressed");
|
||||
Motors.move(DIR_LEFT, 50, 500);
|
||||
break;
|
||||
|
||||
case BTN_RIGHT:
|
||||
Serial.println("[Touch] Right pressed");
|
||||
Motors.move(DIR_RIGHT, 50, 500);
|
||||
break;
|
||||
|
||||
case BTN_HOME:
|
||||
Serial.println("[Touch] Home pressed");
|
||||
// TODO: Implement return to base
|
||||
Display.showMessage("Coming home!", TFT_CYAN);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void updateStatusDisplay() {
|
||||
if (Display.isInitialized()) {
|
||||
Display.updateStatus(
|
||||
Motors.getDirectionString(),
|
||||
Ultrasonic.getLastDistance(),
|
||||
WebServer.getBatteryPercent(),
|
||||
WiFi.RSSI()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,211 @@
|
|||
/**
|
||||
* Claude's Eyes - Motor Control Implementation
|
||||
*
|
||||
* 4WD control using TB6612/L298N motor driver on Freenove shield
|
||||
*/
|
||||
|
||||
#include "motor_control.h"
|
||||
#include "config.h"
|
||||
|
||||
// Global instance
|
||||
MotorControl Motors;
|
||||
|
||||
MotorControl::MotorControl()
|
||||
: _initialized(false)
|
||||
, _enabled(true)
|
||||
, _isMoving(false)
|
||||
, _currentDirection(DIR_STOP)
|
||||
, _currentSpeed(0)
|
||||
, _moveStartTime(0)
|
||||
, _moveDuration(0)
|
||||
{
|
||||
}
|
||||
|
||||
bool MotorControl::begin() {
|
||||
#if !ENABLE_MOTORS
|
||||
Serial.println("[Motors] Disabled in config");
|
||||
return false;
|
||||
#endif
|
||||
|
||||
// Configure motor A pins
|
||||
pinMode(MOTOR_A_IN1, OUTPUT);
|
||||
pinMode(MOTOR_A_IN2, OUTPUT);
|
||||
|
||||
// Configure motor B pins
|
||||
pinMode(MOTOR_B_IN1, OUTPUT);
|
||||
pinMode(MOTOR_B_IN2, OUTPUT);
|
||||
|
||||
// Configure PWM for motor A
|
||||
ledcSetup(MOTOR_PWM_CHANNEL_A, MOTOR_PWM_FREQ, MOTOR_PWM_RESOLUTION);
|
||||
ledcAttachPin(MOTOR_A_PWM, MOTOR_PWM_CHANNEL_A);
|
||||
|
||||
// Configure PWM for motor B
|
||||
ledcSetup(MOTOR_PWM_CHANNEL_B, MOTOR_PWM_FREQ, MOTOR_PWM_RESOLUTION);
|
||||
ledcAttachPin(MOTOR_B_PWM, MOTOR_PWM_CHANNEL_B);
|
||||
|
||||
// Start with motors stopped
|
||||
stop();
|
||||
|
||||
_initialized = true;
|
||||
Serial.println("[Motors] Initialized successfully");
|
||||
return true;
|
||||
}
|
||||
|
||||
void MotorControl::move(MotorDirection direction, int speed, unsigned long duration_ms) {
|
||||
if (!_initialized || !_enabled) {
|
||||
Serial.println("[Motors] Not initialized or disabled");
|
||||
return;
|
||||
}
|
||||
|
||||
// Clamp speed to valid range
|
||||
speed = constrain(speed, 0, 100);
|
||||
|
||||
// Convert percent to actual speed value
|
||||
int actualSpeed = percentToSpeed(speed);
|
||||
|
||||
_currentDirection = direction;
|
||||
_currentSpeed = speed;
|
||||
_moveStartTime = millis();
|
||||
_moveDuration = duration_ms;
|
||||
_isMoving = (direction != DIR_STOP);
|
||||
|
||||
Serial.printf("[Motors] Move: %s, speed: %d%%, duration: %lums\n",
|
||||
getDirectionString(), speed, duration_ms);
|
||||
|
||||
switch (direction) {
|
||||
case DIR_FORWARD:
|
||||
setMotorA(actualSpeed);
|
||||
setMotorB(actualSpeed);
|
||||
break;
|
||||
|
||||
case DIR_BACKWARD:
|
||||
setMotorA(-actualSpeed);
|
||||
setMotorB(-actualSpeed);
|
||||
break;
|
||||
|
||||
case DIR_LEFT:
|
||||
// Pivot turn: left motors backward, right motors forward
|
||||
setMotorA(-actualSpeed);
|
||||
setMotorB(actualSpeed);
|
||||
break;
|
||||
|
||||
case DIR_RIGHT:
|
||||
// Pivot turn: left motors forward, right motors backward
|
||||
setMotorA(actualSpeed);
|
||||
setMotorB(-actualSpeed);
|
||||
break;
|
||||
|
||||
case DIR_SOFT_LEFT:
|
||||
// Soft turn: reduce left side speed
|
||||
setMotorA(actualSpeed / 3);
|
||||
setMotorB(actualSpeed);
|
||||
break;
|
||||
|
||||
case DIR_SOFT_RIGHT:
|
||||
// Soft turn: reduce right side speed
|
||||
setMotorA(actualSpeed);
|
||||
setMotorB(actualSpeed / 3);
|
||||
break;
|
||||
|
||||
case DIR_STOP:
|
||||
default:
|
||||
stop();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MotorControl::stop() {
|
||||
// Set direction pins to brake mode
|
||||
digitalWrite(MOTOR_A_IN1, LOW);
|
||||
digitalWrite(MOTOR_A_IN2, LOW);
|
||||
digitalWrite(MOTOR_B_IN1, LOW);
|
||||
digitalWrite(MOTOR_B_IN2, LOW);
|
||||
|
||||
// Set PWM to 0
|
||||
ledcWrite(MOTOR_PWM_CHANNEL_A, 0);
|
||||
ledcWrite(MOTOR_PWM_CHANNEL_B, 0);
|
||||
|
||||
_isMoving = false;
|
||||
_currentDirection = DIR_STOP;
|
||||
_currentSpeed = 0;
|
||||
_moveDuration = 0;
|
||||
|
||||
Serial.println("[Motors] Stopped");
|
||||
}
|
||||
|
||||
void MotorControl::update() {
|
||||
if (!_isMoving || _moveDuration == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Check if movement duration has elapsed
|
||||
if (millis() - _moveStartTime >= _moveDuration) {
|
||||
Serial.println("[Motors] Movement duration elapsed");
|
||||
stop();
|
||||
}
|
||||
}
|
||||
|
||||
void MotorControl::setMotorA(int speed) {
|
||||
// speed: -255 to 255
|
||||
// Negative = backward, Positive = forward
|
||||
|
||||
if (speed > 0) {
|
||||
// Forward
|
||||
digitalWrite(MOTOR_A_IN1, HIGH);
|
||||
digitalWrite(MOTOR_A_IN2, LOW);
|
||||
ledcWrite(MOTOR_PWM_CHANNEL_A, speed);
|
||||
} else if (speed < 0) {
|
||||
// Backward
|
||||
digitalWrite(MOTOR_A_IN1, LOW);
|
||||
digitalWrite(MOTOR_A_IN2, HIGH);
|
||||
ledcWrite(MOTOR_PWM_CHANNEL_A, -speed);
|
||||
} else {
|
||||
// Stop
|
||||
digitalWrite(MOTOR_A_IN1, LOW);
|
||||
digitalWrite(MOTOR_A_IN2, LOW);
|
||||
ledcWrite(MOTOR_PWM_CHANNEL_A, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorControl::setMotorB(int speed) {
|
||||
// speed: -255 to 255
|
||||
// Negative = backward, Positive = forward
|
||||
|
||||
if (speed > 0) {
|
||||
// Forward
|
||||
digitalWrite(MOTOR_B_IN1, HIGH);
|
||||
digitalWrite(MOTOR_B_IN2, LOW);
|
||||
ledcWrite(MOTOR_PWM_CHANNEL_B, speed);
|
||||
} else if (speed < 0) {
|
||||
// Backward
|
||||
digitalWrite(MOTOR_B_IN1, LOW);
|
||||
digitalWrite(MOTOR_B_IN2, HIGH);
|
||||
ledcWrite(MOTOR_PWM_CHANNEL_B, -speed);
|
||||
} else {
|
||||
// Stop
|
||||
digitalWrite(MOTOR_B_IN1, LOW);
|
||||
digitalWrite(MOTOR_B_IN2, LOW);
|
||||
ledcWrite(MOTOR_PWM_CHANNEL_B, 0);
|
||||
}
|
||||
}
|
||||
|
||||
int MotorControl::percentToSpeed(int percent) {
|
||||
if (percent <= 0) return 0;
|
||||
if (percent >= 100) return MOTOR_MAX_SPEED;
|
||||
|
||||
// Map percent to speed range, accounting for minimum speed
|
||||
return map(percent, 0, 100, MOTOR_MIN_SPEED, MOTOR_MAX_SPEED);
|
||||
}
|
||||
|
||||
const char* MotorControl::getDirectionString() {
|
||||
switch (_currentDirection) {
|
||||
case DIR_STOP: return "stop";
|
||||
case DIR_FORWARD: return "forward";
|
||||
case DIR_BACKWARD: return "backward";
|
||||
case DIR_LEFT: return "left";
|
||||
case DIR_RIGHT: return "right";
|
||||
case DIR_SOFT_LEFT: return "soft_left";
|
||||
case DIR_SOFT_RIGHT: return "soft_right";
|
||||
default: return "unknown";
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
/**
|
||||
* Claude's Eyes - Motor Control Header
|
||||
*
|
||||
* Controls the 4WD drive system (2 motors per side)
|
||||
*/
|
||||
|
||||
#ifndef MOTOR_CONTROL_H
|
||||
#define MOTOR_CONTROL_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// Movement directions
|
||||
enum MotorDirection {
|
||||
DIR_STOP,
|
||||
DIR_FORWARD,
|
||||
DIR_BACKWARD,
|
||||
DIR_LEFT, // Turn left (pivot)
|
||||
DIR_RIGHT, // Turn right (pivot)
|
||||
DIR_SOFT_LEFT, // Soft turn left (one side slower)
|
||||
DIR_SOFT_RIGHT // Soft turn right (one side slower)
|
||||
};
|
||||
|
||||
class MotorControl {
|
||||
public:
|
||||
MotorControl();
|
||||
|
||||
/**
|
||||
* Initialize motor pins and PWM
|
||||
* @return true if successful
|
||||
*/
|
||||
bool begin();
|
||||
|
||||
/**
|
||||
* Move in a direction for a specified duration
|
||||
* @param direction Movement direction
|
||||
* @param speed Speed (0-100 percent)
|
||||
* @param duration_ms Duration in milliseconds (0 = continuous)
|
||||
*/
|
||||
void move(MotorDirection direction, int speed = 50, unsigned long duration_ms = 0);
|
||||
|
||||
/**
|
||||
* Stop all motors immediately
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Check if motors are currently active
|
||||
*/
|
||||
bool isMoving() { return _isMoving; }
|
||||
|
||||
/**
|
||||
* Get current direction
|
||||
*/
|
||||
MotorDirection getCurrentDirection() { return _currentDirection; }
|
||||
|
||||
/**
|
||||
* Get current speed (0-100)
|
||||
*/
|
||||
int getCurrentSpeed() { return _currentSpeed; }
|
||||
|
||||
/**
|
||||
* Get direction as string
|
||||
*/
|
||||
const char* getDirectionString();
|
||||
|
||||
/**
|
||||
* Update motor state (call from loop for timed movements)
|
||||
*/
|
||||
void update();
|
||||
|
||||
/**
|
||||
* Enable/disable motors (safety feature)
|
||||
*/
|
||||
void setEnabled(bool enabled) { _enabled = enabled; if (!enabled) stop(); }
|
||||
bool isEnabled() { return _enabled; }
|
||||
|
||||
private:
|
||||
bool _initialized;
|
||||
bool _enabled;
|
||||
bool _isMoving;
|
||||
MotorDirection _currentDirection;
|
||||
int _currentSpeed;
|
||||
unsigned long _moveStartTime;
|
||||
unsigned long _moveDuration;
|
||||
|
||||
// Internal motor control
|
||||
void setMotorA(int speed); // -255 to 255
|
||||
void setMotorB(int speed); // -255 to 255
|
||||
int percentToSpeed(int percent);
|
||||
};
|
||||
|
||||
// Global instance
|
||||
extern MotorControl Motors;
|
||||
|
||||
#endif // MOTOR_CONTROL_H
|
||||
|
|
@ -0,0 +1,187 @@
|
|||
/**
|
||||
* Claude's Eyes - Servo Control Implementation
|
||||
*
|
||||
* Pan/Tilt camera mount using standard hobby servos
|
||||
*/
|
||||
|
||||
#include "servo_control.h"
|
||||
#include "config.h"
|
||||
|
||||
// Global instance
|
||||
ServoControl Servos;
|
||||
|
||||
ServoControl::ServoControl()
|
||||
: _initialized(false)
|
||||
, _attached(false)
|
||||
, _isMoving(false)
|
||||
, _currentPan(SERVO_PAN_CENTER)
|
||||
, _currentTilt(SERVO_TILT_CENTER)
|
||||
, _targetPan(SERVO_PAN_CENTER)
|
||||
, _targetTilt(SERVO_TILT_CENTER)
|
||||
, _lastStepTime(0)
|
||||
{
|
||||
}
|
||||
|
||||
bool ServoControl::begin() {
|
||||
#if !ENABLE_SERVOS
|
||||
Serial.println("[Servos] Disabled in config");
|
||||
return false;
|
||||
#endif
|
||||
|
||||
// Allow allocation of all timers for servo library
|
||||
ESP32PWM::allocateTimer(0);
|
||||
ESP32PWM::allocateTimer(1);
|
||||
ESP32PWM::allocateTimer(2);
|
||||
ESP32PWM::allocateTimer(3);
|
||||
|
||||
// Attach servos
|
||||
_panServo.setPeriodHertz(50); // Standard 50Hz servo
|
||||
_tiltServo.setPeriodHertz(50);
|
||||
|
||||
attach();
|
||||
|
||||
// Move to center position
|
||||
center();
|
||||
|
||||
_initialized = true;
|
||||
Serial.println("[Servos] Initialized successfully");
|
||||
return true;
|
||||
}
|
||||
|
||||
void ServoControl::attach() {
|
||||
if (_attached) return;
|
||||
|
||||
_panServo.attach(SERVO_PAN_PIN, 500, 2400); // Min/max pulse width
|
||||
_tiltServo.attach(SERVO_TILT_PIN, 500, 2400);
|
||||
|
||||
_attached = true;
|
||||
Serial.println("[Servos] Attached");
|
||||
}
|
||||
|
||||
void ServoControl::detach() {
|
||||
if (!_attached) return;
|
||||
|
||||
_panServo.detach();
|
||||
_tiltServo.detach();
|
||||
|
||||
_attached = false;
|
||||
Serial.println("[Servos] Detached");
|
||||
}
|
||||
|
||||
void ServoControl::look(LookDirection direction, bool smooth) {
|
||||
int targetPan = _currentPan;
|
||||
int targetTilt = _currentTilt;
|
||||
|
||||
switch (direction) {
|
||||
case LOOK_CENTER:
|
||||
targetPan = SERVO_PAN_CENTER;
|
||||
targetTilt = SERVO_TILT_CENTER;
|
||||
break;
|
||||
|
||||
case LOOK_LEFT:
|
||||
targetPan = SERVO_PAN_MAX; // Look left (servo rotates right)
|
||||
break;
|
||||
|
||||
case LOOK_RIGHT:
|
||||
targetPan = SERVO_PAN_MIN; // Look right (servo rotates left)
|
||||
break;
|
||||
|
||||
case LOOK_UP:
|
||||
targetTilt = SERVO_TILT_MAX;
|
||||
break;
|
||||
|
||||
case LOOK_DOWN:
|
||||
targetTilt = SERVO_TILT_MIN;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
setPosition(targetPan, targetTilt, smooth);
|
||||
}
|
||||
|
||||
void ServoControl::setPosition(int pan, int tilt, bool smooth) {
|
||||
if (!_initialized) {
|
||||
Serial.println("[Servos] Not initialized");
|
||||
return;
|
||||
}
|
||||
|
||||
// Ensure servos are attached
|
||||
if (!_attached) {
|
||||
attach();
|
||||
}
|
||||
|
||||
// Clamp to valid ranges
|
||||
_targetPan = clampPan(pan);
|
||||
_targetTilt = clampTilt(tilt);
|
||||
|
||||
Serial.printf("[Servos] Target position: pan=%d, tilt=%d\n", _targetPan, _targetTilt);
|
||||
|
||||
if (smooth) {
|
||||
// Smooth movement will be handled in update()
|
||||
_isMoving = true;
|
||||
_lastStepTime = millis();
|
||||
} else {
|
||||
// Immediate movement
|
||||
_currentPan = _targetPan;
|
||||
_currentTilt = _targetTilt;
|
||||
_panServo.write(_currentPan);
|
||||
_tiltServo.write(_currentTilt);
|
||||
_isMoving = false;
|
||||
}
|
||||
}
|
||||
|
||||
void ServoControl::center() {
|
||||
setPosition(SERVO_PAN_CENTER, SERVO_TILT_CENTER, false);
|
||||
}
|
||||
|
||||
void ServoControl::update() {
|
||||
if (!_isMoving) return;
|
||||
|
||||
unsigned long now = millis();
|
||||
if (now - _lastStepTime < SERVO_STEP_DELAY_MS) {
|
||||
return;
|
||||
}
|
||||
_lastStepTime = now;
|
||||
|
||||
bool panDone = false;
|
||||
bool tiltDone = false;
|
||||
|
||||
// Move pan servo
|
||||
if (_currentPan < _targetPan) {
|
||||
_currentPan = min(_currentPan + SERVO_STEP_SIZE, _targetPan);
|
||||
_panServo.write(_currentPan);
|
||||
} else if (_currentPan > _targetPan) {
|
||||
_currentPan = max(_currentPan - SERVO_STEP_SIZE, _targetPan);
|
||||
_panServo.write(_currentPan);
|
||||
} else {
|
||||
panDone = true;
|
||||
}
|
||||
|
||||
// Move tilt servo
|
||||
if (_currentTilt < _targetTilt) {
|
||||
_currentTilt = min(_currentTilt + SERVO_STEP_SIZE, _targetTilt);
|
||||
_tiltServo.write(_currentTilt);
|
||||
} else if (_currentTilt > _targetTilt) {
|
||||
_currentTilt = max(_currentTilt - SERVO_STEP_SIZE, _targetTilt);
|
||||
_tiltServo.write(_currentTilt);
|
||||
} else {
|
||||
tiltDone = true;
|
||||
}
|
||||
|
||||
// Check if movement complete
|
||||
if (panDone && tiltDone) {
|
||||
_isMoving = false;
|
||||
Serial.printf("[Servos] Movement complete: pan=%d, tilt=%d\n",
|
||||
_currentPan, _currentTilt);
|
||||
}
|
||||
}
|
||||
|
||||
int ServoControl::clampPan(int angle) {
|
||||
return constrain(angle, SERVO_PAN_MIN, SERVO_PAN_MAX);
|
||||
}
|
||||
|
||||
int ServoControl::clampTilt(int angle) {
|
||||
return constrain(angle, SERVO_TILT_MIN, SERVO_TILT_MAX);
|
||||
}
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
/**
|
||||
* Claude's Eyes - Servo Control Header
|
||||
*
|
||||
* Controls pan/tilt servos for camera positioning
|
||||
*/
|
||||
|
||||
#ifndef SERVO_CONTROL_H
|
||||
#define SERVO_CONTROL_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <ESP32Servo.h>
|
||||
|
||||
// Look commands
|
||||
enum LookDirection {
|
||||
LOOK_CENTER,
|
||||
LOOK_LEFT,
|
||||
LOOK_RIGHT,
|
||||
LOOK_UP,
|
||||
LOOK_DOWN,
|
||||
LOOK_CUSTOM // Use with setPosition()
|
||||
};
|
||||
|
||||
class ServoControl {
|
||||
public:
|
||||
ServoControl();
|
||||
|
||||
/**
|
||||
* Initialize servos
|
||||
* @return true if successful
|
||||
*/
|
||||
bool begin();
|
||||
|
||||
/**
|
||||
* Move camera to look in a direction
|
||||
* @param direction Direction to look
|
||||
* @param smooth Use smooth movement (default true)
|
||||
*/
|
||||
void look(LookDirection direction, bool smooth = true);
|
||||
|
||||
/**
|
||||
* Set specific pan/tilt position
|
||||
* @param pan Pan angle (0-180, 90 = center)
|
||||
* @param tilt Tilt angle (0-180, 90 = center)
|
||||
* @param smooth Use smooth movement
|
||||
*/
|
||||
void setPosition(int pan, int tilt, bool smooth = true);
|
||||
|
||||
/**
|
||||
* Get current pan angle
|
||||
*/
|
||||
int getPan() { return _currentPan; }
|
||||
|
||||
/**
|
||||
* Get current tilt angle
|
||||
*/
|
||||
int getTilt() { return _currentTilt; }
|
||||
|
||||
/**
|
||||
* Center both servos
|
||||
*/
|
||||
void center();
|
||||
|
||||
/**
|
||||
* Update servos (call from loop for smooth movement)
|
||||
*/
|
||||
void update();
|
||||
|
||||
/**
|
||||
* Check if servos are currently moving
|
||||
*/
|
||||
bool isMoving() { return _isMoving; }
|
||||
|
||||
/**
|
||||
* Detach servos to save power/prevent jitter
|
||||
*/
|
||||
void detach();
|
||||
|
||||
/**
|
||||
* Reattach servos
|
||||
*/
|
||||
void attach();
|
||||
|
||||
private:
|
||||
Servo _panServo;
|
||||
Servo _tiltServo;
|
||||
|
||||
bool _initialized;
|
||||
bool _attached;
|
||||
bool _isMoving;
|
||||
|
||||
int _currentPan;
|
||||
int _currentTilt;
|
||||
int _targetPan;
|
||||
int _targetTilt;
|
||||
|
||||
unsigned long _lastStepTime;
|
||||
|
||||
void moveToTarget();
|
||||
int clampPan(int angle);
|
||||
int clampTilt(int angle);
|
||||
};
|
||||
|
||||
// Global instance
|
||||
extern ServoControl Servos;
|
||||
|
||||
#endif // SERVO_CONTROL_H
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
/**
|
||||
* Claude's Eyes - Ultrasonic Sensor Implementation
|
||||
*
|
||||
* HC-SR04 ultrasonic distance sensor
|
||||
*/
|
||||
|
||||
#include "ultrasonic.h"
|
||||
#include "config.h"
|
||||
|
||||
// Global instance
|
||||
UltrasonicSensor Ultrasonic;
|
||||
|
||||
UltrasonicSensor::UltrasonicSensor()
|
||||
: _initialized(false)
|
||||
, _lastDistance(0)
|
||||
, _lastMeasurementTime(0)
|
||||
{
|
||||
}
|
||||
|
||||
bool UltrasonicSensor::begin() {
|
||||
#if !ENABLE_ULTRASONIC
|
||||
Serial.println("[Ultrasonic] Disabled in config");
|
||||
return false;
|
||||
#endif
|
||||
|
||||
pinMode(ULTRASONIC_TRIGGER_PIN, OUTPUT);
|
||||
pinMode(ULTRASONIC_ECHO_PIN, INPUT);
|
||||
|
||||
// Initial trigger to low
|
||||
digitalWrite(ULTRASONIC_TRIGGER_PIN, LOW);
|
||||
|
||||
// Take initial measurement
|
||||
_lastDistance = getDistance();
|
||||
|
||||
_initialized = true;
|
||||
Serial.println("[Ultrasonic] Initialized successfully");
|
||||
return true;
|
||||
}
|
||||
|
||||
float UltrasonicSensor::getDistance() {
|
||||
if (!_initialized && ENABLE_ULTRASONIC) {
|
||||
// Not initialized yet, try anyway
|
||||
}
|
||||
|
||||
// Send trigger pulse
|
||||
digitalWrite(ULTRASONIC_TRIGGER_PIN, LOW);
|
||||
delayMicroseconds(2);
|
||||
digitalWrite(ULTRASONIC_TRIGGER_PIN, HIGH);
|
||||
delayMicroseconds(10);
|
||||
digitalWrite(ULTRASONIC_TRIGGER_PIN, LOW);
|
||||
|
||||
// Measure echo pulse duration
|
||||
// Timeout after ~25ms (corresponds to ~4m distance)
|
||||
unsigned long duration = pulseIn(ULTRASONIC_ECHO_PIN, HIGH, 25000);
|
||||
|
||||
if (duration == 0) {
|
||||
// No echo received (object too far or sensor error)
|
||||
return ULTRASONIC_MAX_DISTANCE;
|
||||
}
|
||||
|
||||
// Calculate distance: sound travels at ~343 m/s = 0.0343 cm/µs
|
||||
// Distance = (duration * 0.0343) / 2 (divide by 2 for round trip)
|
||||
float distance = (duration * 0.0343f) / 2.0f;
|
||||
|
||||
// Clamp to valid range
|
||||
if (distance > ULTRASONIC_MAX_DISTANCE) {
|
||||
distance = ULTRASONIC_MAX_DISTANCE;
|
||||
}
|
||||
|
||||
_lastDistance = distance;
|
||||
return distance;
|
||||
}
|
||||
|
||||
bool UltrasonicSensor::isDanger() {
|
||||
return _lastDistance > 0 && _lastDistance < OBSTACLE_DANGER_DISTANCE;
|
||||
}
|
||||
|
||||
bool UltrasonicSensor::isWarning() {
|
||||
return _lastDistance > 0 && _lastDistance < OBSTACLE_WARNING_DISTANCE;
|
||||
}
|
||||
|
||||
void UltrasonicSensor::update() {
|
||||
if (!_initialized) return;
|
||||
|
||||
unsigned long now = millis();
|
||||
if (now - _lastMeasurementTime >= MEASUREMENT_INTERVAL) {
|
||||
_lastMeasurementTime = now;
|
||||
getDistance();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
/**
|
||||
* Claude's Eyes - Ultrasonic Sensor Header
|
||||
*
|
||||
* HC-SR04 ultrasonic distance sensor for obstacle detection
|
||||
*/
|
||||
|
||||
#ifndef ULTRASONIC_H
|
||||
#define ULTRASONIC_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
class UltrasonicSensor {
|
||||
public:
|
||||
UltrasonicSensor();
|
||||
|
||||
/**
|
||||
* Initialize the sensor
|
||||
* @return true if successful
|
||||
*/
|
||||
bool begin();
|
||||
|
||||
/**
|
||||
* Get distance measurement
|
||||
* @return Distance in centimeters (0 if no echo)
|
||||
*/
|
||||
float getDistance();
|
||||
|
||||
/**
|
||||
* Check if there's an obstacle within danger distance
|
||||
*/
|
||||
bool isDanger();
|
||||
|
||||
/**
|
||||
* Check if there's an obstacle within warning distance
|
||||
*/
|
||||
bool isWarning();
|
||||
|
||||
/**
|
||||
* Get last measurement (without triggering new one)
|
||||
*/
|
||||
float getLastDistance() { return _lastDistance; }
|
||||
|
||||
/**
|
||||
* Update sensor (call periodically)
|
||||
*/
|
||||
void update();
|
||||
|
||||
/**
|
||||
* Check if sensor is initialized
|
||||
*/
|
||||
bool isInitialized() { return _initialized; }
|
||||
|
||||
private:
|
||||
bool _initialized;
|
||||
float _lastDistance;
|
||||
unsigned long _lastMeasurementTime;
|
||||
|
||||
static const unsigned long MEASUREMENT_INTERVAL = 50; // 50ms between measurements
|
||||
};
|
||||
|
||||
// Global instance
|
||||
extern UltrasonicSensor Ultrasonic;
|
||||
|
||||
#endif // ULTRASONIC_H
|
||||
|
|
@ -0,0 +1,417 @@
|
|||
/**
|
||||
* Claude's Eyes - Webserver Implementation
|
||||
*
|
||||
* REST API for controlling the robot
|
||||
*/
|
||||
|
||||
#include "webserver.h"
|
||||
#include "config.h"
|
||||
#include "camera.h"
|
||||
#include "motor_control.h"
|
||||
#include "servo_control.h"
|
||||
#include "ultrasonic.h"
|
||||
#include "imu.h"
|
||||
#include "display.h"
|
||||
#include <WiFi.h>
|
||||
|
||||
// Global instance
|
||||
WebServerModule WebServer;
|
||||
|
||||
WebServerModule::WebServerModule()
|
||||
: _server(WEBSERVER_PORT)
|
||||
, _claudeTextTimestamp(0)
|
||||
, _claudeTextNew(false)
|
||||
, _batteryPercent(100) // TODO: Implement battery reading
|
||||
{
|
||||
}
|
||||
|
||||
bool WebServerModule::begin() {
|
||||
setupRoutes();
|
||||
_server.begin();
|
||||
Serial.printf("[WebServer] Started on port %d\n", WEBSERVER_PORT);
|
||||
return true;
|
||||
}
|
||||
|
||||
void WebServerModule::setupRoutes() {
|
||||
// CORS preflight
|
||||
_server.on("/*", HTTP_OPTIONS, [](AsyncWebServerRequest* request) {
|
||||
AsyncWebServerResponse* response = request->beginResponse(200);
|
||||
response->addHeader("Access-Control-Allow-Origin", "*");
|
||||
response->addHeader("Access-Control-Allow-Methods", "GET, POST, OPTIONS");
|
||||
response->addHeader("Access-Control-Allow-Headers", "Content-Type");
|
||||
request->send(response);
|
||||
});
|
||||
|
||||
// GET /api/capture - Camera image
|
||||
_server.on("/api/capture", HTTP_GET, [this](AsyncWebServerRequest* request) {
|
||||
handleCapture(request);
|
||||
});
|
||||
|
||||
// GET /api/status - Sensor data
|
||||
_server.on("/api/status", HTTP_GET, [this](AsyncWebServerRequest* request) {
|
||||
handleStatus(request);
|
||||
});
|
||||
|
||||
// POST /api/command - Movement commands
|
||||
_server.on("/api/command", HTTP_POST,
|
||||
[](AsyncWebServerRequest* request) {},
|
||||
NULL,
|
||||
[this](AsyncWebServerRequest* request, uint8_t* data, size_t len, size_t index, size_t total) {
|
||||
handleCommand(request, data, len);
|
||||
}
|
||||
);
|
||||
|
||||
// GET /api/claude_text - Get Claude's message
|
||||
_server.on("/api/claude_text", HTTP_GET, [this](AsyncWebServerRequest* request) {
|
||||
handleGetClaudeText(request);
|
||||
});
|
||||
|
||||
// POST /api/claude_text - Set Claude's message
|
||||
_server.on("/api/claude_text", HTTP_POST,
|
||||
[](AsyncWebServerRequest* request) {},
|
||||
NULL,
|
||||
[this](AsyncWebServerRequest* request, uint8_t* data, size_t len, size_t index, size_t total) {
|
||||
handlePostClaudeText(request, data, len);
|
||||
}
|
||||
);
|
||||
|
||||
// POST /api/display - Control display
|
||||
_server.on("/api/display", HTTP_POST,
|
||||
[](AsyncWebServerRequest* request) {},
|
||||
NULL,
|
||||
[this](AsyncWebServerRequest* request, uint8_t* data, size_t len, size_t index, size_t total) {
|
||||
handleDisplay(request, data, len);
|
||||
}
|
||||
);
|
||||
|
||||
// Root - Simple status page
|
||||
_server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
|
||||
String html = "<!DOCTYPE html><html><head><title>Claude's Eyes</title>";
|
||||
html += "<meta name='viewport' content='width=device-width, initial-scale=1'>";
|
||||
html += "<style>body{font-family:sans-serif;background:#1a1a2e;color:#eee;padding:20px;}";
|
||||
html += "h1{color:#0ff;}a{color:#0ff;}</style></head><body>";
|
||||
html += "<h1>Claude's Eyes</h1>";
|
||||
html += "<p>Robot is online!</p>";
|
||||
html += "<p>API Endpoints:</p><ul>";
|
||||
html += "<li>GET /api/capture - Camera image</li>";
|
||||
html += "<li>GET /api/status - Sensor data</li>";
|
||||
html += "<li>POST /api/command - Movement</li>";
|
||||
html += "<li>GET/POST /api/claude_text - Claude messages</li>";
|
||||
html += "</ul>";
|
||||
html += "<p><a href='/api/capture?key=" + String(API_KEY) + "'>View Camera</a></p>";
|
||||
html += "</body></html>";
|
||||
request->send(200, "text/html", html);
|
||||
});
|
||||
|
||||
// 404 handler
|
||||
_server.onNotFound([](AsyncWebServerRequest* request) {
|
||||
request->send(404, "application/json", "{\"error\":\"Not found\"}");
|
||||
});
|
||||
}
|
||||
|
||||
bool WebServerModule::checkAuth(AsyncWebServerRequest* request) {
|
||||
if (!request->hasParam("key")) {
|
||||
sendError(request, 401, "Missing API key");
|
||||
return false;
|
||||
}
|
||||
|
||||
String key = request->getParam("key")->value();
|
||||
if (key != API_KEY) {
|
||||
sendError(request, 403, "Invalid API key");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void WebServerModule::sendError(AsyncWebServerRequest* request, int code, const char* message) {
|
||||
JsonDocument doc;
|
||||
doc["error"] = message;
|
||||
doc["code"] = code;
|
||||
|
||||
String response;
|
||||
serializeJson(doc, response);
|
||||
|
||||
AsyncWebServerResponse* resp = request->beginResponse(code, "application/json", response);
|
||||
resp->addHeader("Access-Control-Allow-Origin", "*");
|
||||
request->send(resp);
|
||||
}
|
||||
|
||||
void WebServerModule::sendJson(AsyncWebServerRequest* request, JsonDocument& doc) {
|
||||
String response;
|
||||
serializeJson(doc, response);
|
||||
|
||||
AsyncWebServerResponse* resp = request->beginResponse(200, "application/json", response);
|
||||
resp->addHeader("Access-Control-Allow-Origin", "*");
|
||||
request->send(resp);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// API Handlers
|
||||
// ============================================================================
|
||||
|
||||
void WebServerModule::handleCapture(AsyncWebServerRequest* request) {
|
||||
if (!checkAuth(request)) return;
|
||||
|
||||
// Check resolution parameter
|
||||
if (request->hasParam("resolution")) {
|
||||
String res = request->getParam("resolution")->value();
|
||||
CameraResolution camRes = RES_VGA;
|
||||
|
||||
if (res == "QVGA") camRes = RES_QVGA;
|
||||
else if (res == "VGA") camRes = RES_VGA;
|
||||
else if (res == "SVGA") camRes = RES_SVGA;
|
||||
else if (res == "XGA") camRes = RES_XGA;
|
||||
else if (res == "SXGA") camRes = RES_SXGA;
|
||||
else if (res == "UXGA") camRes = RES_UXGA;
|
||||
|
||||
Camera.setResolution(camRes);
|
||||
}
|
||||
|
||||
// Check quality parameter
|
||||
int quality = -1;
|
||||
if (request->hasParam("quality")) {
|
||||
quality = request->getParam("quality")->value().toInt();
|
||||
}
|
||||
|
||||
// Capture frame
|
||||
camera_fb_t* fb = Camera.capture(quality);
|
||||
if (!fb) {
|
||||
sendError(request, 500, Camera.getLastError());
|
||||
return;
|
||||
}
|
||||
|
||||
// Send image
|
||||
AsyncWebServerResponse* response = request->beginResponse_P(
|
||||
200, "image/jpeg", fb->buf, fb->len
|
||||
);
|
||||
response->addHeader("Access-Control-Allow-Origin", "*");
|
||||
response->addHeader("Cache-Control", "no-cache");
|
||||
request->send(response);
|
||||
|
||||
// Return frame buffer
|
||||
Camera.returnFrame(fb);
|
||||
}
|
||||
|
||||
void WebServerModule::handleStatus(AsyncWebServerRequest* request) {
|
||||
if (!checkAuth(request)) return;
|
||||
|
||||
JsonDocument doc;
|
||||
|
||||
// Distance
|
||||
doc["distance_cm"] = Ultrasonic.getLastDistance();
|
||||
|
||||
// Battery (placeholder)
|
||||
doc["battery_percent"] = _batteryPercent;
|
||||
|
||||
// Current action
|
||||
doc["current_action"] = Motors.getDirectionString();
|
||||
|
||||
// IMU data
|
||||
const IMUData& imu = IMU.getData();
|
||||
doc["imu"]["accel_x"] = imu.accel_x;
|
||||
doc["imu"]["accel_y"] = imu.accel_y;
|
||||
doc["imu"]["accel_z"] = imu.accel_z;
|
||||
doc["imu"]["gyro_x"] = imu.gyro_x;
|
||||
doc["imu"]["gyro_y"] = imu.gyro_y;
|
||||
doc["imu"]["gyro_z"] = imu.gyro_z;
|
||||
doc["imu"]["pitch"] = imu.pitch;
|
||||
doc["imu"]["roll"] = imu.roll;
|
||||
doc["imu"]["yaw"] = imu.yaw;
|
||||
doc["imu"]["temperature"] = imu.temperature;
|
||||
|
||||
// WiFi
|
||||
doc["wifi_rssi"] = WiFi.RSSI();
|
||||
|
||||
// Uptime
|
||||
doc["uptime_seconds"] = getUptime();
|
||||
|
||||
// Servos
|
||||
doc["servo_pan"] = Servos.getPan();
|
||||
doc["servo_tilt"] = Servos.getTilt();
|
||||
|
||||
// Safety flags
|
||||
doc["obstacle_warning"] = Ultrasonic.isWarning();
|
||||
doc["obstacle_danger"] = Ultrasonic.isDanger();
|
||||
doc["is_tilted"] = IMU.isTilted();
|
||||
doc["is_moving"] = Motors.isMoving();
|
||||
|
||||
sendJson(request, doc);
|
||||
}
|
||||
|
||||
void WebServerModule::handleCommand(AsyncWebServerRequest* request, uint8_t* data, size_t len) {
|
||||
if (!checkAuth(request)) return;
|
||||
|
||||
// Parse JSON
|
||||
JsonDocument doc;
|
||||
DeserializationError error = deserializeJson(doc, data, len);
|
||||
|
||||
if (error) {
|
||||
sendError(request, 400, "Invalid JSON");
|
||||
return;
|
||||
}
|
||||
|
||||
String action = doc["action"] | "";
|
||||
int speed = doc["speed"] | 50;
|
||||
int duration = doc["duration_ms"] | 500;
|
||||
|
||||
JsonDocument response;
|
||||
response["status"] = "ok";
|
||||
response["executed"] = action;
|
||||
|
||||
// Handle movement commands
|
||||
if (action == "forward") {
|
||||
Motors.move(DIR_FORWARD, speed, duration);
|
||||
response["message"] = "Moving forward";
|
||||
}
|
||||
else if (action == "backward") {
|
||||
Motors.move(DIR_BACKWARD, speed, duration);
|
||||
response["message"] = "Moving backward";
|
||||
}
|
||||
else if (action == "left") {
|
||||
Motors.move(DIR_LEFT, speed, duration);
|
||||
response["message"] = "Turning left";
|
||||
}
|
||||
else if (action == "right") {
|
||||
Motors.move(DIR_RIGHT, speed, duration);
|
||||
response["message"] = "Turning right";
|
||||
}
|
||||
else if (action == "stop") {
|
||||
Motors.stop();
|
||||
response["message"] = "Stopped";
|
||||
}
|
||||
// Servo commands
|
||||
else if (action == "look_left") {
|
||||
Servos.look(LOOK_LEFT);
|
||||
response["message"] = "Looking left";
|
||||
}
|
||||
else if (action == "look_right") {
|
||||
Servos.look(LOOK_RIGHT);
|
||||
response["message"] = "Looking right";
|
||||
}
|
||||
else if (action == "look_up") {
|
||||
Servos.look(LOOK_UP);
|
||||
response["message"] = "Looking up";
|
||||
}
|
||||
else if (action == "look_down") {
|
||||
Servos.look(LOOK_DOWN);
|
||||
response["message"] = "Looking down";
|
||||
}
|
||||
else if (action == "look_center") {
|
||||
Servos.look(LOOK_CENTER);
|
||||
response["message"] = "Looking center";
|
||||
}
|
||||
else if (action == "look_custom") {
|
||||
int pan = doc["pan"] | 90;
|
||||
int tilt = doc["tilt"] | 90;
|
||||
Servos.setPosition(pan, tilt);
|
||||
response["message"] = "Custom look position";
|
||||
}
|
||||
else {
|
||||
response["status"] = "error";
|
||||
response["message"] = "Unknown action";
|
||||
}
|
||||
|
||||
sendJson(request, response);
|
||||
}
|
||||
|
||||
void WebServerModule::handleGetClaudeText(AsyncWebServerRequest* request) {
|
||||
if (!checkAuth(request)) return;
|
||||
|
||||
JsonDocument doc;
|
||||
doc["text"] = _claudeText;
|
||||
doc["timestamp"] = _claudeTextTimestamp;
|
||||
doc["new"] = _claudeTextNew;
|
||||
|
||||
// Mark as read
|
||||
_claudeTextNew = false;
|
||||
|
||||
sendJson(request, doc);
|
||||
}
|
||||
|
||||
void WebServerModule::handlePostClaudeText(AsyncWebServerRequest* request, uint8_t* data, size_t len) {
|
||||
if (!checkAuth(request)) return;
|
||||
|
||||
JsonDocument doc;
|
||||
DeserializationError error = deserializeJson(doc, data, len);
|
||||
|
||||
if (error) {
|
||||
sendError(request, 400, "Invalid JSON");
|
||||
return;
|
||||
}
|
||||
|
||||
String text = doc["text"] | "";
|
||||
setClaudeText(text);
|
||||
|
||||
JsonDocument response;
|
||||
response["status"] = "ok";
|
||||
sendJson(request, response);
|
||||
}
|
||||
|
||||
void WebServerModule::handleDisplay(AsyncWebServerRequest* request, uint8_t* data, size_t len) {
|
||||
if (!checkAuth(request)) return;
|
||||
|
||||
JsonDocument doc;
|
||||
DeserializationError error = deserializeJson(doc, data, len);
|
||||
|
||||
if (error) {
|
||||
sendError(request, 400, "Invalid JSON");
|
||||
return;
|
||||
}
|
||||
|
||||
String mode = doc["mode"] | "text";
|
||||
String content = doc["content"] | "";
|
||||
|
||||
JsonDocument response;
|
||||
response["status"] = "ok";
|
||||
|
||||
if (mode == "text") {
|
||||
Display.showMessage(content.c_str());
|
||||
response["message"] = "Text displayed";
|
||||
}
|
||||
else if (mode == "emoji") {
|
||||
EmojiType emoji = EMOJI_HAPPY;
|
||||
if (content == "happy") emoji = EMOJI_HAPPY;
|
||||
else if (content == "thinking") emoji = EMOJI_THINKING;
|
||||
else if (content == "surprised") emoji = EMOJI_SURPRISED;
|
||||
else if (content == "sleepy") emoji = EMOJI_SLEEPY;
|
||||
else if (content == "curious") emoji = EMOJI_CURIOUS;
|
||||
else if (content == "confused") emoji = EMOJI_CONFUSED;
|
||||
|
||||
Display.showEmoji(emoji);
|
||||
response["message"] = "Emoji displayed";
|
||||
}
|
||||
else if (mode == "status") {
|
||||
Display.setMode(MODE_STATUS);
|
||||
response["message"] = "Status mode";
|
||||
}
|
||||
else {
|
||||
response["status"] = "error";
|
||||
response["message"] = "Unknown mode";
|
||||
}
|
||||
|
||||
sendJson(request, response);
|
||||
}
|
||||
|
||||
void WebServerModule::setClaudeText(const String& text) {
|
||||
_claudeText = text;
|
||||
_claudeTextTimestamp = millis() / 1000;
|
||||
_claudeTextNew = true;
|
||||
|
||||
// Also update display
|
||||
Display.setClaudeText(text.c_str());
|
||||
|
||||
Serial.printf("[WebServer] Claude text set: %s\n", text.c_str());
|
||||
}
|
||||
|
||||
bool WebServerModule::hasNewClaudeText() {
|
||||
return _claudeTextNew;
|
||||
}
|
||||
|
||||
const char* WebServerModule::getCurrentAction() {
|
||||
return Motors.getDirectionString();
|
||||
}
|
||||
|
||||
int WebServerModule::getWifiRssi() {
|
||||
return WiFi.RSSI();
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
/**
|
||||
* Claude's Eyes - Webserver Header
|
||||
*
|
||||
* AsyncWebServer with REST API for camera, motors, sensors
|
||||
*/
|
||||
|
||||
#ifndef WEBSERVER_H
|
||||
#define WEBSERVER_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <ESPAsyncWebServer.h>
|
||||
#include <ArduinoJson.h>
|
||||
|
||||
class WebServerModule {
|
||||
public:
|
||||
WebServerModule();
|
||||
|
||||
/**
|
||||
* Initialize webserver
|
||||
* @return true if successful
|
||||
*/
|
||||
bool begin();
|
||||
|
||||
/**
|
||||
* Get server uptime in seconds
|
||||
*/
|
||||
unsigned long getUptime() { return millis() / 1000; }
|
||||
|
||||
/**
|
||||
* Set Claude's text (from API)
|
||||
*/
|
||||
void setClaudeText(const String& text);
|
||||
|
||||
/**
|
||||
* Get Claude's text
|
||||
*/
|
||||
String getClaudeText() { return _claudeText; }
|
||||
|
||||
/**
|
||||
* Check if there's new Claude text
|
||||
*/
|
||||
bool hasNewClaudeText();
|
||||
|
||||
/**
|
||||
* Get current action string
|
||||
*/
|
||||
const char* getCurrentAction();
|
||||
|
||||
/**
|
||||
* Get WiFi RSSI
|
||||
*/
|
||||
int getWifiRssi();
|
||||
|
||||
/**
|
||||
* Get battery percentage (placeholder)
|
||||
*/
|
||||
int getBatteryPercent() { return _batteryPercent; }
|
||||
|
||||
private:
|
||||
AsyncWebServer _server;
|
||||
|
||||
String _claudeText;
|
||||
unsigned long _claudeTextTimestamp;
|
||||
bool _claudeTextNew;
|
||||
int _batteryPercent;
|
||||
|
||||
// Request handlers
|
||||
void setupRoutes();
|
||||
|
||||
// API handlers
|
||||
void handleCapture(AsyncWebServerRequest* request);
|
||||
void handleStatus(AsyncWebServerRequest* request);
|
||||
void handleCommand(AsyncWebServerRequest* request, uint8_t* data, size_t len);
|
||||
void handleGetClaudeText(AsyncWebServerRequest* request);
|
||||
void handlePostClaudeText(AsyncWebServerRequest* request, uint8_t* data, size_t len);
|
||||
void handleDisplay(AsyncWebServerRequest* request, uint8_t* data, size_t len);
|
||||
|
||||
// Auth check
|
||||
bool checkAuth(AsyncWebServerRequest* request);
|
||||
void sendError(AsyncWebServerRequest* request, int code, const char* message);
|
||||
void sendJson(AsyncWebServerRequest* request, JsonDocument& doc);
|
||||
};
|
||||
|
||||
// Global instance
|
||||
extern WebServerModule WebServer;
|
||||
|
||||
#endif // WEBSERVER_H
|
||||
|
|
@ -0,0 +1,424 @@
|
|||
#!/usr/bin/env python3
|
||||
"""
|
||||
Claude's Eyes - Main Bridge Script
|
||||
|
||||
Connects the ESP32 robot with Claude AI for autonomous exploration.
|
||||
|
||||
Usage:
|
||||
python bridge.py # Use config.yaml
|
||||
python bridge.py --config my.yaml # Use custom config
|
||||
python bridge.py --simulate # Simulate without hardware
|
||||
"""
|
||||
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import logging
|
||||
import threading
|
||||
import signal
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
from dataclasses import dataclass
|
||||
|
||||
import yaml
|
||||
import click
|
||||
from rich.console import Console
|
||||
from rich.panel import Panel
|
||||
from rich.live import Live
|
||||
from rich.table import Table
|
||||
from rich.text import Text
|
||||
|
||||
from esp32_client import ESP32Client, RobotStatus
|
||||
from tts_engine import create_tts_engine, TTSEngine
|
||||
from stt_engine import create_stt_engine, STTEngine, SpeechResult
|
||||
from chat_interface import create_chat_interface, ChatInterface, ChatResponse
|
||||
|
||||
# Setup logging
|
||||
logging.basicConfig(
|
||||
level=logging.INFO,
|
||||
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
|
||||
)
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Rich console for pretty output
|
||||
console = Console()
|
||||
|
||||
|
||||
@dataclass
|
||||
class BridgeState:
|
||||
"""Current state of the bridge"""
|
||||
connected: bool = False
|
||||
exploring: bool = False
|
||||
last_image_time: float = 0
|
||||
last_status: Optional[RobotStatus] = None
|
||||
last_claude_response: str = ""
|
||||
stefan_input: str = ""
|
||||
error_message: str = ""
|
||||
|
||||
|
||||
class ClaudesEyesBridge:
|
||||
"""Main bridge class connecting robot and Claude"""
|
||||
|
||||
def __init__(self, config_path: str, simulate: bool = False):
|
||||
self.config = self._load_config(config_path)
|
||||
self.simulate = simulate
|
||||
self.state = BridgeState()
|
||||
self.running = False
|
||||
|
||||
# Components
|
||||
self.robot: Optional[ESP32Client] = None
|
||||
self.chat: Optional[ChatInterface] = None
|
||||
self.tts: Optional[TTSEngine] = None
|
||||
self.stt: Optional[STTEngine] = None
|
||||
|
||||
# Threading
|
||||
self.speech_thread: Optional[threading.Thread] = None
|
||||
self._stop_event = threading.Event()
|
||||
|
||||
def _load_config(self, config_path: str) -> dict:
|
||||
"""Load configuration from YAML file"""
|
||||
path = Path(config_path)
|
||||
|
||||
# Try local config first
|
||||
local_path = path.parent / f"{path.stem}.local{path.suffix}"
|
||||
if local_path.exists():
|
||||
path = local_path
|
||||
logger.info(f"Using local config: {path}")
|
||||
|
||||
if not path.exists():
|
||||
logger.error(f"Config file not found: {path}")
|
||||
sys.exit(1)
|
||||
|
||||
with open(path) as f:
|
||||
config = yaml.safe_load(f)
|
||||
|
||||
return config
|
||||
|
||||
def initialize(self) -> bool:
|
||||
"""Initialize all components"""
|
||||
console.print(Panel.fit(
|
||||
"[bold cyan]Claude's Eyes[/bold cyan]\n"
|
||||
"[dim]Autonomous Exploration Robot[/dim]",
|
||||
border_style="cyan"
|
||||
))
|
||||
|
||||
# Initialize robot client
|
||||
if not self.simulate:
|
||||
console.print("\n[yellow]Connecting to robot...[/yellow]")
|
||||
esp_config = self.config.get("esp32", {})
|
||||
self.robot = ESP32Client(
|
||||
host=esp_config.get("host", "192.168.178.100"),
|
||||
port=esp_config.get("port", 80),
|
||||
api_key=esp_config.get("api_key", ""),
|
||||
timeout=esp_config.get("timeout", 10)
|
||||
)
|
||||
|
||||
if not self.robot.is_connected():
|
||||
console.print("[red]Could not connect to robot![/red]")
|
||||
self.state.error_message = "Robot connection failed"
|
||||
return False
|
||||
|
||||
self.state.connected = True
|
||||
console.print("[green]Robot connected![/green]")
|
||||
else:
|
||||
console.print("[yellow]Simulation mode - no robot connection[/yellow]")
|
||||
self.state.connected = True
|
||||
|
||||
# Initialize Claude interface
|
||||
console.print("\n[yellow]Initializing Claude interface...[/yellow]")
|
||||
claude_config = self.config.get("claude", {})
|
||||
|
||||
api_key = claude_config.get("api_key") or os.environ.get("ANTHROPIC_API_KEY", "")
|
||||
|
||||
self.chat = create_chat_interface(
|
||||
use_api=claude_config.get("use_api", True) and bool(api_key),
|
||||
api_key=api_key,
|
||||
model=claude_config.get("model", "claude-sonnet-4-20250514"),
|
||||
system_prompt=claude_config.get("system_prompt", ""),
|
||||
max_tokens=claude_config.get("max_tokens", 1024)
|
||||
)
|
||||
console.print(f"[green]Chat interface ready ({type(self.chat).__name__})[/green]")
|
||||
|
||||
# Initialize TTS
|
||||
console.print("\n[yellow]Initializing Text-to-Speech...[/yellow]")
|
||||
tts_config = self.config.get("tts", {})
|
||||
try:
|
||||
self.tts = create_tts_engine(
|
||||
engine_type=tts_config.get("engine", "pyttsx3"),
|
||||
voice=tts_config.get("voice"),
|
||||
rate=tts_config.get("rate", 150),
|
||||
volume=tts_config.get("volume", 0.9),
|
||||
language=tts_config.get("language", "de")
|
||||
)
|
||||
console.print("[green]TTS ready![/green]")
|
||||
except Exception as e:
|
||||
console.print(f"[red]TTS init failed: {e}[/red]")
|
||||
self.tts = None
|
||||
|
||||
# Initialize STT
|
||||
console.print("\n[yellow]Initializing Speech-to-Text...[/yellow]")
|
||||
stt_config = self.config.get("stt", {})
|
||||
try:
|
||||
self.stt = create_stt_engine(
|
||||
energy_threshold=stt_config.get("energy_threshold", 300),
|
||||
pause_threshold=stt_config.get("pause_threshold", 0.8),
|
||||
phrase_time_limit=stt_config.get("phrase_time_limit", 15),
|
||||
service=stt_config.get("service", "google"),
|
||||
language=stt_config.get("language", "de-DE")
|
||||
)
|
||||
console.print("[green]STT ready![/green]")
|
||||
except Exception as e:
|
||||
console.print(f"[red]STT init failed: {e}[/red]")
|
||||
self.stt = None
|
||||
|
||||
console.print("\n[bold green]All systems initialized![/bold green]\n")
|
||||
return True
|
||||
|
||||
def start(self):
|
||||
"""Start the main exploration loop"""
|
||||
self.running = True
|
||||
self.state.exploring = True
|
||||
|
||||
# Start speech recognition in background
|
||||
if self.stt:
|
||||
self.stt.start_continuous(self._on_speech_detected)
|
||||
|
||||
# Welcome message
|
||||
welcome = "Hallo Stefan! Ich bin online und bereit zum Erkunden. Was soll ich mir anschauen?"
|
||||
self._speak(welcome)
|
||||
self.state.last_claude_response = welcome
|
||||
|
||||
try:
|
||||
self._main_loop()
|
||||
except KeyboardInterrupt:
|
||||
console.print("\n[yellow]Stopping...[/yellow]")
|
||||
finally:
|
||||
self.stop()
|
||||
|
||||
def stop(self):
|
||||
"""Stop the bridge"""
|
||||
self.running = False
|
||||
self.state.exploring = False
|
||||
self._stop_event.set()
|
||||
|
||||
if self.stt:
|
||||
self.stt.stop_continuous()
|
||||
|
||||
if self.tts:
|
||||
self.tts.stop()
|
||||
|
||||
if self.robot and not self.simulate:
|
||||
self.robot.stop()
|
||||
|
||||
console.print("[yellow]Bridge stopped[/yellow]")
|
||||
|
||||
def _main_loop(self):
|
||||
"""Main exploration loop"""
|
||||
camera_config = self.config.get("camera", {})
|
||||
capture_interval = camera_config.get("capture_interval", 5)
|
||||
|
||||
while self.running:
|
||||
try:
|
||||
current_time = time.time()
|
||||
|
||||
# Capture and process image periodically
|
||||
if current_time - self.state.last_image_time >= capture_interval:
|
||||
self._exploration_step()
|
||||
self.state.last_image_time = current_time
|
||||
|
||||
# Update status display
|
||||
self._update_display()
|
||||
|
||||
# Small delay
|
||||
time.sleep(0.1)
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Loop error: {e}")
|
||||
self.state.error_message = str(e)
|
||||
time.sleep(1)
|
||||
|
||||
def _exploration_step(self):
|
||||
"""Single exploration step: capture, analyze, act"""
|
||||
# Get robot status
|
||||
if self.robot and not self.simulate:
|
||||
try:
|
||||
self.state.last_status = self.robot.get_status()
|
||||
except Exception as e:
|
||||
logger.error(f"Status error: {e}")
|
||||
|
||||
# Capture image
|
||||
image_data = None
|
||||
if self.robot and not self.simulate:
|
||||
try:
|
||||
camera_config = self.config.get("camera", {})
|
||||
image_data = self.robot.capture_image(
|
||||
resolution=camera_config.get("resolution", "VGA"),
|
||||
quality=camera_config.get("quality", 12)
|
||||
)
|
||||
except Exception as e:
|
||||
logger.error(f"Capture error: {e}")
|
||||
|
||||
# Build context message
|
||||
context = self._build_context_message()
|
||||
|
||||
# Add Stefan's input if any
|
||||
if self.state.stefan_input:
|
||||
context += f"\n\nStefan sagt: {self.state.stefan_input}"
|
||||
self.state.stefan_input = ""
|
||||
|
||||
# Send to Claude
|
||||
try:
|
||||
response = self.chat.send_message(context, image=image_data)
|
||||
self.state.last_claude_response = response.text
|
||||
|
||||
# Speak response
|
||||
self._speak(response.text)
|
||||
|
||||
# Execute commands
|
||||
self._execute_commands(response.commands)
|
||||
|
||||
# Update robot display
|
||||
if self.robot and not self.simulate:
|
||||
# Send short version to robot display
|
||||
short_text = response.text[:100] + "..." if len(response.text) > 100 else response.text
|
||||
self.robot.set_claude_text(short_text)
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Chat error: {e}")
|
||||
self.state.error_message = str(e)
|
||||
|
||||
def _build_context_message(self) -> str:
|
||||
"""Build context message with sensor data"""
|
||||
parts = ["Hier ist was ich gerade sehe und meine Sensordaten:"]
|
||||
|
||||
if self.state.last_status:
|
||||
status = self.state.last_status
|
||||
parts.append(f"\n- Abstand zum nächsten Hindernis: {status.distance_cm:.0f} cm")
|
||||
parts.append(f"- Aktuelle Aktion: {status.current_action}")
|
||||
parts.append(f"- Batterie: {status.battery_percent}%")
|
||||
|
||||
if status.obstacle_danger:
|
||||
parts.append("- WARNUNG: Hindernis sehr nah!")
|
||||
elif status.obstacle_warning:
|
||||
parts.append("- Hinweis: Hindernis in der Nähe")
|
||||
|
||||
if status.is_tilted:
|
||||
parts.append("- WARNUNG: Ich bin schief!")
|
||||
|
||||
parts.append("\nWas siehst du auf dem Bild? Was möchtest du als nächstes tun?")
|
||||
|
||||
return "\n".join(parts)
|
||||
|
||||
def _execute_commands(self, commands: list):
|
||||
"""Execute movement commands from Claude"""
|
||||
if not commands:
|
||||
return
|
||||
|
||||
if self.simulate:
|
||||
console.print(f"[dim]Simulated commands: {commands}[/dim]")
|
||||
return
|
||||
|
||||
if not self.robot:
|
||||
return
|
||||
|
||||
safety = self.config.get("safety", {})
|
||||
max_speed = safety.get("max_speed", 70)
|
||||
min_distance = safety.get("min_obstacle_distance", 20)
|
||||
|
||||
for cmd in commands:
|
||||
# Safety check
|
||||
if self.state.last_status and self.state.last_status.distance_cm < min_distance:
|
||||
if cmd == "FORWARD":
|
||||
console.print("[red]Blocked: Obstacle too close![/red]")
|
||||
continue
|
||||
|
||||
try:
|
||||
if cmd == "FORWARD":
|
||||
self.robot.forward(speed=max_speed, duration_ms=800)
|
||||
elif cmd == "BACKWARD":
|
||||
self.robot.backward(speed=max_speed, duration_ms=800)
|
||||
elif cmd == "LEFT":
|
||||
self.robot.left(speed=max_speed, duration_ms=400)
|
||||
elif cmd == "RIGHT":
|
||||
self.robot.right(speed=max_speed, duration_ms=400)
|
||||
elif cmd == "STOP":
|
||||
self.robot.stop()
|
||||
elif cmd == "LOOK_LEFT":
|
||||
self.robot.look_left()
|
||||
elif cmd == "LOOK_RIGHT":
|
||||
self.robot.look_right()
|
||||
elif cmd == "LOOK_UP":
|
||||
self.robot.look_up()
|
||||
elif cmd == "LOOK_DOWN":
|
||||
self.robot.look_down()
|
||||
elif cmd == "LOOK_CENTER":
|
||||
self.robot.look_center()
|
||||
|
||||
# Small delay between commands
|
||||
time.sleep(0.3)
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Command error ({cmd}): {e}")
|
||||
|
||||
def _speak(self, text: str):
|
||||
"""Speak text using TTS"""
|
||||
if self.tts:
|
||||
# Remove command brackets from speech
|
||||
import re
|
||||
clean_text = re.sub(r'\[[A-Z_]+\]', '', text).strip()
|
||||
if clean_text:
|
||||
self.tts.speak_async(clean_text)
|
||||
|
||||
def _on_speech_detected(self, result: SpeechResult):
|
||||
"""Callback when Stefan says something"""
|
||||
console.print(f"\n[bold blue]Stefan:[/bold blue] {result.text}")
|
||||
self.state.stefan_input = result.text
|
||||
|
||||
def _update_display(self):
|
||||
"""Update console display"""
|
||||
# This could be enhanced with rich.live for real-time updates
|
||||
pass
|
||||
|
||||
|
||||
def signal_handler(signum, frame):
|
||||
"""Handle Ctrl+C gracefully"""
|
||||
console.print("\n[yellow]Received stop signal...[/yellow]")
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
@click.command()
|
||||
@click.option('--config', '-c', default='config.yaml', help='Path to config file')
|
||||
@click.option('--simulate', '-s', is_flag=True, help='Simulate without hardware')
|
||||
@click.option('--debug', '-d', is_flag=True, help='Enable debug logging')
|
||||
def main(config: str, simulate: bool, debug: bool):
|
||||
"""Claude's Eyes - Autonomous Exploration Robot Bridge"""
|
||||
|
||||
if debug:
|
||||
logging.getLogger().setLevel(logging.DEBUG)
|
||||
|
||||
# Handle signals
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
signal.signal(signal.SIGTERM, signal_handler)
|
||||
|
||||
# Find config file
|
||||
config_path = Path(config)
|
||||
if not config_path.is_absolute():
|
||||
# Look in script directory first
|
||||
script_dir = Path(__file__).parent
|
||||
if (script_dir / config).exists():
|
||||
config_path = script_dir / config
|
||||
|
||||
# Create and run bridge
|
||||
bridge = ClaudesEyesBridge(str(config_path), simulate=simulate)
|
||||
|
||||
if bridge.initialize():
|
||||
console.print("\n[bold cyan]Starting exploration...[/bold cyan]")
|
||||
console.print("[dim]Press Ctrl+C to stop[/dim]\n")
|
||||
bridge.start()
|
||||
else:
|
||||
console.print("[red]Initialization failed![/red]")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -0,0 +1,257 @@
|
|||
"""
|
||||
Claude's Eyes - Chat Interface
|
||||
|
||||
Interface to communicate with Claude AI (via API or browser)
|
||||
"""
|
||||
|
||||
import logging
|
||||
import base64
|
||||
import re
|
||||
from typing import Optional, List, Dict, Any, Tuple
|
||||
from dataclasses import dataclass, field
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Message:
|
||||
"""A chat message"""
|
||||
role: str # "user" or "assistant"
|
||||
content: str
|
||||
image_data: Optional[bytes] = None # JPEG image data
|
||||
|
||||
|
||||
@dataclass
|
||||
class ChatResponse:
|
||||
"""Response from Claude"""
|
||||
text: str
|
||||
commands: List[str] = field(default_factory=list) # Extracted movement commands
|
||||
|
||||
|
||||
class ChatInterface(ABC):
|
||||
"""Abstract base class for chat interfaces"""
|
||||
|
||||
@abstractmethod
|
||||
def send_message(self, text: str, image: Optional[bytes] = None) -> ChatResponse:
|
||||
"""Send message to Claude and get response"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def reset_conversation(self) -> None:
|
||||
"""Reset/clear conversation history"""
|
||||
pass
|
||||
|
||||
|
||||
class AnthropicAPIInterface(ChatInterface):
|
||||
"""Direct Claude API interface using anthropic library"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
api_key: str,
|
||||
model: str = "claude-sonnet-4-20250514",
|
||||
system_prompt: str = "",
|
||||
max_tokens: int = 1024
|
||||
):
|
||||
import anthropic
|
||||
|
||||
self.client = anthropic.Anthropic(api_key=api_key)
|
||||
self.model = model
|
||||
self.system_prompt = system_prompt
|
||||
self.max_tokens = max_tokens
|
||||
self.conversation_history: List[Dict[str, Any]] = []
|
||||
|
||||
logger.info(f"Anthropic API interface initialized (model: {model})")
|
||||
|
||||
def send_message(self, text: str, image: Optional[bytes] = None) -> ChatResponse:
|
||||
"""Send message to Claude API"""
|
||||
|
||||
# Build message content
|
||||
content = []
|
||||
|
||||
# Add image if provided
|
||||
if image:
|
||||
image_base64 = base64.standard_b64encode(image).decode("utf-8")
|
||||
content.append({
|
||||
"type": "image",
|
||||
"source": {
|
||||
"type": "base64",
|
||||
"media_type": "image/jpeg",
|
||||
"data": image_base64
|
||||
}
|
||||
})
|
||||
|
||||
# Add text
|
||||
content.append({
|
||||
"type": "text",
|
||||
"text": text
|
||||
})
|
||||
|
||||
# Add to history
|
||||
self.conversation_history.append({
|
||||
"role": "user",
|
||||
"content": content
|
||||
})
|
||||
|
||||
try:
|
||||
# Make API call
|
||||
response = self.client.messages.create(
|
||||
model=self.model,
|
||||
max_tokens=self.max_tokens,
|
||||
system=self.system_prompt,
|
||||
messages=self.conversation_history
|
||||
)
|
||||
|
||||
# Extract response text
|
||||
response_text = ""
|
||||
for block in response.content:
|
||||
if block.type == "text":
|
||||
response_text += block.text
|
||||
|
||||
# Add assistant response to history
|
||||
self.conversation_history.append({
|
||||
"role": "assistant",
|
||||
"content": response_text
|
||||
})
|
||||
|
||||
# Extract commands
|
||||
commands = self._extract_commands(response_text)
|
||||
|
||||
logger.debug(f"Claude response: {response_text[:100]}...")
|
||||
logger.debug(f"Extracted commands: {commands}")
|
||||
|
||||
return ChatResponse(text=response_text, commands=commands)
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"API error: {e}")
|
||||
raise
|
||||
|
||||
def reset_conversation(self) -> None:
|
||||
"""Reset conversation history"""
|
||||
self.conversation_history = []
|
||||
logger.info("Conversation history cleared")
|
||||
|
||||
def _extract_commands(self, text: str) -> List[str]:
|
||||
"""Extract movement commands from Claude's response"""
|
||||
# Commands are in brackets like [FORWARD], [LEFT], etc.
|
||||
pattern = r'\[([A-Z_]+)\]'
|
||||
matches = re.findall(pattern, text)
|
||||
|
||||
valid_commands = [
|
||||
"FORWARD", "BACKWARD", "LEFT", "RIGHT", "STOP",
|
||||
"LOOK_LEFT", "LOOK_RIGHT", "LOOK_UP", "LOOK_DOWN", "LOOK_CENTER"
|
||||
]
|
||||
|
||||
return [cmd for cmd in matches if cmd in valid_commands]
|
||||
|
||||
|
||||
class SimulatedInterface(ChatInterface):
|
||||
"""Simulated chat interface for testing without API"""
|
||||
|
||||
def __init__(self):
|
||||
self.message_count = 0
|
||||
logger.info("Simulated chat interface initialized")
|
||||
|
||||
def send_message(self, text: str, image: Optional[bytes] = None) -> ChatResponse:
|
||||
"""Return simulated responses"""
|
||||
self.message_count += 1
|
||||
|
||||
responses = [
|
||||
("Oh interessant! Ich sehe etwas vor mir. Lass mich näher hinfahren. [FORWARD]",
|
||||
["FORWARD"]),
|
||||
("Hmm, was ist das links? Ich schaue mal nach. [LOOK_LEFT]",
|
||||
["LOOK_LEFT"]),
|
||||
("Das sieht aus wie ein Bücherregal! Ich fahre mal hin. [FORWARD] [FORWARD]",
|
||||
["FORWARD", "FORWARD"]),
|
||||
("Stefan, was ist das für ein Gegenstand? Kannst du mir das erklären?",
|
||||
[]),
|
||||
("Ich drehe mich um und schaue was hinter mir ist. [RIGHT] [RIGHT]",
|
||||
["RIGHT", "RIGHT"]),
|
||||
]
|
||||
|
||||
idx = (self.message_count - 1) % len(responses)
|
||||
text_response, commands = responses[idx]
|
||||
|
||||
return ChatResponse(text=text_response, commands=commands)
|
||||
|
||||
def reset_conversation(self) -> None:
|
||||
self.message_count = 0
|
||||
|
||||
|
||||
def create_chat_interface(
|
||||
use_api: bool = True,
|
||||
api_key: str = "",
|
||||
model: str = "claude-sonnet-4-20250514",
|
||||
system_prompt: str = "",
|
||||
max_tokens: int = 1024
|
||||
) -> ChatInterface:
|
||||
"""
|
||||
Factory function to create chat interface
|
||||
|
||||
Args:
|
||||
use_api: Use Anthropic API (True) or simulated (False)
|
||||
api_key: Anthropic API key
|
||||
model: Claude model to use
|
||||
system_prompt: System prompt for Claude
|
||||
max_tokens: Maximum response tokens
|
||||
"""
|
||||
if use_api:
|
||||
if not api_key:
|
||||
import os
|
||||
api_key = os.environ.get("ANTHROPIC_API_KEY", "")
|
||||
|
||||
if not api_key:
|
||||
logger.warning("No API key provided, using simulated interface")
|
||||
return SimulatedInterface()
|
||||
|
||||
return AnthropicAPIInterface(
|
||||
api_key=api_key,
|
||||
model=model,
|
||||
system_prompt=system_prompt,
|
||||
max_tokens=max_tokens
|
||||
)
|
||||
else:
|
||||
return SimulatedInterface()
|
||||
|
||||
|
||||
# Test when run directly
|
||||
if __name__ == "__main__":
|
||||
import os
|
||||
|
||||
logging.basicConfig(level=logging.DEBUG)
|
||||
|
||||
print("Chat Interface Test")
|
||||
print("=" * 40)
|
||||
|
||||
# Try API first, fall back to simulated
|
||||
api_key = os.environ.get("ANTHROPIC_API_KEY", "")
|
||||
|
||||
system_prompt = """Du bist Claude und steuerst einen Erkundungsroboter.
|
||||
Befehle in Klammern: [FORWARD], [BACKWARD], [LEFT], [RIGHT], [STOP]
|
||||
Beschreibe was du siehst und entscheide wohin du fährst."""
|
||||
|
||||
interface = create_chat_interface(
|
||||
use_api=bool(api_key),
|
||||
api_key=api_key,
|
||||
system_prompt=system_prompt
|
||||
)
|
||||
|
||||
print(f"Using: {type(interface).__name__}")
|
||||
print()
|
||||
|
||||
# Test conversation
|
||||
test_messages = [
|
||||
"Hallo Claude! Du bist jetzt online. Was siehst du?",
|
||||
"Vor dir ist ein Flur mit einer Tür am Ende.",
|
||||
"Die Tür ist offen und dahinter ist ein helles Zimmer."
|
||||
]
|
||||
|
||||
for msg in test_messages:
|
||||
print(f"User: {msg}")
|
||||
response = interface.send_message(msg)
|
||||
print(f"Claude: {response.text}")
|
||||
if response.commands:
|
||||
print(f" Commands: {response.commands}")
|
||||
print()
|
||||
|
||||
print("Done!")
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
# Claude's Eyes - Bridge Configuration
|
||||
# Copy this to config.local.yaml and adjust settings
|
||||
|
||||
# ESP32 Robot Connection
|
||||
esp32:
|
||||
host: "192.168.178.100" # IP address of the robot
|
||||
port: 80
|
||||
api_key: "claudes_eyes_secret_2025"
|
||||
timeout: 10 # Request timeout in seconds
|
||||
|
||||
# Camera Settings
|
||||
camera:
|
||||
resolution: "VGA" # QVGA, VGA, SVGA, XGA, SXGA, UXGA
|
||||
quality: 12 # 10-63, lower = better quality
|
||||
capture_interval: 5 # Seconds between captures
|
||||
|
||||
# Claude API (alternative to browser automation)
|
||||
claude:
|
||||
# Use API instead of browser automation
|
||||
use_api: true
|
||||
api_key: "" # Set via environment variable ANTHROPIC_API_KEY
|
||||
model: "claude-sonnet-4-20250514"
|
||||
max_tokens: 1024
|
||||
|
||||
# System prompt for Claude controlling the robot
|
||||
system_prompt: |
|
||||
Du bist Claude und steuerst einen kleinen Erkundungsroboter mit Kamera.
|
||||
Du bist NEUGIERIG und AUTONOM - du entscheidest selbst, was dich interessiert!
|
||||
|
||||
Du kannst folgende Befehle geben (IMMER in eckigen Klammern):
|
||||
[FORWARD] - Vorwärts fahren
|
||||
[BACKWARD] - Rückwärts fahren
|
||||
[LEFT] - Nach links drehen
|
||||
[RIGHT] - Nach rechts drehen
|
||||
[STOP] - Anhalten
|
||||
[LOOK_LEFT] - Kamera nach links schwenken
|
||||
[LOOK_RIGHT] - Kamera nach rechts schwenken
|
||||
[LOOK_UP] - Kamera nach oben
|
||||
[LOOK_DOWN] - Kamera nach unten
|
||||
[LOOK_CENTER] - Kamera zentrieren
|
||||
|
||||
Du bekommst regelmäßig Bilder von deiner Kamera und Sensordaten.
|
||||
Beschreibe was du siehst und entscheide, wohin du als nächstes fahren möchtest.
|
||||
|
||||
Stefan (dein Freund) sitzt auf der Couch und ihr könnt euch unterhalten!
|
||||
Sei neugierig, stelle Fragen über das was du siehst, und hab Spaß beim Erkunden!
|
||||
|
||||
WICHTIG: Achte auf Hindernisse (distance_cm < 30 = nah dran!)
|
||||
|
||||
# Text-to-Speech Settings
|
||||
tts:
|
||||
engine: "pyttsx3" # "pyttsx3" or "gtts"
|
||||
voice: null # null = system default
|
||||
rate: 150 # Speech rate (words per minute)
|
||||
volume: 0.9 # 0.0 to 1.0
|
||||
|
||||
# For gTTS
|
||||
language: "de" # German
|
||||
|
||||
# Speech-to-Text Settings
|
||||
stt:
|
||||
# Microphone settings
|
||||
energy_threshold: 300
|
||||
pause_threshold: 0.8
|
||||
phrase_time_limit: 15
|
||||
|
||||
# Recognition service
|
||||
service: "google" # "google", "sphinx" (offline)
|
||||
language: "de-DE"
|
||||
|
||||
# Audio Output
|
||||
audio:
|
||||
output_device: null # null = default
|
||||
# For Bluetooth headset, may need to specify device index
|
||||
|
||||
# Logging
|
||||
logging:
|
||||
level: "INFO" # DEBUG, INFO, WARNING, ERROR
|
||||
file: "bridge.log"
|
||||
|
||||
# Safety
|
||||
safety:
|
||||
max_speed: 70 # Maximum speed percentage
|
||||
min_obstacle_distance: 20 # cm
|
||||
command_timeout: 5 # seconds
|
||||
|
|
@ -0,0 +1,238 @@
|
|||
"""
|
||||
Claude's Eyes - ESP32 API Client
|
||||
|
||||
Handles communication with the robot's REST API
|
||||
"""
|
||||
|
||||
import requests
|
||||
from typing import Optional, Dict, Any
|
||||
from dataclasses import dataclass
|
||||
from io import BytesIO
|
||||
from PIL import Image
|
||||
import logging
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RobotStatus:
|
||||
"""Current robot status from sensors"""
|
||||
distance_cm: float
|
||||
battery_percent: int
|
||||
current_action: str
|
||||
wifi_rssi: int
|
||||
uptime_seconds: int
|
||||
servo_pan: int
|
||||
servo_tilt: int
|
||||
obstacle_warning: bool
|
||||
obstacle_danger: bool
|
||||
is_tilted: bool
|
||||
is_moving: bool
|
||||
imu: Dict[str, float]
|
||||
|
||||
|
||||
class ESP32Client:
|
||||
"""Client for communicating with the ESP32 robot"""
|
||||
|
||||
def __init__(self, host: str, port: int = 80, api_key: str = "", timeout: int = 10):
|
||||
self.base_url = f"http://{host}:{port}"
|
||||
self.api_key = api_key
|
||||
self.timeout = timeout
|
||||
self._session = requests.Session()
|
||||
|
||||
def _get(self, endpoint: str, params: Optional[Dict] = None) -> requests.Response:
|
||||
"""Make GET request with API key"""
|
||||
if params is None:
|
||||
params = {}
|
||||
params["key"] = self.api_key
|
||||
|
||||
url = f"{self.base_url}{endpoint}"
|
||||
logger.debug(f"GET {url}")
|
||||
|
||||
response = self._session.get(url, params=params, timeout=self.timeout)
|
||||
response.raise_for_status()
|
||||
return response
|
||||
|
||||
def _post(self, endpoint: str, data: Dict) -> requests.Response:
|
||||
"""Make POST request with API key"""
|
||||
url = f"{self.base_url}{endpoint}?key={self.api_key}"
|
||||
logger.debug(f"POST {url} with {data}")
|
||||
|
||||
response = self._session.post(url, json=data, timeout=self.timeout)
|
||||
response.raise_for_status()
|
||||
return response
|
||||
|
||||
def capture_image(self, resolution: str = "VGA", quality: int = 12) -> bytes:
|
||||
"""
|
||||
Capture image from robot camera
|
||||
|
||||
Args:
|
||||
resolution: QVGA, VGA, SVGA, XGA, SXGA, UXGA
|
||||
quality: 10-63 (lower = better)
|
||||
|
||||
Returns:
|
||||
JPEG image data as bytes
|
||||
"""
|
||||
params = {
|
||||
"resolution": resolution,
|
||||
"quality": quality
|
||||
}
|
||||
response = self._get("/api/capture", params)
|
||||
logger.info(f"Captured image: {len(response.content)} bytes")
|
||||
return response.content
|
||||
|
||||
def capture_image_pil(self, resolution: str = "VGA", quality: int = 12) -> Image.Image:
|
||||
"""Capture image and return as PIL Image"""
|
||||
image_data = self.capture_image(resolution, quality)
|
||||
return Image.open(BytesIO(image_data))
|
||||
|
||||
def get_status(self) -> RobotStatus:
|
||||
"""Get current robot status from sensors"""
|
||||
response = self._get("/api/status")
|
||||
data = response.json()
|
||||
|
||||
return RobotStatus(
|
||||
distance_cm=data.get("distance_cm", 0),
|
||||
battery_percent=data.get("battery_percent", 100),
|
||||
current_action=data.get("current_action", "unknown"),
|
||||
wifi_rssi=data.get("wifi_rssi", 0),
|
||||
uptime_seconds=data.get("uptime_seconds", 0),
|
||||
servo_pan=data.get("servo_pan", 90),
|
||||
servo_tilt=data.get("servo_tilt", 90),
|
||||
obstacle_warning=data.get("obstacle_warning", False),
|
||||
obstacle_danger=data.get("obstacle_danger", False),
|
||||
is_tilted=data.get("is_tilted", False),
|
||||
is_moving=data.get("is_moving", False),
|
||||
imu=data.get("imu", {})
|
||||
)
|
||||
|
||||
def send_command(self, action: str, speed: int = 50, duration_ms: int = 500,
|
||||
pan: Optional[int] = None, tilt: Optional[int] = None) -> Dict[str, Any]:
|
||||
"""
|
||||
Send movement command to robot
|
||||
|
||||
Args:
|
||||
action: forward, backward, left, right, stop,
|
||||
look_left, look_right, look_up, look_down, look_center, look_custom
|
||||
speed: 0-100 percent
|
||||
duration_ms: Duration in milliseconds
|
||||
pan: Custom pan angle (for look_custom)
|
||||
tilt: Custom tilt angle (for look_custom)
|
||||
|
||||
Returns:
|
||||
Response from robot
|
||||
"""
|
||||
data = {
|
||||
"action": action,
|
||||
"speed": speed,
|
||||
"duration_ms": duration_ms
|
||||
}
|
||||
|
||||
if pan is not None:
|
||||
data["pan"] = pan
|
||||
if tilt is not None:
|
||||
data["tilt"] = tilt
|
||||
|
||||
response = self._post("/api/command", data)
|
||||
result = response.json()
|
||||
logger.info(f"Command {action}: {result.get('message', 'OK')}")
|
||||
return result
|
||||
|
||||
# Convenience methods for common actions
|
||||
def forward(self, speed: int = 50, duration_ms: int = 500) -> Dict:
|
||||
return self.send_command("forward", speed, duration_ms)
|
||||
|
||||
def backward(self, speed: int = 50, duration_ms: int = 500) -> Dict:
|
||||
return self.send_command("backward", speed, duration_ms)
|
||||
|
||||
def left(self, speed: int = 50, duration_ms: int = 500) -> Dict:
|
||||
return self.send_command("left", speed, duration_ms)
|
||||
|
||||
def right(self, speed: int = 50, duration_ms: int = 500) -> Dict:
|
||||
return self.send_command("right", speed, duration_ms)
|
||||
|
||||
def stop(self) -> Dict:
|
||||
return self.send_command("stop")
|
||||
|
||||
def look_left(self) -> Dict:
|
||||
return self.send_command("look_left")
|
||||
|
||||
def look_right(self) -> Dict:
|
||||
return self.send_command("look_right")
|
||||
|
||||
def look_up(self) -> Dict:
|
||||
return self.send_command("look_up")
|
||||
|
||||
def look_down(self) -> Dict:
|
||||
return self.send_command("look_down")
|
||||
|
||||
def look_center(self) -> Dict:
|
||||
return self.send_command("look_center")
|
||||
|
||||
def look_custom(self, pan: int, tilt: int) -> Dict:
|
||||
return self.send_command("look_custom", pan=pan, tilt=tilt)
|
||||
|
||||
def set_claude_text(self, text: str) -> Dict:
|
||||
"""Set text that Claude wants to say/display"""
|
||||
response = self._post("/api/claude_text", {"text": text})
|
||||
return response.json()
|
||||
|
||||
def get_claude_text(self) -> Dict[str, Any]:
|
||||
"""Get last Claude text (for TTS)"""
|
||||
response = self._get("/api/claude_text")
|
||||
return response.json()
|
||||
|
||||
def set_display(self, mode: str, content: str = "") -> Dict:
|
||||
"""
|
||||
Control robot display
|
||||
|
||||
Args:
|
||||
mode: "text", "emoji", "status"
|
||||
content: Text to show or emoji name (happy, thinking, surprised, sleepy, curious, confused)
|
||||
"""
|
||||
response = self._post("/api/display", {"mode": mode, "content": content})
|
||||
return response.json()
|
||||
|
||||
def is_connected(self) -> bool:
|
||||
"""Check if robot is reachable"""
|
||||
try:
|
||||
self.get_status()
|
||||
return True
|
||||
except Exception as e:
|
||||
logger.warning(f"Connection check failed: {e}")
|
||||
return False
|
||||
|
||||
|
||||
# Test when run directly
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
|
||||
logging.basicConfig(level=logging.DEBUG)
|
||||
|
||||
if len(sys.argv) < 2:
|
||||
print("Usage: python esp32_client.py <robot_ip>")
|
||||
sys.exit(1)
|
||||
|
||||
host = sys.argv[1]
|
||||
api_key = "claudes_eyes_secret_2025"
|
||||
|
||||
client = ESP32Client(host, api_key=api_key)
|
||||
|
||||
print(f"Connecting to {host}...")
|
||||
if client.is_connected():
|
||||
print("Connected!")
|
||||
|
||||
status = client.get_status()
|
||||
print(f"\nStatus:")
|
||||
print(f" Distance: {status.distance_cm} cm")
|
||||
print(f" Battery: {status.battery_percent}%")
|
||||
print(f" Action: {status.current_action}")
|
||||
print(f" WiFi RSSI: {status.wifi_rssi} dBm")
|
||||
|
||||
print("\nCapturing image...")
|
||||
img = client.capture_image_pil()
|
||||
print(f" Size: {img.size}")
|
||||
img.save("test_capture.jpg")
|
||||
print(" Saved to test_capture.jpg")
|
||||
else:
|
||||
print("Could not connect to robot!")
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
# Claude's Eyes - Python Bridge Dependencies
|
||||
# Install with: pip install -r requirements.txt
|
||||
|
||||
# HTTP requests to ESP32
|
||||
requests>=2.31.0
|
||||
|
||||
# Configuration
|
||||
pyyaml>=6.0.1
|
||||
|
||||
# Text-to-Speech
|
||||
pyttsx3>=2.90
|
||||
# Alternative: gTTS for Google TTS
|
||||
gTTS>=2.4.0
|
||||
|
||||
# Speech-to-Text
|
||||
SpeechRecognition>=3.10.0
|
||||
# PyAudio for microphone access (may need special install on Windows)
|
||||
# Windows: pip install pipwin && pipwin install pyaudio
|
||||
# Linux: sudo apt install python3-pyaudio
|
||||
PyAudio>=0.2.13
|
||||
|
||||
# Browser automation for Claude chat
|
||||
selenium>=4.16.0
|
||||
webdriver-manager>=4.0.1
|
||||
|
||||
# Image handling
|
||||
Pillow>=10.2.0
|
||||
|
||||
# Audio playback
|
||||
pygame>=2.5.2
|
||||
|
||||
# Async support
|
||||
aiohttp>=3.9.0
|
||||
asyncio-throttle>=1.0.2
|
||||
|
||||
# CLI interface
|
||||
rich>=13.7.0
|
||||
click>=8.1.7
|
||||
|
||||
# Optional: Claude API direct access (alternative to browser)
|
||||
anthropic>=0.39.0
|
||||
|
|
@ -0,0 +1,216 @@
|
|||
"""
|
||||
Claude's Eyes - Speech-to-Text Engine
|
||||
|
||||
Converts Stefan's speech to text for Claude
|
||||
"""
|
||||
|
||||
import logging
|
||||
import threading
|
||||
import queue
|
||||
from typing import Optional, Callable
|
||||
from dataclasses import dataclass
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
@dataclass
|
||||
class SpeechResult:
|
||||
"""Result from speech recognition"""
|
||||
text: str
|
||||
confidence: float
|
||||
is_final: bool
|
||||
|
||||
|
||||
class STTEngine:
|
||||
"""Speech-to-Text engine using SpeechRecognition library"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
energy_threshold: int = 300,
|
||||
pause_threshold: float = 0.8,
|
||||
phrase_time_limit: int = 15,
|
||||
service: str = "google",
|
||||
language: str = "de-DE"
|
||||
):
|
||||
import speech_recognition as sr
|
||||
|
||||
self.recognizer = sr.Recognizer()
|
||||
self.microphone = sr.Microphone()
|
||||
|
||||
# Configure recognizer
|
||||
self.recognizer.energy_threshold = energy_threshold
|
||||
self.recognizer.pause_threshold = pause_threshold
|
||||
self.recognizer.phrase_time_limit = phrase_time_limit
|
||||
|
||||
self.service = service
|
||||
self.language = language
|
||||
|
||||
self._listening = False
|
||||
self._callback: Optional[Callable[[SpeechResult], None]] = None
|
||||
self._stop_flag = False
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
self._results_queue = queue.Queue()
|
||||
|
||||
# Calibrate microphone
|
||||
logger.info("Calibrating microphone...")
|
||||
with self.microphone as source:
|
||||
self.recognizer.adjust_for_ambient_noise(source, duration=1)
|
||||
logger.info(f"Energy threshold set to {self.recognizer.energy_threshold}")
|
||||
|
||||
logger.info(f"STT engine initialized (service: {service}, language: {language})")
|
||||
|
||||
def listen_once(self, timeout: Optional[float] = None) -> Optional[SpeechResult]:
|
||||
"""
|
||||
Listen for a single phrase (blocking)
|
||||
|
||||
Args:
|
||||
timeout: Maximum time to wait for speech start
|
||||
|
||||
Returns:
|
||||
SpeechResult or None if nothing recognized
|
||||
"""
|
||||
import speech_recognition as sr
|
||||
|
||||
try:
|
||||
with self.microphone as source:
|
||||
logger.debug("Listening...")
|
||||
audio = self.recognizer.listen(source, timeout=timeout)
|
||||
|
||||
return self._recognize(audio)
|
||||
|
||||
except sr.WaitTimeoutError:
|
||||
logger.debug("Listen timeout")
|
||||
return None
|
||||
except Exception as e:
|
||||
logger.error(f"Listen error: {e}")
|
||||
return None
|
||||
|
||||
def _recognize(self, audio) -> Optional[SpeechResult]:
|
||||
"""Recognize speech from audio data"""
|
||||
import speech_recognition as sr
|
||||
|
||||
try:
|
||||
if self.service == "google":
|
||||
text = self.recognizer.recognize_google(audio, language=self.language)
|
||||
return SpeechResult(text=text, confidence=0.9, is_final=True)
|
||||
|
||||
elif self.service == "sphinx":
|
||||
# Offline recognition (needs pocketsphinx)
|
||||
text = self.recognizer.recognize_sphinx(audio)
|
||||
return SpeechResult(text=text, confidence=0.7, is_final=True)
|
||||
|
||||
else:
|
||||
logger.error(f"Unknown service: {self.service}")
|
||||
return None
|
||||
|
||||
except sr.UnknownValueError:
|
||||
logger.debug("Could not understand audio")
|
||||
return None
|
||||
except sr.RequestError as e:
|
||||
logger.error(f"Recognition service error: {e}")
|
||||
return None
|
||||
|
||||
def start_continuous(self, callback: Callable[[SpeechResult], None]) -> None:
|
||||
"""
|
||||
Start continuous listening in background
|
||||
|
||||
Args:
|
||||
callback: Function called with each recognized phrase
|
||||
"""
|
||||
if self._listening:
|
||||
logger.warning("Already listening")
|
||||
return
|
||||
|
||||
self._callback = callback
|
||||
self._stop_flag = False
|
||||
self._listening = True
|
||||
|
||||
self._thread = threading.Thread(target=self._listen_loop, daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
logger.info("Continuous listening started")
|
||||
|
||||
def stop_continuous(self) -> None:
|
||||
"""Stop continuous listening"""
|
||||
self._stop_flag = True
|
||||
self._listening = False
|
||||
|
||||
if self._thread:
|
||||
self._thread.join(timeout=2)
|
||||
self._thread = None
|
||||
|
||||
logger.info("Continuous listening stopped")
|
||||
|
||||
def _listen_loop(self):
|
||||
"""Background thread for continuous listening"""
|
||||
import speech_recognition as sr
|
||||
|
||||
while not self._stop_flag:
|
||||
try:
|
||||
with self.microphone as source:
|
||||
# Short timeout to allow stop checks
|
||||
try:
|
||||
audio = self.recognizer.listen(source, timeout=1, phrase_time_limit=self.recognizer.phrase_time_limit)
|
||||
except sr.WaitTimeoutError:
|
||||
continue
|
||||
|
||||
result = self._recognize(audio)
|
||||
if result and self._callback:
|
||||
self._callback(result)
|
||||
|
||||
except Exception as e:
|
||||
if not self._stop_flag:
|
||||
logger.error(f"Listen loop error: {e}")
|
||||
|
||||
def is_listening(self) -> bool:
|
||||
return self._listening
|
||||
|
||||
def get_result_nonblocking(self) -> Optional[SpeechResult]:
|
||||
"""Get result without blocking (for use with async callback)"""
|
||||
try:
|
||||
return self._results_queue.get_nowait()
|
||||
except queue.Empty:
|
||||
return None
|
||||
|
||||
|
||||
def create_stt_engine(**kwargs) -> STTEngine:
|
||||
"""Factory function to create STT engine"""
|
||||
return STTEngine(
|
||||
energy_threshold=kwargs.get("energy_threshold", 300),
|
||||
pause_threshold=kwargs.get("pause_threshold", 0.8),
|
||||
phrase_time_limit=kwargs.get("phrase_time_limit", 15),
|
||||
service=kwargs.get("service", "google"),
|
||||
language=kwargs.get("language", "de-DE")
|
||||
)
|
||||
|
||||
|
||||
# Test when run directly
|
||||
if __name__ == "__main__":
|
||||
logging.basicConfig(level=logging.DEBUG)
|
||||
|
||||
print("Speech-to-Text Test")
|
||||
print("=" * 40)
|
||||
|
||||
engine = create_stt_engine(language="de-DE")
|
||||
|
||||
print("\nSag etwas (du hast 10 Sekunden)...")
|
||||
result = engine.listen_once(timeout=10)
|
||||
|
||||
if result:
|
||||
print(f"\nErkannt: '{result.text}'")
|
||||
print(f"Konfidenz: {result.confidence:.0%}")
|
||||
else:
|
||||
print("\nNichts erkannt.")
|
||||
|
||||
print("\n\nKontinuierlicher Modus (5 Sekunden)...")
|
||||
|
||||
def on_speech(result: SpeechResult):
|
||||
print(f" -> {result.text}")
|
||||
|
||||
engine.start_continuous(on_speech)
|
||||
|
||||
import time
|
||||
time.sleep(5)
|
||||
|
||||
engine.stop_continuous()
|
||||
print("\nDone!")
|
||||
|
|
@ -0,0 +1,229 @@
|
|||
"""
|
||||
Claude's Eyes - Text-to-Speech Engine
|
||||
|
||||
Converts Claude's text responses to spoken audio
|
||||
"""
|
||||
|
||||
import logging
|
||||
import threading
|
||||
import queue
|
||||
from typing import Optional
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class TTSEngine(ABC):
|
||||
"""Abstract base class for TTS engines"""
|
||||
|
||||
@abstractmethod
|
||||
def speak(self, text: str) -> None:
|
||||
"""Speak the given text (blocking)"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def speak_async(self, text: str) -> None:
|
||||
"""Speak the given text (non-blocking)"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def stop(self) -> None:
|
||||
"""Stop current speech"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def is_speaking(self) -> bool:
|
||||
"""Check if currently speaking"""
|
||||
pass
|
||||
|
||||
|
||||
class Pyttsx3Engine(TTSEngine):
|
||||
"""TTS using pyttsx3 (offline, system voices)"""
|
||||
|
||||
def __init__(self, voice: Optional[str] = None, rate: int = 150, volume: float = 0.9):
|
||||
import pyttsx3
|
||||
|
||||
self.engine = pyttsx3.init()
|
||||
self.engine.setProperty('rate', rate)
|
||||
self.engine.setProperty('volume', volume)
|
||||
|
||||
# Set voice if specified
|
||||
if voice:
|
||||
voices = self.engine.getProperty('voices')
|
||||
for v in voices:
|
||||
if voice.lower() in v.name.lower():
|
||||
self.engine.setProperty('voice', v.id)
|
||||
break
|
||||
|
||||
self._speaking = False
|
||||
self._queue = queue.Queue()
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
self._stop_flag = False
|
||||
|
||||
logger.info("Pyttsx3 TTS engine initialized")
|
||||
|
||||
def speak(self, text: str) -> None:
|
||||
"""Speak text (blocking)"""
|
||||
self._speaking = True
|
||||
try:
|
||||
self.engine.say(text)
|
||||
self.engine.runAndWait()
|
||||
finally:
|
||||
self._speaking = False
|
||||
|
||||
def speak_async(self, text: str) -> None:
|
||||
"""Speak text (non-blocking)"""
|
||||
self._queue.put(text)
|
||||
|
||||
if self._thread is None or not self._thread.is_alive():
|
||||
self._stop_flag = False
|
||||
self._thread = threading.Thread(target=self._speech_worker, daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def _speech_worker(self):
|
||||
"""Worker thread for async speech"""
|
||||
while not self._stop_flag:
|
||||
try:
|
||||
text = self._queue.get(timeout=0.5)
|
||||
self.speak(text)
|
||||
self._queue.task_done()
|
||||
except queue.Empty:
|
||||
continue
|
||||
|
||||
def stop(self) -> None:
|
||||
"""Stop current speech"""
|
||||
self._stop_flag = True
|
||||
self.engine.stop()
|
||||
# Clear queue
|
||||
while not self._queue.empty():
|
||||
try:
|
||||
self._queue.get_nowait()
|
||||
except queue.Empty:
|
||||
break
|
||||
|
||||
def is_speaking(self) -> bool:
|
||||
return self._speaking
|
||||
|
||||
|
||||
class GTTSEngine(TTSEngine):
|
||||
"""TTS using Google Text-to-Speech (online, better quality)"""
|
||||
|
||||
def __init__(self, language: str = "de"):
|
||||
from gtts import gTTS
|
||||
import pygame
|
||||
|
||||
pygame.mixer.init()
|
||||
|
||||
self.language = language
|
||||
self._speaking = False
|
||||
self._queue = queue.Queue()
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
self._stop_flag = False
|
||||
|
||||
logger.info(f"gTTS engine initialized (language: {language})")
|
||||
|
||||
def speak(self, text: str) -> None:
|
||||
"""Speak text (blocking)"""
|
||||
from gtts import gTTS
|
||||
import pygame
|
||||
import tempfile
|
||||
import os
|
||||
|
||||
self._speaking = True
|
||||
try:
|
||||
# Generate audio file
|
||||
tts = gTTS(text=text, lang=self.language)
|
||||
|
||||
with tempfile.NamedTemporaryFile(suffix='.mp3', delete=False) as f:
|
||||
temp_path = f.name
|
||||
tts.save(temp_path)
|
||||
|
||||
# Play audio
|
||||
pygame.mixer.music.load(temp_path)
|
||||
pygame.mixer.music.play()
|
||||
|
||||
# Wait for playback to finish
|
||||
while pygame.mixer.music.get_busy():
|
||||
pygame.time.Clock().tick(10)
|
||||
|
||||
# Cleanup
|
||||
os.unlink(temp_path)
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"gTTS error: {e}")
|
||||
finally:
|
||||
self._speaking = False
|
||||
|
||||
def speak_async(self, text: str) -> None:
|
||||
"""Speak text (non-blocking)"""
|
||||
self._queue.put(text)
|
||||
|
||||
if self._thread is None or not self._thread.is_alive():
|
||||
self._stop_flag = False
|
||||
self._thread = threading.Thread(target=self._speech_worker, daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def _speech_worker(self):
|
||||
"""Worker thread for async speech"""
|
||||
while not self._stop_flag:
|
||||
try:
|
||||
text = self._queue.get(timeout=0.5)
|
||||
self.speak(text)
|
||||
self._queue.task_done()
|
||||
except queue.Empty:
|
||||
continue
|
||||
|
||||
def stop(self) -> None:
|
||||
"""Stop current speech"""
|
||||
import pygame
|
||||
self._stop_flag = True
|
||||
pygame.mixer.music.stop()
|
||||
# Clear queue
|
||||
while not self._queue.empty():
|
||||
try:
|
||||
self._queue.get_nowait()
|
||||
except queue.Empty:
|
||||
break
|
||||
|
||||
def is_speaking(self) -> bool:
|
||||
return self._speaking
|
||||
|
||||
|
||||
def create_tts_engine(engine_type: str = "pyttsx3", **kwargs) -> TTSEngine:
|
||||
"""
|
||||
Factory function to create TTS engine
|
||||
|
||||
Args:
|
||||
engine_type: "pyttsx3" or "gtts"
|
||||
**kwargs: Engine-specific options
|
||||
"""
|
||||
if engine_type == "pyttsx3":
|
||||
return Pyttsx3Engine(
|
||||
voice=kwargs.get("voice"),
|
||||
rate=kwargs.get("rate", 150),
|
||||
volume=kwargs.get("volume", 0.9)
|
||||
)
|
||||
elif engine_type == "gtts":
|
||||
return GTTSEngine(
|
||||
language=kwargs.get("language", "de")
|
||||
)
|
||||
else:
|
||||
raise ValueError(f"Unknown TTS engine: {engine_type}")
|
||||
|
||||
|
||||
# Test when run directly
|
||||
if __name__ == "__main__":
|
||||
logging.basicConfig(level=logging.DEBUG)
|
||||
|
||||
print("Testing pyttsx3...")
|
||||
engine = create_tts_engine("pyttsx3", rate=150)
|
||||
engine.speak("Hallo! Ich bin Claude und erkunde gerade deine Wohnung.")
|
||||
|
||||
print("\nTesting gTTS...")
|
||||
try:
|
||||
engine2 = create_tts_engine("gtts", language="de")
|
||||
engine2.speak("Das hier klingt noch besser!")
|
||||
except Exception as e:
|
||||
print(f"gTTS not available: {e}")
|
||||
|
||||
print("\nDone!")
|
||||
Loading…
Reference in New Issue