Arduino ESP32 Camera State Machine with ESP-NOW

1import network
2import espnow
3import time
4import ubinascii
5from arduino import *
6from arduino_alvik import ArduinoAlvik
7from collections import deque
8
9# === CONFIGURATION ===
10e = None
11w = None
12CAMERA_MAC = ubinascii.unhexlify('841FE8683E64')
13
14# === STATE ===
15alvik = ArduinoAlvik()
16msg_queue = deque((), 20)  # Message queue (max 20)
17
18MESSAGES_COUNT = 0
19TRIGGER_COUNT = 5
20ROBOT_STATE = 'IDLE'
21MOVE_DURATION_MS = 2000
22MOVE_START_TIME = 0
23
24# === ESP-NOW SETUP ===
25def setup_espnow():
26    global e, w
27    w = network.WLAN(network.STA_IF)
28    w.active(True)
29    w.config(channel=1)
30
31    e = espnow.ESPNow()
32    e.active(True)
33    e.add_peer(CAMERA_MAC)
34
35    # IMPORTANT: only ONE recv per IRQ, not while loop!
36    e.irq(on_receive)
37    print("ESP-NOW pronto. MAC:", ubinascii.hexlify(w.config('mac'), ':').decode())
38
39# === IRQ CALLBACK: MINIMAL! ===
40def on_receive(esp_obj):
41    # Read ONLY ONE message at a time → avoid blocking!
42    packet = esp_obj.recv(0)
43    if packet and packet[1]:
44        try:
45            msg_queue.append(1)  # Add to queue
46        except:
47            pass  # queue full
48
49# === SETUP ===
50def setup():
51    global e
52    alvik.begin()
53    alvik.set_wheels_speed(0, 0)
54    alvik.left_led.set_color(0, 0, 0)
55    alvik.right_led.set_color(0, 0, 0)
56    
57    setup_espnow()
58    print("Setup completato. Attendo 5 messaggi.")
59
60# === MAIN LOOP ===
61def loop():
62    global MESSAGES_COUNT, ROBOT_STATE, MOVE_START_TIME
63
64    current_time = time.ticks_ms()
65
66    # --- 1. Process Message Queue (non-blocking) ---
67    while msg_queue and ROBOT_STATE == 'IDLE':
68        msg_queue.popleft()
69        MESSAGES_COUNT += 1
70        print(f"Messaggio ricevuto: {MESSAGES_COUNT}/{TRIGGER_COUNT}")
71
72        if MESSAGES_COUNT >= TRIGGER_COUNT:
73            MESSAGES_COUNT = 0
74            ROBOT_STATE = 'MOVING'
75            MOVE_START_TIME = current_time
76            alvik.set_wheels_speed(50, 50)
77            print("AVVIO MOVIMENTO: 2 secondi")
78
79    # --- 2. Movement Timer Management ---
80    if ROBOT_STATE == 'MOVING':
81        elapsed = time.ticks_diff(current_time, MOVE_START_TIME)
82        if elapsed >= MOVE_DURATION_MS:
83            alvik.set_wheels_speed(0, 0)
84            ROBOT_STATE = 'IDLE'
85            print("FERMATO. In attesa di altri 5 messaggi.\n")
86
87    # --- 3. Emergency Button ---
88    if alvik.get_touch_up():
89        while alvik.get_touch_up():
90            delay(50)
91        alvik.set_wheels_speed(0, 0)
92        ROBOT_STATE = 'IDLE'
93        MESSAGES_COUNT = 0
94        print("RESET FORZATO")
95
96    delay(10)  # Smooth loop
97
98# === CLEANUP ===
99def cleanup():
100    alvik.stop()
101    alvik.set_wheels_speed(0, 0)
102    if e: e.active(False)
103    if w: w.active(False)
104    print("Cleanup completato.")
105
106# === START ===
107try:
108    start(setup, loop, cleanup)
109except Exception as err:
110    print("ERRORE:", err)
111    cleanup()

1#include <esp_now.h>
2#include <WiFi.h>
3#include <esp_wifi.h> // Added Library: Necessary for the correct callback function
4
5// --- MAC ADDRESSES ---
6// Alvik's MAC Address (the receiver).
7// Your Alvik MAC is: 3C:84:27:C3:3A:C0
8uint8_t alvikAddress[] = {0x3C, 0x84, 0x27, 0xC3, 0x3A, 0xC0};
9
10// Structure for the data to be sent (data type to be exchanged)
11typedef struct struct_message {
12    char a[32];
13} struct_message;
14
15struct_message myData;
16
17// Function called AFTER a message has been sent
18// The signature has been corrected to support new core versions (wifi_tx_info_t)
19void OnDataSent(const wifi_tx_info_t *tx_info, esp_now_send_status_t status) {
20    Serial.print("\r\nStato Ultimo Invio Pacchetto: ");
21    
22    // Prints the sending status
23    if (status == ESP_NOW_SEND_SUCCESS) {
24        Serial.println("SUCCESS: Messaggio inviato ad Alvik.");
25    } else {
26        Serial.println("FALLITO: Errore nell'invio del messaggio!");
27    }
28}
29
30void setup() {
31    Serial.begin(115200);
32    delay(1000); // Wait one second to stabilize the serial port
33    
34    Serial.println("--- Avvio Camera ESP-NOW ---");
35
36    // Sets the WiFi mode to Station (necessary for ESP-NOW)
37    WiFi.mode(WIFI_STA);
38
39    // Initializes ESP-NOW
40    if (esp_now_init() != ESP_OK) {
41        Serial.println("ERRORE: Impossibile inizializzare ESP-NOW");
42        return;
43    }
44    
45    // Registers the correct callback function
46    esp_now_register_send_cb(OnDataSent);
47    
48    // --- Register Alvik as a Peer ---
49    esp_now_peer_info_t peerInfo;
50    // Copies Alvik's MAC address into the peer structure
51    memcpy(peerInfo.peer_addr, alvikAddress, 6);
52    peerInfo.channel = 0;   
53    peerInfo.encrypt = false; // We are not using encryption for this test
54    
55    // Adds the peer (Alvik)
56    if (esp_now_add_peer(&peerInfo) != ESP_OK){
57        Serial.println("ERRORE: Impossibile aggiungere il peer Alvik.");
58        return;
59    }
60    Serial.println("Peer Alvik (3C:84:27:C3:3A:C0) aggiunto con successo.");
61}
62
63void loop() {
64    // 1. Prepare data to send
65    strcpy(myData.a, "Immagine Pronta!");
66    
67    // 2. Send data to Alvik (peer)
68    esp_err_t result = esp_now_send(alvikAddress, (uint8_t *) &myData, sizeof(myData));
69        
70    if (result == ESP_OK) {
71        Serial.println("Avvio Invio: 'Immagine Pronta!'");
72    } else {
73        Serial.println("Avvio Invio: Errore preliminare, controllo lo stato...");
74    }
75    
76    delay(5000); // Sends a message every 5 seconds
77}