Hilfe bei Arduino Programmierung

Hallo,
ich hoffe mir kann einer helfen...
Ich möchte eine Box machen ...
1. es soll über ein Tastenfeld ein Code eingegeben werden... z.B 0815
2.ist der Code falsch bleibt die Box zu
3.ist der Code richtig springt ein Servo an und öffnet die Box...

Wie muss ich das Programmieren ??? kann mir da jemand helfen???? der 
Servo hängt an pin 13.
Es hing noch ein Relay dran aber das soll weg...
das war der Code bis jetzt .....


#include <Keypad.h>
#include <LiquidCrystal.h>

LiquidCrystal lcd(0, 1, 2, 3, 4, 12);

#define relayPin 13

const byte ROWS = 4; //four rows
const byte COLS = 3; //four columns
//define the cymbols on the buttons of the keypads
char hexaKeys[ROWS][COLS] = {
{'1','2','3'},
{'4','5','6'},
{'7','8','9'},
{'*','0','#'}
};
byte rowPins[ROWS] = {11, 10, 9, 8}; //connect to the row pinouts of the 
keypad
byte colPins[COLS] = { 7, 6, 5}; //connect to the column pinouts of the 
keypad

int pos = 0;
char secretCode[6] = {'1', '2', '3', '4', '5', '6'};
char inputCode[6] = {'0', '0', '0', '0', '0', '0'};

//initialize an instance of class NewKeypad
Keypad customKeypad = Keypad( makeKeymap(hexaKeys), rowPins, colPins, 
ROWS, COLS);

void setup()
{
lcd.begin(16, 2);
pinMode(relayPin, OUTPUT);
//Serial.begin(9600);
lcd.setCursor(0,0);
lcd.print(" Welcome! ");
delay(2000);
}
void loop()
{
readKey();
}

void readKey()
{
int correct = 0;
int i;
char customKey = customKeypad.getKey();
if (customKey)
{
switch(customKey)
{
case '*':
pos = 0;
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Input Your Code:");
break;
case '#':
for(i = 0; i < 6; i++)
{
if(inputCode[i] == secretCode[i])
{
correct ++;
}
}
if(correct == 6)
{
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Input correctly!");
lcd.setCursor(0, 1);
lcd.print(" Please Come In ");
digitalWrite(relayPin, HIGH);
delay(2000);
lcd.clear();
lcd.setCursor(0,0);
lcd.print(" Welcome! ");
}
else
{
lcd.clear();
lcd.setCursor(0, 0);
lcd.print(" Input Error! ");
lcd.setCursor(0, 1);
lcd.print(" Please Again ");
digitalWrite(relayPin, LOW);
delay(2000);
lcd.clear();
lcd.setCursor(0,0);
lcd.print(" Welcome! ");
}
break;
default:
inputCode[pos] = customKey;
lcd.setCursor(pos,1);
lcd.print(inputCode[pos]);
pos ++;
}
}
}

Danke

LG Reiner