ArduinoCode.ino

/*
Voice Controlled Robotic Lamp - Arduino Sketch Reference
========================================================
This is your existing Arduino sketch that should be uploaded to your Arduino.
Place this file in your Arduino IDE and upload to your board.

Required Libraries:
- Adafruit_GFX
- Adafruit_SSD1306  
- CuteBuzzerSounds
- FluxGarage_RoboEyes
- ESP32Servo
- FreeRTOS (ESP32 only)
*/

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <CuteBuzzerSounds.h>
#include <ESP32Servo.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Include RoboEyes AFTER declaring global 'display' so the header can use it
#include <FluxGarage_RoboEyes.h>

// RoboEyes instance - pass display object to constructor
RoboEyes<Adafruit_SSD1306> eyes(display);
bool f=true;

// Servo setup for ears
Servo servo1; // Left ear
Servo servo2; // Right ear (mirror)
const int servoPin1 = 18;
const int servoPin2 = 19;

// Ground pin
const int groundPin = 35;

// Emotion reset timer
unsigned long lastEmotionTime = 0;
const unsigned long EMOTION_RESET_DELAY = 5000; // 5 seconds in milliseconds
bool hasActiveEmotion = false;

// Sleep state tracking
bool isSleeping = false;

// Listen state tracking
bool isListening = false;

// Cyclops positioning delay tracking
bool cyclopsPositionPending = false;
unsigned long cyclopsPositionTime = 0;

// Buzzer setup
#ifndef BUZZER_PIN
#define BUZZER_PIN 25
#endif

// Rick Roll Song Setup
#define a3f    208     // 208 Hz
#define b3f    233     // 233 Hz
#define b3     247     // 247 Hz
#define c4     261     // 261 Hz MIDDLE C
#define c4s    277     // 277 Hz
#define e4f    311     // 311 Hz    
#define f4     349     // 349 Hz 
#define a4f    415     // 415 Hz  
#define b4f    466     // 466 Hz 
#define b4     493     // 493 Hz 
#define c5     523     // 523 Hz 
#define c5s    554     // 554 Hz
#define e5f    622     // 622 Hz  
#define f5     698     // 698 Hz 
#define f5s    740     // 740 Hz
#define a5f    831     // 831 Hz 
#define rest   -1

volatile int beatlength = 100; // determines tempo
float beatseparationconstant = 0.3;

// Song state variables
bool songPlaying = false;
bool songFlag = false;
int songPart = 4; // Start at verse
int songIndex = 0;
int lyricIndex = 0;

// Song data arrays
int song1_intro_melody[] = {c5s, e5f, e5f, f5, a5f, f5s, f5, e5f, c5s, e5f, rest, a4f, a4f};
int song1_intro_rhythmn[] = {6, 10, 6, 6, 1, 1, 1, 1, 6, 10, 4, 2, 10};

int song1_verse1_melody[] = {
  rest, c4s, c4s, c4s, c4s, e4f, rest, c4, b3f, a3f,
  rest, b3f, b3f, c4, c4s, a3f, a4f, a4f, e4f,
  rest, b3f, b3f, c4, c4s, b3f, c4s, e4f, rest, c4, b3f, b3f, a3f,
  rest, b3f, b3f, c4, c4s, a3f, a3f, e4f, e4f, e4f, f4, e4f,
  c4s, e4f, f4, c4s, e4f, e4f, e4f, f4, e4f, a3f, rest,
  b3f, c4, c4s, a3f, rest, e4f, f4, e4f
};

int song1_verse1_rhythmn[] = {
  2, 1, 1, 1, 1, 2, 1, 1, 1, 5,
  1, 1, 1, 1, 3, 1, 2, 1, 5,
  1, 1, 1, 1, 1, 1, 1, 2, 1, 1, 1, 1, 3,
  1, 1, 1, 1, 2, 1, 1, 1, 1, 1, 1, 4,
  5, 1, 1, 1, 1, 1, 1, 1, 2, 2,
  2, 1, 1, 1, 3, 1, 1, 1, 3
};

int song1_chorus_melody[] = {
  b4f, b4f, a4f, a4f, f5, f5, e5f, b4f, b4f, a4f, a4f, e5f, e5f, c5s, c5, b4f,
  c5s, c5s, c5s, c5s, c5s, e5f, c5, b4f, a4f, a4f, a4f, e5f, c5s,
  b4f, b4f, a4f, a4f, f5, f5, e5f, b4f, b4f, a4f, a4f, a5f, c5, c5s, c5, b4f,
  c5s, c5s, c5s, c5s, c5s, e5f, c5, b4f, a4f, rest, a4f, e5f, c5s, rest
};

int song1_chorus_rhythmn[] = {
  1, 1, 1, 1, 3, 3, 6, 1, 1, 1, 1, 3, 3, 3, 1, 2,
  1, 1, 1, 1, 3, 3, 3, 1, 2, 2, 2, 4, 8,
  1, 1, 1, 1, 3, 3, 6, 1, 1, 1, 1, 3, 3, 3, 1, 2,
  1, 1, 1, 1, 3, 3, 3, 1, 2, 2, 2, 4, 8, 4
};

// RTOS task handles and queue
TaskHandle_t eyesTaskHandle = NULL;
TaskHandle_t soundTaskHandle = NULL;
TaskHandle_t servoTaskHandle = NULL;
QueueHandle_t soundQueue = NULL;
QueueHandle_t servoQueue = NULL;

// Task priorities
#define EYES_TASK_PRIORITY 2
#define SOUND_TASK_PRIORITY 1
#define SERVO_TASK_PRIORITY 3

// Task stack sizes
#define EYES_TASK_STACK_SIZE 4096
#define SOUND_TASK_STACK_SIZE 2048
#define SERVO_TASK_STACK_SIZE 3072

// Task delay
#define EYES_TASK_DELAY 16  // ~60 FPS (1000ms / 60fps ≈ 16ms)

#define RELAY_PIN 23

// Eyes update task
void eyesUpdateTask(void *parameter) {
  while (true) {
    eyes.update();
    vTaskDelay(pdMS_TO_TICKS(EYES_TASK_DELAY));
  }
}

// Sound playing task
void soundTask(void *parameter) {
  int soundToPlay;
  
  while (true) {
    if (xQueueReceive(soundQueue, &soundToPlay, portMAX_DELAY) == pdTRUE) {
      cute.play(soundToPlay);
    }
  }
}

// Servo animation task
void servoTask(void *parameter) {
  int animationType;
  
  while (true) {
    if (xQueueReceive(servoQueue, &animationType, portMAX_DELAY) == pdTRUE) {
      switch (animationType) {
        case 1: // happy
          animHappyNonBlocking();
          break;
        case 2: // sad
          animSadNonBlocking();
          break;
        case 3: // angry
          animAngryNonBlocking();
          break;
        case 4: // scared
          animScaredNonBlocking();
          break;
        case 5: // confused
          animConfusedNonBlocking();
          break;
        case 6: // laughing
          animLaughingNonBlocking();
          break;
        case 7: // song animation
          animSongNonBlocking();
          break;
        case 0: // neutral
          setEars(90);
          break;
      }
    }
  }
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22); // SDA, SCL
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 init failed");
    for(;;);
  }
  display.clearDisplay();

  // Initialize CuteBuzzerSounds (global singleton 'cute')
  cute.init(BUZZER_PIN);

  // Initialize servo ears
  servo1.attach(servoPin1, 500, 2400);
  servo2.attach(servoPin2, 500, 2400);
  setEars(90); // neutral position
  Serial.println("Servo ears initialized");
  
  // Initialize ground pin
  pinMode(groundPin, OUTPUT);
  digitalWrite(groundPin, LOW);
  Serial.println("Ground pin D35 initialized");
  
  // Initialize relay pin
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, HIGH); // Start with LED OFF (HIGH = OFF for low-level trigger relay)
  Serial.println("Relay initialized - LED OFF");

  // Initialize RoboEyes
  const int EYES_DRAW_WIDTH = SCREEN_WIDTH;
  eyes.begin(EYES_DRAW_WIDTH, SCREEN_HEIGHT, 60); // 60 FPS cap
  eyes.setDisplayColors(0, 1); // monochrome: background=0, main=1
  eyes.setWidth(48, 48);
  eyes.setHeight(40, 40);
  eyes.setSpacebetween(20);
  eyes.setBorderradius(12, 12);
  eyes.setPosition(DEFAULT);
  eyes.setAutoblinker(true, 3, 2); // blink ~every 5s +/-2s

  // Create sound queue
  soundQueue = xQueueCreate(10, sizeof(int));
  if (soundQueue == NULL) {
    Serial.println("Failed to create sound queue!");
    for(;;);
  }

  // Create servo queue
  servoQueue = xQueueCreate(5, sizeof(int));
  if (servoQueue == NULL) {
    Serial.println("Failed to create servo queue!");
    for(;;);
  }

  // Create eyes update task
  xTaskCreatePinnedToCore(
    eyesUpdateTask,           // Task function
    "EyesUpdate",             // Task name
    EYES_TASK_STACK_SIZE,     // Stack size
    NULL,                     // Task parameters
    EYES_TASK_PRIORITY,       // Task priority
    &eyesTaskHandle,          // Task handle
    0                         // Core to run on (core 0)
  );

  // Create sound task
  xTaskCreatePinnedToCore(
    soundTask,                // Task function
    "SoundTask",              // Task name
    SOUND_TASK_STACK_SIZE,    // Stack size
    NULL,                     // Task parameters
    SOUND_TASK_PRIORITY,      // Task priority
    &soundTaskHandle,         // Task handle
    1                         // Core to run on (core 1)
  );

  // Create servo task
  xTaskCreatePinnedToCore(
    servoTask,                // Task function
    "ServoTask",              // Task name
    SERVO_TASK_STACK_SIZE,    // Stack size
    NULL,                     // Task parameters
    SERVO_TASK_PRIORITY,      // Task priority
    &servoTaskHandle,         // Task handle
    1                         // Core to run on (core 1)
  );

  Serial.println("Voice Controlled Robotic Lamp Ready!");
  Serial.println("Waiting for voice commands from Python script...");
}

// Function to set emotion and start reset timer
void setEmotionWithTimer(String emotionCmd, int soundToPlay) {
  int servoAnimation = 0; // Default to neutral
  
  if (emotionCmd == "happy") {
    eyes.setMood(HAPPY);
    servoAnimation = 1; // happy
    hasActiveEmotion = true;
  } else if (emotionCmd == "sad") {
    eyes.setMood(TIRED);
    servoAnimation = 2; // sad
    hasActiveEmotion = true;
  } else if (emotionCmd == "angry") {
    eyes.setMood(ANGRY);
    servoAnimation = 3; // angry
    hasActiveEmotion = true;
  } else if (emotionCmd == "scared") {
    eyes.anim_confused();
    servoAnimation = 4; // scared
    hasActiveEmotion = true;
  } else if (emotionCmd == "confused") {
    eyes.anim_confused();
    servoAnimation = 5; // confused
    hasActiveEmotion = true;
  } else if (emotionCmd == "laughing") {
    eyes.anim_laugh();
    servoAnimation = 6; // laughing
    hasActiveEmotion = true;
  } else {
    eyes.setMood(DEFAULT);
    servoAnimation = 0; // neutral
    hasActiveEmotion = false;
  }
  
  // Start timer for emotion reset
  if (hasActiveEmotion) {
    lastEmotionTime = millis();
  }
  
  // Send servo animation to queue (non-blocking)
  xQueueSend(servoQueue, &servoAnimation, 0);
  
  // Play sound if provided (immediate, non-blocking)
  if (soundToPlay != 0) {
    xQueueSend(soundQueue, &soundToPlay, 0);
  }
}

// Function to check and reset emotion if needed
void checkEmotionReset() {
  if (hasActiveEmotion && !isSleeping && !isListening && (millis() - lastEmotionTime >= EMOTION_RESET_DELAY)) {
    eyes.setMood(DEFAULT);
    int servoAnimation = 0; // neutral
    xQueueSend(servoQueue, &servoAnimation, 0); // Reset ears to neutral
    hasActiveEmotion = false;
    Serial.println("EMOTION RESET TO DEFAULT");
  }
}

// === RELAY CONTROL FUNCTIONS ===
void turnLightOn() {
  digitalWrite(RELAY_PIN, LOW);  // LOW = ON for low-level trigger relay
  Serial.println("LED ON");
}

void turnLightOff() {
  digitalWrite(RELAY_PIN, HIGH); // HIGH = OFF for low-level trigger relay
  Serial.println("LED OFF");
}

// === SERVO EAR ANIMATIONS ===
void setEars(int angle) {
  angle = constrain(angle, 0, 180);
  servo1.write(angle);
  servo2.write(180 - angle);  // mirror movement
}

// Non-blocking servo animations using vTaskDelay
void animHappyNonBlocking() {
  for (int i = 70; i <= 110; i += 5) {
    setEars(i);
    vTaskDelay(pdMS_TO_TICKS(60));
  }
  for (int i = 110; i >= 70; i -= 5) {
    setEars(i);
    vTaskDelay(pdMS_TO_TICKS(60));
  }
  setEars(90);
}

void animSadNonBlocking() {
  setEars(120);
  vTaskDelay(pdMS_TO_TICKS(500));
  setEars(130);
  vTaskDelay(pdMS_TO_TICKS(500));
  setEars(120);
}

// Sleep function - close eyes and droop ears to maximum
void enterSleepMode() {
  isSleeping = true;
  eyes.close(); // Close both eyes
  eyes.setAutoblinker(false, 0, 0); // Disable auto-blinking
  setEars(150); // Maximum droop position (reference from scared function)
  hasActiveEmotion = false; // Clear any active emotions
  Serial.println("SLEEP MODE - Eyes closed, ears drooped");
}

// Wake function - open eyes and reset ears to neutral
void exitSleepMode() {
  isSleeping = false;
  eyes.open(); // Open both eyes
  eyes.setAutoblinker(true, 3, 2); // Re-enable auto-blinking
  setEars(90); // Neutral ear position
  eyes.setMood(DEFAULT); // Reset to default mood
  Serial.println("WAKE MODE - Eyes open, ears neutral");
}

// Listen mode - perk one ear forward and set happy eyes
void enterListenMode() {
  isListening = true;
  eyes.setMood(HAPPY);
  servo1.write(45); // Left ear perked forward
  servo2.write(135); // Right ear stays back (mirrored)
  hasActiveEmotion = false; // Don't auto-reset from listen mode
  Serial.println("LISTEN MODE - Ear perked, happy eyes");
}

// Exit listen mode - reset to default
void exitListenMode() {
  isListening = false;
  eyes.setMood(DEFAULT);
  setEars(90); // Reset both ears to neutral
  Serial.println("LISTEN MODE OFF - Reset to default");
}

// Stop all actions - halt emotions, sounds, and servo movements
void stopAllActions() {
  // Stop song if playing (must be before stopping buzzer)
  if (songPlaying) {
    stopSong();
  }
  
  // Stop any playing sounds - both tone() and cute sounds
  noTone(BUZZER_PIN);
  cute.stop();  // Stop CuteBuzzerSounds library sounds
  
  // Clear sound queue
  xQueueReset(soundQueue);
  
  // Reset eyes to default
  eyes.setMood(DEFAULT);
  
  // Reset servos to neutral
  setEars(90);
  
  // Clear emotion state
  hasActiveEmotion = false;
  
  Serial.println("STOP - All actions halted, reset to default");
}

void animAngryNonBlocking() {
  for (int i = 70; i >= 50; i -= 5) {
    setEars(i);
    vTaskDelay(pdMS_TO_TICKS(40));
  }
  for (int j = 0; j < 2; j++) {
    setEars(55);
    vTaskDelay(pdMS_TO_TICKS(100));
    setEars(65);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
  setEars(90);
}

void animScaredNonBlocking() {
  setEars(150);
  vTaskDelay(pdMS_TO_TICKS(200));
  setEars(100);
  vTaskDelay(pdMS_TO_TICKS(200));
  setEars(150);
  vTaskDelay(pdMS_TO_TICKS(200));
  setEars(90);
}

void animConfusedNonBlocking() {
  // Exaggerated opposite ear motion
  for (int i = 0; i < 2; i++) {
    servo1.write(60);   // Left ear far down
    servo2.write(60);  // Right ear far up
    vTaskDelay(pdMS_TO_TICKS(500));

    servo1.write(120);  // Left ear far up
    servo2.write(120);   // Right ear far down
    vTaskDelay(pdMS_TO_TICKS(500));
  }
  // Return to neutral (mirrored)
  setEars(90);
}

void animLaughingNonBlocking() {
  for (int k = 0; k < 3; k++) {
    setEars(75);
    vTaskDelay(pdMS_TO_TICKS(150));
    setEars(105);
    vTaskDelay(pdMS_TO_TICKS(150));
  }
  setEars(90);
}

// Rick Roll Song Functions
void startSong() {
  songPlaying = true;
  songFlag = true;
  songPart = 1; // Start with intro
  songIndex = 0;
  lyricIndex = 0;
  
  // Set eyes to happy and start servo animation
  eyes.setMood(HAPPY);
  int songAnimation = 7; // Song animation type
  xQueueSend(servoQueue, &songAnimation, portMAX_DELAY);
  
  Serial.println("🎵 Rick Roll song started!");
}

void stopSong() {
  songPlaying = false;
  songFlag = false;
  songPart = 4; // Reset to verse
  songIndex = 0;
  lyricIndex = 0;
  
  // Stop buzzer and reset servos
  noTone(BUZZER_PIN);
  setEars(90);
  eyes.setPosition(DEFAULT);
  
  Serial.println("🛑 Rick Roll song stopped!");
}

void playSong() {
  if (!songPlaying) return;
  
  int notelength;
  
  if (songPart == 1 || songPart == 2) {
    // intro
    notelength = beatlength * song1_intro_rhythmn[songIndex];
    if (song1_intro_melody[songIndex] > 0) {
      tone(BUZZER_PIN, song1_intro_melody[songIndex], notelength);
    }
    songIndex++;
    if (songIndex >= sizeof(song1_intro_melody) / sizeof(int)) {
      songPart++;
      songIndex = 0;
      lyricIndex = 0;
    }
  }
  else if (songPart == 3 || songPart == 5) {
    // verse
    notelength = beatlength * 2 * song1_verse1_rhythmn[songIndex];
    if (song1_verse1_melody[songIndex] > 0) {
      tone(BUZZER_PIN, song1_verse1_melody[songIndex], notelength);
    }
    songIndex++;
    if (songIndex >= sizeof(song1_verse1_melody) / sizeof(int)) {
      songPart++;
      songIndex = 0;
      lyricIndex = 0;
    }
  }
  else if (songPart == 4 || songPart == 6) {
    // chorus
    notelength = beatlength * song1_chorus_rhythmn[songIndex];
    if (song1_chorus_melody[songIndex] > 0) {
      tone(BUZZER_PIN, song1_chorus_melody[songIndex], notelength);
    }
    songIndex++;
    if (songIndex >= sizeof(song1_chorus_rhythmn) / sizeof(int)) {
      songPart++;
      songIndex = 0;
      lyricIndex = 0;
    }
  }
  
  delay(notelength);
  noTone(BUZZER_PIN);
  delay(notelength * beatseparationconstant);
  
  // Song finished - stop playing
  if (songPart == 7) {
    stopSong();
  }
}

void animSongNonBlocking() {
  // Happy ear movement during song - slower and with flipped direction
  while (songPlaying) {
    // Move servos in opposite directions for more dynamic movement
    servo1.write(75);   // Left ear down
    servo2.write(755);  // Right ear up (flipped direction)
    vTaskDelay(pdMS_TO_TICKS(400)); // Slower movement (doubled delay)
    
    servo1.write(105);  // Left ear up
    servo2.write(105);   // Right ear down (flipped direction)
    vTaskDelay(pdMS_TO_TICKS(400)); // Slower movement (doubled delay)
  }
  setEars(90);
}

// Parse compound command structure: #idle#position#curiosity#emotion#
// Example: #off#N#on#happy#
void parseCompoundCommand(String cmd) {
  if (!cmd.startsWith("#") || !cmd.endsWith("#")) {
    Serial.println("ERROR: Invalid command format");
    return;
  }
  
  // Remove leading and trailing #
  cmd = cmd.substring(1, cmd.length() - 1);
  
  // Split by # delimiter
  String segments[4];
  int partIndex = 0;
  int startIndex = 0;
  
  for (int i = 0; i <= cmd.length(); i++) {
    if (i == cmd.length() || cmd.charAt(i) == '#') {
      if (partIndex < 4) {
        segments[partIndex] = cmd.substring(startIndex, i);
        partIndex++;
      }
      startIndex = i + 1;
    }
  }
  
  if (partIndex != 4) {
    Serial.println("ERROR: Expected 4 command segments");
    return;
  }
  
  String idleCmd = segments[0];
  String positionCmd = segments[1]; 
  String curiosityCmd = segments[2];
  String emotionCmd = segments[3];
  
  int soundToPlay = 0;
  
  // Process idle command
  if (idleCmd == "on") {
    eyes.setIdleMode(true, 6, 3);
    Serial.println("IDLE ON");
  } else if (idleCmd == "off") {
    eyes.setIdleMode(false, 0, 0);
    Serial.println("IDLE OFF");
  }
  
  // Process position command
  if (positionCmd == "KEEP") {
    // Don't change position - leave it as is (voice command)
    Serial.println("POSITION KEPT (voice command)");
  }
  else if (positionCmd == "N") eyes.setPosition(N);
  else if (positionCmd == "NE") eyes.setPosition(NE);
  else if (positionCmd == "E") eyes.setPosition(E);
  else if (positionCmd == "SE") eyes.setPosition(SE);
  else if (positionCmd == "S") eyes.setPosition(S);
  else if (positionCmd == "SW") eyes.setPosition(SW);
  else if (positionCmd == "W") eyes.setPosition(W);
  else if (positionCmd == "NW") eyes.setPosition(NW);
  else eyes.setPosition(DEFAULT);
  
  if (positionCmd != "KEEP") {
    Serial.print("POS ");
    Serial.println(positionCmd);
  }
  
  // Process curiosity command
  if (curiosityCmd == "on") {
    eyes.setCuriosity(true);
    soundToPlay = S_CUDDLY;
    Serial.println("CURIOSITY ON");
  } else if (curiosityCmd == "off") {
    eyes.setCuriosity(false);
    Serial.println("CURIOSITY OFF");
  }
  
  // Process emotion command with timer
  if (emotionCmd == "happy") {
    soundToPlay = S_SUPER_HAPPY;
    Serial.println("HAPPY");
  } else if (emotionCmd == "sad") {
    soundToPlay = S_SAD;
    Serial.println("SAD");
  } else if (emotionCmd == "angry") {
    soundToPlay = S_MODE3;
    Serial.println("ANGRY");
  } else if (emotionCmd == "scared") {
    soundToPlay = S_SURPRISE;
    Serial.println("SCARED");
  } else if (emotionCmd == "confused") {
    soundToPlay = S_CONFUSED;
    Serial.println("CONFUSED");
  } else if (emotionCmd == "laughing") {
    soundToPlay = S_HAPPY;
    Serial.println("LAUGHING");
  } else {
    Serial.println("DEFAULT MOOD");
  }
  
  // Set emotion with auto-reset timer
  setEmotionWithTimer(emotionCmd, soundToPlay);
  
  Serial.println("COMMAND PROCESSED");
}

void loop() {
  String cmd="";
  int s=0;
  
  // Check for emotion auto-reset
  checkEmotionReset();
  
  // Check for cyclops positioning delay
  if (cyclopsPositionPending && (millis() - cyclopsPositionTime >= 100)) {
    // Try multiple approaches to center the cyclops eye
    eyes.setSpacebetween(0); // Reset spacing to 0
    eyes.setPosition(DEFAULT); // Center the single eye
    eyes.drawEyes(); // Force immediate update
    cyclopsPositionPending = false;
    Serial.println("Single eye centered");
  }
  
  if(f) {
    eyes.setPosition(DEFAULT);
    f=false;
  }

  if (Serial.available()) {
    cmd = Serial.readStringUntil('\n');
    cmd.trim();
    
    // Check if it's a position command or light command (!!COMMAND!!)
    if (cmd.startsWith("!!") && cmd.endsWith("!!")) {
      String command = cmd.substring(2, cmd.length() - 2);
      command.toUpperCase();
      
      // Handle light control commands
      if (command == "LIGHTON") {
        turnLightOn();
      }
      else if (command == "LIGHTOFF") {
        turnLightOff();
      }
      // Handle song commands
      else if (command == "STARTSONG") {
        startSong();
        
      }
      else if (command == "STOPSONG") {
        stopSong();
      }
      // Handle listen mode commands
      else if (command == "STARTLISTEN") {
        enterListenMode();
      }
      else if (command == "STOPLISTEN") {
        exitListenMode();
      }
      // Handle stop command
      else if (command == "STOP") {
        stopAllActions();
      }
      // Handle position commands
      else if (command == "N") eyes.setPosition(N);
      else if (command == "NE") eyes.setPosition(NE);
      else if (command == "E") eyes.setPosition(E);
      else if (command == "SE") eyes.setPosition(SE);
      else if (command == "S") eyes.setPosition(S);
      else if (command == "SW") eyes.setPosition(SW);
      else if (command == "W") eyes.setPosition(W);
      else if (command == "NW") eyes.setPosition(NW);
      else {
        eyes.setPosition(DEFAULT);
        Serial.print("POSITION SET: "); Serial.println(command);
      }
      
      // Only print position message for actual position commands
      if (command == "N" || command == "NE" || command == "E" || command == "SE" || 
          command == "S" || command == "SW" || command == "W" || command == "NW") {
        Serial.print("POSITION SET: "); Serial.println(command);
      }
    }
    // Check if it's a compound command (starts and ends with #)
    else if (cmd.startsWith("#") && cmd.endsWith("#")) {
      parseCompoundCommand(cmd);
    } else {
      // Legacy single commands for backward compatibility
      
      // Emotion commands with auto-reset
      if (cmd == "happy") {
        setEmotionWithTimer("happy", S_SUPER_HAPPY);
        Serial.println("HAPPY");
      }
      else if (cmd == "sad") {
        setEmotionWithTimer("sad", S_SAD);
        Serial.println("SAD(TIRED)");
      }
      else if (cmd == "angry") {
        setEmotionWithTimer("angry", S_MODE3);
        Serial.println("ANGRY");
      }
      else if (cmd == "scared") {
        setEmotionWithTimer("scared", S_SURPRISE);
        Serial.println("SCARED(confused)");
      }
      else if (cmd == "confused") {
        setEmotionWithTimer("confused", S_CONFUSED);
        Serial.println("CONFUSED");
      }
      else if (cmd == "laughing") {
        setEmotionWithTimer("laughing", S_HAPPY);
        Serial.println("LAUGH");
      }
      
      // Sleep and wake commands
      else if (cmd == "sleep") {
        enterSleepMode();
      }
      else if (cmd == "wake") {
        exitSleepMode();
      }
      
      // Cyclops mode commands
      else if (cmd == "cyclops_on") {
        eyes.setCyclops(true);
        cyclopsPositionPending = true;
        cyclopsPositionTime = millis(); // Start non-blocking delay timer
        Serial.println("CYCLOPS MODE ON - Centering...");
      }
      else if (cmd == "cyclops_off") {
        eyes.setCyclops(false);
        eyes.setSpacebetween(20); // Reset to original spacing
        eyes.setPosition(DEFAULT); // Reset to default position
        Serial.println("CYCLOPS MODE OFF - Two eyes");
      }
      
      // Blink commands
      else if (cmd == "blink") {
        eyes.blink();
        s=(S_BUTTON_PUSHED);
        Serial.println("BLINK");
      }
      else if (cmd == "blinkleft") {
        eyes.blink(1,0);
        s=(S_BUTTON_PUSHED);
        Serial.println("BLINK LEFT");
      }
      else if (cmd == "blinkright") {
        eyes.blink(0,1);
        s=(S_BUTTON_PUSHED);
        Serial.println("BLINK RIGHT");
      }
      
      // Mode commands
      else if (cmd == "idle_on") {
        eyes.setIdleMode(true, 6, 3);
        s=(S_CONNECTION);
        Serial.println("IDLE ON");
      }
      else if (cmd == "idle_off") {
        eyes.setIdleMode(false, 0, 0);
        s=(S_DISCONNECTION);
        Serial.println("IDLE OFF");
      }
      else if (cmd == "curious_on") {
        eyes.setCuriosity(true);
        s=(S_CUDDLY);
        Serial.println("CURIOSITY ON");
      }
      else if (cmd == "curious_off") {
        eyes.setCuriosity(false);
        Serial.println("CURIOSITY OFF");
      }
      else if (cmd == "sweat_on") {
        eyes.setSweat(true);
        Serial.println("SWEAT ON");
      }
      else if (cmd == "sweat_off") {
        eyes.setSweat(false);
        Serial.println("SWEAT OFF");
      }
      
      // Flicker effects
      else if (cmd == "hflicker_on") {
        eyes.setHFlicker(true,2);
        Serial.println("H-FLICKER ON");
      }
      else if (cmd == "hflicker_off") {
        eyes.setHFlicker(false,2);
        Serial.println("H-FLICKER OFF");
      }
      else if (cmd == "vflicker_on") {
        eyes.setVFlicker(true,2);
        Serial.println("V-FLICKER ON");
      }
      else if (cmd == "vflicker_off") {
        eyes.setVFlicker(false,2);
        Serial.println("V-FLICKER OFF");
      }
      
      // Position commands
      else if (cmd.startsWith("pos ")) {
        String p = cmd.substring(4);
        p.toUpperCase();
        if (p == "N") eyes.setPosition(N);
        else if (p == "NE") eyes.setPosition(NE);
        else if (p == "E") eyes.setPosition(E);
        else if (p == "SE") eyes.setPosition(SE);
        else if (p == "S") eyes.setPosition(S);
        else if (p == "SW") eyes.setPosition(SW);
        else if (p == "W") eyes.setPosition(W);
        else if (p == "NW") eyes.setPosition(NW);
        else eyes.setPosition(DEFAULT);
        Serial.print("POS "); Serial.println(p);
      }
      
      else {
        eyes.setMood(DEFAULT);
        Serial.println("UNKNOWN COMMAND");
      }
      
      // Send sound to queue for non-emotion legacy commands
      if(cmd!="" && s!=0) {
        xQueueSend(soundQueue, &s, 0);
      }
    }
  }
  
  // Play song if active
  if (songPlaying) {
    playSong();
  }
  
  // Minimal delay for faster response
  vTaskDelay(pdMS_TO_TICKS(1));
}