Acceso con Seguridad- Implementar al codigo acceso RFID

Hola compañeros he conseguido llegar hasta el siguiente codigo, que lo que hace es meter una clave, que se visualice en la pantalla y que si es correcta mueva un servo 180º, y si es incorrecto que pite eun buzzer que he colocado. El caso es añadirle el rfid, tengo las librerias y he extraido los codigos de la tarjeta y el tag, pero soy incapaz de implementarlo para que haga lo mismo, que si la contraseña guardada es correcta al pasar la tarjeta mueva el servo, si no es correcto el altavoz pitara…
Os dejo el codigo que tengo, sin lo del rfid, pues no se me ocurre manera alguna y he acabado por borrarlo por que tampoco entiendo mucho

/*Pantalla Nokia 5110*
 * #PINES#
 *    Pin 7-->SCLK
 *    Pin 6-->Din
 *    Pin 5-->DC
 *    Pin 4-->CE
 *    Pin 3-->RST
 *    Pin 5V-->Vcc
 *    Pin 3,3V-->BL
 *    Pin GND-->GND
 */
/*#LIBRERIAS#*/
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h>
/*Teclado*
 * #PINES#
 *    Pin 22-->Fila 1
 *    Pin 24-->Fila 2
 *    Pin 26-->Fila 3
 *    Pin 28-->Fila 1
 *    Pin 30-->Columna 1
 *    Pin 32-->Columna 2
 *    Pin 34-->Columna 3
 */
/*#LIBRERIAS#*/
#include <Keypad.h>

/*Defines*/
#define altavoz 2 //altavoz definido en el pin 2

/*ServoMotor 180º*/
#include <Servo.h>

/*Tiempo*/
#include <TimerOne.h>

/*Variables*/
Adafruit_PCD8544 display = Adafruit_PCD8544(7,6,5,4,3);

const byte ROWS = 4; //four rows
const byte COLS = 3; //three columns
char keys[ROWS][COLS] =
{
  {'1','2','3'},
  {'4','5','6'},
  {'7','8','9'},
  {'*','0','#'}
};
byte rowPins[ROWS] = {22, 24, 26, 28}; //connect to the row pinouts of the keypad
byte colPins[COLS] = {30, 32, 34}; //connect to the column pinouts of the keypad

Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );

String codigo;
String pass="0000";
byte caracteres = 0 ;
Servo puerta;
volatile unsigned long decimas;

void setup()
{
/*INICIO PUERTO SERIE*/
  Serial.begin(9600);

/*INICIO,CONTRASTE,LIMPIEZA LCD*/
  display.begin();
  display.setContrast(60);
  display.clearDisplay();
  display.display();

/*SERVOMOTOR*/
  puerta.attach(12);
  puerta.write(0);

/*INTERUPCIONES*/
  Timer1.initialize (100000);
  Timer1.attachInterrupt (inicio);
}

void loop()
{
char key = keypad.getKey();

/*Muestra lo que se pulsa en el teclado en la pantalla*/  
  if (key)
  {
    Serial.print(caracteres);
    Serial.print("-");
    display.clearDisplay();
    codigo.concat(key);
    display.print(codigo);
    caracteres = codigo.length();
    Serial.println(key);
    display.display();
  }
  
  switch (caracteres)
  {
  case 0:
  display.clearDisplay();
  display.setCursor(14,0);
  display.println("Introduzca");
  display.setCursor(36,10);
  display.println("La");
  display.setCursor(25,20);
  display.println("Clave");
  display.setCursor(27,35);
  display.println("****");
  display.display();
  break;
  
  case 4: 
  if (codigo == pass) 
  {
    puerta.write(180);
    
    if (decimas>50)
    {
      codigo= "****";
      caracteres = 0;
      puerta.write(0);
      decimas=0;
    }
    
    display.clearDisplay();
    display.print("clave correcta");
    display.display();
    
    
  }
  else
  {
    /*ALTAVOZ*/
    tone (altavoz, 250, 200);
    
    display.clearDisplay();
    display.print("clave incorrecta");
    display.display();
    
    codigo= "";
   }
  break;
 }
}

void inicio()
{
  if (caracteres==4)
  {
    decimas++;
  }
}