Entering a 3 digit code with keypad and PIC

I am working on using a keypad and a 3 digit code to arm and disarm an alarm system. The code will be 1-2-3 and when it is entered it will either arm or disarm a system. I have written code to do this however it is not working correctly, the code can be seen below.

I put 1 in the NUM1 file, 2 in the NUM2 file and 3 in the NUM3 file and then I scan the keypad to find the numbers 1-2-3 in order then it will light an LED to say the system is on, enter the code again and the LED goes off to say the system is off. I am not sure that I am doing this correctly as far as the PIC storing the code and checking it with the entered code. Could someone help me work through this or maybe try a different approach? I appreciated the help this is the first time I am trying to do something with a code and a keypad.


#include "CS110000.h"

#define W 0
#define NUM1 4
#define NUM2 5
#define NUM3 6
#define CA 10
#define CB 12
#define CC 13
#define CD 14

void main(void)
{
_asm

//Sets zerobit
BSF STATUS, 2, ACCESS

//Keypad PORTB Input/Output Set Up
BCF TRISB, 0, ACCESS //Column 1 output
BCF TRISB, 1, ACCESS //Column 2 output
BCF TRISB, 2, ACCESS //Column 3 output
BCF TRISD, 7, ACCESS //Column 4 output //***pin might be bad***
BSF TRISB, 4, ACCESS //Row 1 input
BSF TRISB, 5, ACCESS //Row 2 input
BSF TRISB, 6, ACCESS //Row 3 input
BSF TRISB, 7, ACCESS //Row 4 input

GOTO START

START: NOP
//Enter 3 digit code here
MOVLW 1 //First digit
MOVWF NUM1, ACCESS
MOVLW 2 //Second digit
MOVWF NUM2, ACCESS
MOVLW 3 //Third digit
MOVWF NUM3, ACCESS

//****NEEDS WORK****
BEGIN: CALL SCAN, 0 //Get 1st number
SUBWF NUM1, W, ACCESS
BTFSC STATUS, NUM1, ACCESS //IS NUMBER=1?
GOTO BEGIN //No
CALL SCAN, 0 //Get 2nd number
SUBWF NUM2, W, ACCESS
BTFSC STATUS, NUM2, ACCESS //IS NUMBER=2?
GOTO BEGIN //No
CALL SCAN, 0 //Get 3rd number.
SUBWF NUM3, W, ACCESS
BTFSC STATUS, NUM3, ACCESS //IS NUMBER=3?
GOTO BEGIN //No
BSF TRISE, 0, ACCESS //Turn on LED, 123 entered

OFF: CALL SCAN, 0
SUBWF NUM1, W, ACCESS
BTFSC STATUS, NUM1, ACCESS
GOTO OFF
CALL SCAN, 0
SUBWF NUM2, W, ACCESS
BTFSC STATUS, NUM2, ACCESS
GOTO OFF
CALL SCAN, 0
SUBWF NUM3, W, ACCESS
BTFSC STATUS, NUM3, ACCESS
GOTO OFF
BCF TRISE, 0, ACCESS
GOTO BEGIN

//Keypad scan
SCAN: NOP
//Scans Column 1
COLUMN1: BSF LATB, 0, ACCESS
BCF LATB, 1, ACCESS
BCF LATB, 2, ACCESS
BCF LATD, 7, ACCESS

CHECK1: BTFSS PORTB, 4, ACCESS
GOTO CHECK4
CALL DELAYP1,0
CHECK1A: BTFSC PORTB, 4, ACCESS
GOTO CHECK1A
CALL DELAYP1,0
RETLW 1

CHECK4: BTFSS PORTB, 5, ACCESS
GOTO CHECK7
CALL DELAYP1,0
CHECK4A: BTFSC PORTB, 5, ACCESS
GOTO CHECK4A
CALL DELAYP1,0
RETLW 4

CHECK7: BTFSS PORTB, 6, ACCESS
GOTO CHECK10
CALL DELAYP1,0
CHECK7A: BTFSC PORTB, 6, ACCESS
GOTO CHECK7A
CALL DELAYP1,0
RETLW 7

CHECK10: BTFSS PORTB, 7, ACCESS
GOTO COLUMN2
CALL DELAYP1,0
CHECK10A: BTFSC PORTB, 7, ACCESS
GOTO CHECK10A
CALL DELAYP1,0
RETLW 10

//Scans column 2
COLUMN2: BCF LATB, 0, ACCESS
BSF LATB, 1, ACCESS
BCF LATB, 2, ACCESS
BCF LATD, 7, ACCESS

CHECK2: BTFSS PORTB, 4, ACCESS
GOTO CHECK5
CALL DELAYP1,0
CHECK2A: BTFSC PORTB, 4, ACCESS
GOTO CHECK2A
CALL DELAYP1,0
RETLW 2

CHECK5: BTFSS PORTB, 5, ACCESS
GOTO CHECK8
CALL DELAYP1,0
CHECK5A: BTFSC PORTB, 5, ACCESS
GOTO CHECK5A
CALL DELAYP1,0
RETLW 5

CHECK8: BTFSS PORTB, 6, ACCESS
GOTO CHECK0
CALL DELAYP1,0
CHECK8A: BTFSC PORTB, 6, ACCESS
GOTO CHECK8A
CALL DELAYP1,0
RETLW 8

CHECK0: BTFSS PORTB, 7, ACCESS
GOTO COLUMN3
CALL DELAYP1,0
CHECK0A: BTFSC PORTB, 7, ACCESS
GOTO CHECK0A
CALL DELAYP1,0
RETLW 0

//Scans column 3
COLUMN3: BCF LATB, 0, ACCESS
BCF LATB, 1, ACCESS
BSF LATB, 2, ACCESS
BCF LATD, 7, ACCESS

CHECK3: BTFSS PORTB, 4, ACCESS
GOTO CHECK6
CALL DELAYP1,0
CHECK3A: BTFSC PORTB, 4, ACCESS
GOTO CHECK3A
CALL DELAYP1,0
RETLW 3

CHECK6: BTFSS PORTB, 5, ACCESS
GOTO CHECK9
CALL DELAYP1,0
CHECK6A: BTFSC PORTB, 5, ACCESS
GOTO CHECK6A
CALL DELAYP1,0
RETLW 6

CHECK9: BTFSS PORTB, 6, ACCESS
GOTO CHECK11
CALL DELAYP1,0
CHECK9A: BTFSC PORTB, 6, ACCESS
GOTO CHECK9A
CALL DELAYP1,0
RETLW 9

CHECK11: BTFSS PORTB, 7, ACCESS
GOTO COLUMN4
CALL DELAYP1,0
CHECK11A: BTFSC PORTB, 7, ACCESS
GOTO CHECK11A
CALL DELAYP1,0
RETLW 11

//Scans column 4
COLUMN4: BCF LATB, 0, ACCESS
BCF LATB, 1, ACCESS
BCF LATB, 2, ACCESS
BSF LATD, 7, ACCESS

CHECKA: BTFSS PORTB, 4, ACCESS
GOTO CHECKB
CALL DELAYP1,0
CHECKAA: BTFSC PORTB, 4, ACCESS
GOTO CHECKAA
CALL DELAYP1,0
RETLW 12

CHECKB: BTFSS PORTB, 5, ACCESS
GOTO CHECKC
CALL DELAYP1,0
CHECKBA: BTFSC PORTB, 5, ACCESS
GOTO CHECKBA
CALL DELAYP1,0
RETLW 13

CHECKC: BTFSS PORTB, 6, ACCESS
GOTO CHECKD
CALL DELAYP1,0
CHECKCA: BTFSC PORTB, 6, ACCESS
GOTO CHECKCA
CALL DELAYP1,0
RETLW 14

CHECKD: BTFSS PORTB, 7, ACCESS
GOTO COLUMN1
CALL DELAYP1,0
CHECKDA: BTFSC PORTB, 7, ACCESS
GOTO CHECKDA
CALL DELAYP1,0
RETLW 15

//0.1sec delay for keypad antibounce
DELAYP1:MOVLW 6
MOVWF CC, ACCESS
MOVLW 19
MOVWF CB, ACCESS
MOVLW 173
MOVWF CA, ACCESS
LOOPP1: DECFSZ CA,1, ACCESS
GOTO LOOPP1
DECFSZ CB,1, ACCESS
GOTO LOOPP1
DECFSZ CC,1, ACCESS
GOTO LOOPP1
RETLW 0

RETURN 0



_endasm
}

Check out Nigels tutorial Tutorial 9_2 has a code lock. It can be adapted to your needs. I'm currently trying to modify the code to output to an LED (or lock solenoid in the future) when the correct code is entered. If you dont need the LCD, you'll have plenty of i/o and the changes to the code should be trivial.