;File: SFARover.asm
;Program function: This program monitors two front impact switches and executes 
;an appropriate collision avoidance routine.
;PORTA: RA0 = Left switch, RA1 = Right switch.
;PORTB: RB0, RB1 = Left Wheel, RB2, RB3 = Right Wheel
;CPU Configuration: PIC16F84, RC Oscillator, Watchdog timer Off, Power-up timer ON.

	processor 16f84
	include <p16f84.inc>
	__config _RC_OSC &_WDT_OFF &_PWRTE_ON

;Variable declaration
J	equ		H'1E'	
K	equ		H'1F'

;Main program
	org		0			;Start program at address zero.

;Initialize Ports
	clrf		PORTB			;Clear PORTB.
	movlw		B'00011'		;Move binary 00011 to w register.
	banksel	    TRISA			;Select bank with TRISA in it.
	movwf		TRISA			;Set PORTA for input on pins RA0 and RA1.
	clrf		TRISB			;PORTB all zeros (all pins are outputs).
	banksel	    PORTB			;Go back to bank 0.
	clrf		PORTB			;PORTB set to all zeros.
	movlw   	B'00001010'		;Move binary 1010 to w register.
	movwf		PORTB			;Move binary 1 to PORTB (Wheels straight, drive on).

;Scan input switches.
scan	
    btfsc	    PORTA,0	        ;Is PORTA, pin 0 high?
	call		ALO			    ;Call Avoid Left Obstacle subroutine.
	btfsc		PORTA,1		    ;Is PORTA, pin 1 high?
	call		ARO			    ;Call Avoid Right Obstacle subroutine.	
	goto		scan		    ;Check switches again.

;Avoid Left Obsatcle
ALO	
    call		DELAY1		    ;Stop delay here.
	btfss		PORTA,0		    ;Is PORTA, pin 1 still high? 
    return				        ;If not, return to main program.
	movlw		B'00000101'		;Backup
	movwf		PORTB			
	call		DELAY2		
	movlw		B'00001001'		;Tank right
	movwf		PORTB			
	call		DELAY2		
	movlw		B'00001010'		;Go forward	
	movwf		PORTB			
	return				        ;Return to main program.


;Avoid Right Obsatcle
ARO	
    call		DELAY1			;Stop delay here.
	btfss		PORTA,1			;Is PORTA, pin 1 still high?     
    return
	movlw		B'00000101'		;Backup
	movwf		PORTB			
	call		DELAY2			
	movlw		B'00000110'		;Tank left
	movwf		PORTB			
	call		DELAY2			
	movlw		B'00001010'		;Go forward
	movwf		PORTB				
	return				

;Time delay to check for voltage spikes
DELAY1	movlw		D'2'			
		movwf		J			
j1loop	movwf		K			
k1loop	decfsz	    K,f			
		goto 		k1loop
		decfsz	    J,f
		goto 		k1loop
		return

;Time delay for reverse runs.
DELAY2	movlw		D'10'			
		movwf		J			
j2loop	movwf		K			
k2loop	decfsz	    K,f			
		goto 		k2loop
		decfsz	    J,f
		goto 		k2loop
		return

	end