Duende Servo Control

#include <Servo.h>

Servo cabezaServo;

const int PIN_SERVO = 9;
unsigned long tiempoUltimaAccion = 0;
const long INTERVALO_VIDA = 6000; 

enum Estado { CENTRO, VIDA, MODO_NO };
Estado estadoActual = VIDA; 

void setup() {
  Serial.begin(9600);
  cabezaServo.attach(PIN_SERVO);
  cabezaServo.write(90); 
  delay(500);
  tiempoUltimaAccion = millis();
  Serial.println("Duende listo. Modo VIDA (Exagerado 60 deg) activo.");
}

// He mejorado el movimiento NO para que sea fluido pero rápido
void movimientoNo() {
  cabezaServo.write(40);  delay(250); // Un poco más rápido que el natural
  cabezaServo.write(140); delay(250);
}

void movimientoNatural() {
  // RANGO EXAGERADO: 90 +/- 60 grados (va de 30 a 150)
  int randomOffset = random(-60, 61); 
  int objetivo = 90 + randomOffset;
  
  int posActual = cabezaServo.read();
  
  // IDA: Hacia el punto aleatorio
  if (posActual < objetivo) {
    for (int p = posActual; p <= objetivo; p++) {
      cabezaServo.write(p);
      delay(20); // Un poco más ágil para cubrir los 60 grados
    }
  } else {
    for (int p = posActual; p >= objetivo; p--) {
      cabezaServo.write(p);
      delay(20);
    }
  }
  
  delay(1000); // Se queda observando
  
  // VUELTA: Regreso al centro obligatorio
  posActual = cabezaServo.read();
  if (posActual < 90) {
    for (int p = posActual; p <= 90; p++) {
      cabezaServo.write(p);
      delay(20);
    }
  } else {
    for (int p = posActual; p >= 90; p--) {
      cabezaServo.write(p);
      delay(20);
    }
  }
  
  tiempoUltimaAccion = millis(); 
}

void loop() {
  if (Serial.available() > 0) {
    String comando = Serial.readStringUntil('\n');
    comando.trim();
    comando.toLowerCase();

    if (comando == "vida") {
      estadoActual = VIDA;
      Serial.println("-> Modo VIDA.");
      tiempoUltimaAccion = millis();
    } 
    else if (comando == "no") {
      estadoActual = MODO_NO;
      Serial.println("-> Modo NO.");
    } 
    else if (comando == "centro") {
      estadoActual = CENTRO;
      // Regreso suave al centro antes de apagar
      int pAct = cabezaServo.read();
      if(pAct < 90) { for(int i=pAct; i<=90; i++) { cabezaServo.write(i); delay(15); } }
      else { for(int i=pAct; i>=90; i--) { cabezaServo.write(i); delay(15); } }
      Serial.println("-> Modo CENTRO.");
    }
  }

  switch (estadoActual) {
    case VIDA:
      if (millis() - tiempoUltimaAccion > INTERVALO_VIDA) {
        movimientoNatural();
      }
      break;
    case MODO_NO:
      movimientoNo();
      break;
    case CENTRO:
      break;
  }
}