Cómo hacer una caja con clave y servo

#include <Servo.h>

Servo miCierre;

// --- Configuración de Hardware ---
const int pinServo = 9;
const int pinLedVerde = 5;
const int pinLedRojo = 6;

// Botones para introducir la clave (Botón 1, Botón 2, Botón 3)
const int pinBotones[] = {2, 3, 4}; 
const int NUM_BOTONES = 3;

// --- Configuración de Seguridad ---
// Definimos la clave correcta: Botón 1, Botón 1, Botón 3, Botón 2
const int LONGITUD_CLAVE = 4;
const int CLAVE_MAESTRA[LONGITUD_CLAVE] = {0, 0, 2, 1}; 

// Variables de estado
int intentoUsuario[LONGITUD_CLAVE]; // Buffer para guardar lo que se pulsa
int indiceActual = 0;               // Cuántos números lleva pulsados

bool estaAbierta = false;

void setup() {
  Serial.begin(9600);

  // Configurar LEDs
  pinMode(pinLedRojo, OUTPUT);
  pinMode(pinLedVerde, OUTPUT);
  
  // Configurar Botones con PULLUP interno
  for(int i=0; i < NUM_BOTONES; i++){
    pinMode(pinBotones[i], INPUT_PULLUP);
  }

  // Configurar servo y adoptar el estado inicial cerrado
  miCierre.attach(pinServo);
  cerrarCaja();
  
  Serial.println("SISTEMA BLOQUEADO. Introduzca clave.");
}

void loop() {
  if (!estaAbierta) {
    // Solo leemos botones si la caja está cerrada
    leerBotones();
  } else {
    // Si está abierta, esperamos pulsar cualquier botón para cerrar
    if (detectarCualquierBoton()) {
      cerrarCaja();
    }
  }
}

void leerBotones() {
  for (int i = 0; i < NUM_BOTONES; i++) {
    // Leemos el botón (LOW significa pulsado en Input Pullup)
    if (digitalRead(pinBotones[i]) == LOW) {
      
      // Feedback: parpadeo rápido del LED amarillo/rojo al pulsar
      digitalWrite(pinLedRojo, HIGH); 
      delay(100); 
      digitalWrite(pinLedRojo, LOW);
      
      // Guardamos el botón pulsado en el array (0, 1 o 2)
      intentoUsuario[indiceActual] = i;
      indiceActual++;
      
      Serial.print("* "); // Imprimimos asterisco como seguridad
      
      // Esperamos a que suelte el botón (Antirebote simple)
      while(digitalRead(pinBotones[i]) == LOW) { delay(10); }

      // ¿Hemos llegado al final de la longitud de la clave?
      if (indiceActual == LONGITUD_CLAVE) {
        verificarClave();
      }
    }
  }
}

void verificarClave() {
  bool claveCorrecta = true;
  
  // Recorremos ambos arrays comparando posición a posición
  for (int i = 0; i < LONGITUD_CLAVE; i++) {
    if (intentoUsuario[i] != CLAVE_MAESTRA[i]) {
      claveCorrecta = false;
      break; // Si uno falla, ya no hace falta seguir mirando
    }
  }
  
  if (claveCorrecta) {
    abrirCaja();
  } else {
    errorClave();
  }
  
  // Reiniciamos el índice para el siguiente intento
  indiceActual = 0;
}

void abrirCaja() {
  Serial.println(" -> CLAVE CORRECTA. Abriendo...");
  digitalWrite(pinLedVerde, HIGH);
  miCierre.write(90); // Ángulo para desbloquear
  estaAbierta = true;
  delay(500); // Tiempo para que el servo llegue
}

void cerrarCaja() {
  Serial.println(" -> CERRANDO...");
  digitalWrite(pinLedVerde, LOW);
  miCierre.write(0); // Ángulo para bloquear
  estaAbierta = false;
  indiceActual = 0;  // Reseteamos cualquier intento a medias
  delay(500);
}

void errorClave() {
  Serial.println(" -> ERROR. Clave incorrecta.");
  // Parpadeo de error
  for(int i=0; i<3; i++){
    digitalWrite(pinLedRojo, HIGH);
    delay(100);
    digitalWrite(pinLedRojo, LOW);
    delay(100);
  }
}

// Función auxiliar para re-cerrar
bool detectarCualquierBoton() {
   for (int i = 0; i < NUM_BOTONES; i++) {
     if (digitalRead(pinBotones[i]) == LOW) {
       while(digitalRead(pinBotones[i]) == LOW) delay(10); // Esperar soltar
       return true;
     }
   }
   return false;
}