Dijkstra algorithm Pbasic

Hello I'm doing a project with a robot caled the boe-bot. My project deals with the boe-bot going through a maze twice: the first time it goes through every single path, even the dead ends, and on the second run it has the entire maze stored in memory so it takes the shortest path to the finish line avoiding all dead ends.

Here is my code so far in pbasic. It goes through the entire maze but doesn't memorize the paths taken. Can you guys add an algorithm in pbasic for memorizing paths taken to my code thanks.

' {$STAMP BS2}
' {$PBASIC 2.5}



DEBUG "Program Running!"
counter VAR Word
FREQOUT 4, 2000, 3000 ' Signal program start/reset.
FOR counter = 1 TO 41
PULSOUT 13, 850
PULSOUT 12, 650
PAUSE 20
NEXT
DO
SELECT INL ' Select line follower states
CASE %11000000, %11100000 ' Pivot right
PULSOUT 13, 650
PULSOUT 12, 650
CASE %01100000, %01110000 ' Curve right
PULSOUT 13, 700
PULSOUT 12, 650
CASE %00110000 ' Slight right
PULSOUT 13, 750
PULSOUT 12, 650
CASE %00111000 ' Adjust right
PULSOUT 13, 800
PULSOUT 12, 650
CASE %11111111 ' T Intersection
PULSOUT 13, 750
PULSOUT 12, 650
CASE %00011000 ' Straight
PULSOUT 13, 850
PULSOUT 12, 650
CASE %11111111 ' T Intersection Right Turn
PULSOUT 13, 750
PULSOUT 12, 650
CASE %00011100 ' Adjust left
PULSOUT 13, 850
PULSOUT 12, 700
CASE %00001100 ' Slight left
PULSOUT 13, 850
PULSOUT 12, 750
CASE %00000110, %00001110 ' Curve left
PULSOUT 13, 850
PULSOUT 12, 800
CASE %00000011, %00000111 ' Pivot left
PULSOUT 13, 850
PULSOUT 12, 850
CASE %00000000 'U-Turn
PULSOUT 13, 850
PULSOUT 12, 850
ENDSELECT
LOOP