Problema con sensor RC522 para alarma

Hola amigos, estoy haciendo un curso de arduino y en una practica me he quedado bloqueado y no se como resolverlo. La practica es hacer una alarma con rc522, un led RGB y un boton, al apretar el boton se active la alarma y si paso el llavero bueno se desactive la alarma, si paso el llavero malo que suene la alarma y al pasar el llavero bueno se desactive.

El problema que tengo es que cuando paso el llavero bueno no me desactiva la alarma a la primera sino que tengo que pasarlo una segunda vez y ya si la desactiva.

Os paso el codigo:

#include <SPI.h>
#include <MFRC522.h>

#define SS_PIN 10
#define RST_PIN 9


const int ledR = 3;
const int ledG = 5;
const int ledB = 6;
const int buzzer = 2;
const int botonPulsador = 4;

boolean estadoAlarma = false;
boolean estadoSonando = false;

int estadoActual = 0;
int estadoAnterior = 0;

MFRC522 rfid(SS_PIN, RST_PIN);

byte readCard[4];
byte accessCard[4] = {0x25, 0xB5, 0x23, 0x77};

void setup() {
Serial.begin(9600);
SPI.begin();
rfid.PCD_Init();
pinMode(ledR, OUTPUT);
pinMode(ledG, OUTPUT);
pinMode(ledB, OUTPUT);
pinMode(botonPulsador, INPUT_PULLUP);

Serial.println("-- Alarma DESACTIVADA --");
}

void loop() {

if (estadoAlarma && !estadoSonando){
  if (rfid.PICC_IsNewCardPresent()){
    if (!getUID()){
      setColorBuzzer(255, 0, 0, 600, 100, 100); // Luz roja y sonido agudo y rapido
      setColorBuzzer(255, 0, 0, 600, 100, 100);
      Serial.println("-- Intruso -- ¡ALARMA!");
      estadoSonando = true;
      delay(1);
    } else {
        estadoSonando = false;
    }
  }
}
else if (estadoSonando){
    if (!getUID()){
      setColorBuzzer(255, 0, 0, 600, 100, 100); // Luz roja y sonido agudo y rapido
      Serial.println("-- Alarma SONANDO !!!--");
      delay(1);
    } else {
      setColorBuzzer(0, 255, 0, 600, 100, 100); // Luz Verde y sonido agudo y rapido
      setColorBuzzer(0, 255, 0, 600, 100, 100);
      Serial.println("-- Alarma DESACTIVADA --");
      estadoAlarma = false;
      estadoSonando = false;
      delay(1);
    }
}
else if(!estadoAlarma){
  if (estadoBoton()== HIGH) {
    setColorBuzzer(255, 0, 0, 600, 100, 100); // Luz roja y sonido agudo y rapido
    setColorBuzzer(255, 0, 0, 600, 100, 100); 
    Serial.println("-- Alarma ACTIVADA --");
    estadoAlarma = true;
  }
}
}


boolean getUID(){
if (! rfid.PICC_IsNewCardPresent()){
  return false;
}
if (! rfid.PICC_ReadCardSerial()){
  return false;
}

for (int i = 0; i < 4; i++){
  readCard[i] = rfid.uid.uidByte[i];
  if (readCard[i] != accessCard[i]){
    estadoAlarma = true;
    return false;
  }
}

rfid.PICC_HaltA();
estadoAlarma = false;
return true;
}


int estadoBoton() {
estadoActual = !digitalRead(botonPulsador);

if(estadoActual == HIGH && estadoAnterior == LOW) { //Si el boton se ha pulsado y antes no lo estaba
  estadoAnterior = estadoActual;
  delay(1); 
  return HIGH;
} else {
    estadoAnterior = estadoActual;
    delay(10);
    return LOW;
}
}


int setColorBuzzer(int R, int G, int B, int freq, int pause, int pause2) {
analogWrite(ledR, R);
analogWrite(ledG, G);
analogWrite(ledB, B);
//tone(buzzer, freq);
delay(pause);
analogWrite(ledR, 0);
analogWrite(ledG, 0);
analogWrite(ledB, 0);
//noTone(buzzer);
delay(pause2);
}

¿Donde puede estar el error?

Saludos y enhorabuena por el foro.

Moderador: Por favor edita tu post usando etiquetas de código. Lee las Normas del foro

Perdon, ya está modificado el post.