need help for micromouse maze algorithm code.

Hi,i am final year student at local university and my final project is to develop the micromouse robot.i use the PIC18f4550,a couple DC motor with encoder and three IR range detector.The sensors are implemented at the front,left and right.my objective of the project is to memorize the map when the robot get into the maze area while it try to find the way out by its own.the design of the maze are 5x5.my problem is how can i make the algorithm of simple 5x5 maze map so that the robot can memorize the map while it try to find its way out without crashing the maze's wall. i have made the code for algorithm but the robot not execute as my requirement.it would be pleasure if somebody could give me the hint or suggestion of the maze algorithm that i made. i attach my code for your reference. for your information i use PICbasic microcode studio version 2.2.1.1 to make my code of maze algorithm. Thanks.

The code in format pbp file.

'****************************************************************
'* Name : UNTITLED.BAS *
'* Author : [select VIEW...EDITOR OPTIONS] *
'* Notice : Copyright (c) 2009 [select VIEW...EDITOR OPTIONS] *
'* : All Rights Reserved *
'* Date : 10/28/2009 *
'* Version : 1.0 *
'* Notes : *
'* : *
'****************************************************************
define osc 8

'defenition
DEFINE CCP1_REG PORTC ' Hpwm 1 pin port
DEFINE CCP1_BIT 2 ' Hpwm 1 pin bit
DEFINE CCP2_REG PORTC ' Hpwm 2 pin port
DEFINE CCP2_BIT 1 ' Hpwm 2 pin bit
DEFINE ADC_BITS 8 ' Set number of bits in result (8, 10 or 12)
DEFINE ADC_CLOCK 3 ' Set clock source (rc = 3)
DEFINE ADC_SAMPLEUS 50 ' Set sampling time in microseconds
MWPWM1 VAR PORTC.2 'PWM MOTOR 1
MWPWM2 VAR PORTC.1 'PWM MOTOR 2
MRCW1 VAR PORTD.4
MRCCW1 VAR PORTD.5 'MOTOR RIGHT DRIVE
MRCW2 VAR PORTD.6
MRCCW2 VAR PORTD.7 'MOTOR LEFT DRIVE
TRISD.4=0
TRISD.5=0
TRISD.6=0
TRISD.7=0
TRISA.2=1
TRISA.1=1
TRISA.0=1
ADCON1=%00000000
SENSOR1 VAR BYTE
SENSOR2 VAR BYTE
SENSOR3 VAR BYTE
i var byte 'step in one box
junction var byte 'at the jubction
status_sensor var byte '0 - 1, 1 - 2, 2 -3
porte.7=1 'activate internal resistor

'kire distance
distance var word 'distance in pulse
counter var word
stat var byte
portin var portd.0
input portin

HPWM 1,50,500
HPWM 2,50,500

'MAIN PROGRAM
MAIN:
status_sensor=0
GOSUB IR1 'JUMP TO IR1 SUBOUTINE
GOSUB IR2 'JUMP TO IR2 SUBROUTINE
GOSUB IR3 'JUMP TO IR3 SUBROUTINE

'maze algorithm'
i=0
select case status_sensor

case 0 'no detection
gosub fwd 'go straight
pause 1000 'delay for 1 second
gosub stopmotor 'robot stop

select case status_sensor

case 4 's3 detect
gosub left 'turn left
pause 1000 'delay for 1 second
gosub fwd 'go straight

select case status_sensor

case 2 's2 detect
gosub right 'turn right
pause 1000 'delay for 1 second
gosub fwd 'go straight

select case status_sensor

case 1 's1 detect
gosub right 'turn right
pause 1000 'delay for 1 second
gosub fwd 'go straight

select case status_sensor

case 6 's2&s3 detect
i=0 'i for per block=0
junction=i 'junction at block
gosub left 'turn left
i=i+0 'block+1
pause 1000 'delay for 1 second
gosub fwd 'go straight
i=i+1 'block+1

select case status_sensor

case 5 's1&s3 detect
i=0 'i for per block=0
gosub fwd 'go straight
i=i+0 'block+1

select case status_sensor

case 3 's1&s2 detect
i=0 'i for per block=0
junction=i 'junction at block
gosub right 'turn right
i=i+0 'block+1
pause 1000 'delay for 1 second
gosub fwd 'go straigh
i=i+0 'block+1

select case status_sensor

case 7 's1,s2,s3 detect
i=0 'i for per block=0
gosub bwd 'reverse
i=i-0 'block-0
pause 1000 'delay for 1 second
gosub uturn 'make uturn
pause 1000 'delay for 1 second
gosub fwd 'go straight
pause 1000 'delay for 1 second

END SELECT
END SELECT
END SELECT
END SELECT
END SELECT
END SELECT
END SELECT
END SELECT
GOTO MAIN 'LOOP TO MAIN FOREVER

'SUBROUTINE PROGRAM
IR1:
adcin 0, SENSOR1
IF SENSOR1>82 THEN
status_sensor.0=1
ENDIF

RETURN

IR2:
ADCIN 1,SENSOR2
IF SENSOR2>82 THEN
status_sensor.1=1
ENDIF

RETURN

IR3:
ADCIN 2,SENSOR3
IF SENSOR3>82 THEN
status_sensor.2=1
ENDIF
RETURN

gosub fwd
distance=179
gosub countnstop
gosub stopmotor
pause 10000
return

gosub right
distance=179
gosub countnstop
gosub stopmotor
pause 10000
return

gosub left
distance=179
gosub countnstop
gosub stopmotor
pause 10000
return

gosub bwd
distance=179
gosub countnstop
gosub stopmotor
pause 10000
return

gosub uturn
distance=179
gosub countnstop
gosub stopmotor
pause 10000
return

fwd:
HPWM 1,50,500
MRCCW1=0
MRCW1=1
HPWM 2,50,500
MRCCW2=1
MRCW2=0
RETURN

stopmotor:
MRCCW1=0
MRCW1=0
MRCCW2=0
MRCW2=0
RETURN

bwd:
HPWM 1,50,500
MRCCW1=1
MRCW1=0
pause 100
HPWM 2,50,500
MRCCW2=0
MRCW2=1
pause 100
return

left:
HPWM 1,50,500
MRCCW1=0
MRCW1=1
pause 100
HPWM 2,50,500
MRCCW2=0
MRCW2=1
pause 100
return

right:
HPWM 1,50,500
MRCCW1=1
MRCW1=0
' pause 100
HPWM 2,50,500
MRCCW2=1
MRCW2=0
' pause 100
return

uturn:
HPWM 1,50,500
MRCCW1=1
MRCW1=0
' pause 100
HPWM 2,50,500
MRCCW2=1
MRCW2=0
' pause 100
return

countnstop:

stat=0
counter=0
while distance>counter
if portin=1 then
stat=1
endif
if stat=1 and portin=0 then
counter=counter+1
stat=0
endif
wend
return