Code:
; ====== symbol setup ============
STATUS EQU 03
OPT EQU 01
RPO EQU 05
PORTA EQU 05
TRISA EQU 05
PORTB EQU 06
TRISB EQU 06
RBPU EQU 07
ZFLAG EQU 02
rightsensor EQU 04
leftsensor EQU 05
frontsensor EQU 06
STRTBTN EQU 07
DelayL EQU 0C
DelayM EQU 0D
DelayH EQU 0E
RecHits EQU 0F
; ====== port setup ============
START BSF STATUS,RPO
CLRF TRISA ; port a = outputs
MOVLW B'11110000'
MOVWF TRISB ; rb4-rb7 = input, rb0 - rb3 = output
BCF OPT,RBPU ; enable pull ups
BCF STATUS,RPO
anfang NOP
CLRF PORTA ; initialize ports
CLRF PORTB
CLRF RecHits ; clear recorded hits by sensors
CALL Delay2000
STRTLP BTFSC PORTB,STRTBTN ; waits for activate button to begin...
GOTO STRTLP
GOTO BEGIN
BEGIN CALL Delay2000 ; x4 = 2sec delay after start button pushed.
resume NOP
CALL run
GOTO anfang
;======== 500 ms delay loop=====
DELAY500 CLRF DelayL
CLRF DelayM
MOVLW H'1' ;Fixed delay entry point
MOVWF DelayH ;Variable Delay Entry point
LOOPD DECFSZ DelayL
GOTO LOOPD ;returns to loop while counter > 0
DECFSZ DelayM
GOTO LOOPD
DECFSZ DelayH
GOTO LOOPD
RETURN ; return to program
;======= fwd =====
fwd MOVLW B'1001'
MOVWF PORTA
MOVWF PORTB
CALL DELAY500
BTFSS PORTB,frontsensor
BSF RecHits,04
BTFSS PORTB,rightsensor
BSF RecHits,05
BTFSS PORTB,leftsensor
BSF RecHits,06
BTFSC RecHits,04
GOTO exitfwd
BTFSC RecHits,05
GOTO exitfwd
BTFSC RecHits,06
GOTO exitfwd
MOVLW B'1010'
MOVWF PORTA
MOVWF PORTB
CALL DELAY500
BTFSS PORTB,frontsensor
BSF RecHits,04
BTFSS PORTB,rightsensor
BSF RecHits,05
BTFSS PORTB,leftsensor
BSF RecHits,06
BTFSC RecHits,04
GOTO exitfwd
BTFSC RecHits,05
GOTO exitfwd
BTFSC RecHits,06
GOTO exitfwd
MOVLW B'0110'
MOVWF PORTA
MOVWF PORTB
CALL DELAY500
BTFSS PORTB,frontsensor
BSF RecHits,04
BTFSS PORTB,rightsensor
BSF RecHits,05
BTFSS PORTB,leftsensor
BSF RecHits,06
BTFSC RecHits,04
GOTO exitfwd
BTFSC RecHits,05
GOTO exitfwd
BTFSC RecHits,06
GOTO exitfwd
MOVLW B'0101'
MOVWF PORTA
MOVWF PORTB
CALL DELAY500
BTFSS PORTB,frontsensor
BSF RecHits,04
BTFSS PORTB,rightsensor
BSF RecHits,05
BTFSS PORTB,leftsensor
BSF RecHits,06
exitfwd MOVLW B'1001'
MOVWF PORTA
MOVWF PORTB
RETURN
left MOVLW B'1001'
MOVWF PORTA
MOVLW B'1001'
MOVWF PORTB
CALL DELAY500
MOVLW B'0101'
MOVWF PORTA
MOVLW B'1010'
MOVWF PORTB
CALL DELAY500
MOVLW B'0110'
MOVWF PORTA
MOVLW B'0110'
MOVWF PORTB
CALL DELAY500
MOVLW B'1010'
MOVWF PORTA
MOVLW B'0101'
MOVWF PORTB
CALL DELAY500
MOVLW B'1001'
MOVWF PORTA
MOVLW B'1001'
MOVWF PORTB
RETURN
adjusta MOVLW B'1001' ; move left wheel backwards
MOVWF PORTA
CALL DELAY500
MOVLW B'0101'
MOVWF PORTA
CALL DELAY500
MOVLW B'0110'
MOVWF PORTA
CALL DELAY500
MOVLW B'1010'
MOVWF PORTA
CALL DELAY500
MOVLW B'1001'
MOVWF PORTA
RETURN
circleleft CALL adjusta
CALL left
CALL left
CALL left
BSF RecHits,01 ; record bump to trigger right turn on next bump
CALL fwd
BTFSC RecHits,04 ; test if bumping front
RETURN
CALL fwd
BTFSC RecHits,04
RETURN
CALL fwd
BTFSC RecHits,04
RETURN
CALL fwd
BTFSC RecHits,04
RETURN
CALL fwd
BCF RecHits,01 ; recording of bump cleared, next turn left, not right on bump.
RETURN
adjustleft CALL back
CALL adjustb
RETURN
run CALL fwd
CALL align
BCF RecHits,05
BCF RecHits,06
BTFSS RecHits,04
goto run
BCF RecHits,04
BTFSS RecHits,01
CALL circleleft
BCF RecHits,04
BTFSS RecHits,01
GOTO run
CALL circleright
GOTO run
RETURN
END
EDIT: I removed half of the code due to the 10,000 char limit per post ... but the essentials of how it works is in tact.