list p=16f684
		include "p16f684.inc"
		__config _INTOSCIO & _PWRTE_ON & _WDT_OFF & _MCLRE_OFF & _CP_OFF & _CPD_OFF & _FCMEN_OFF & _BOD_OFF & _IESO_OFF

		CBLOCK 0x20
		duty1, duty2, conw, cons, dif, dutyp, sweeprdy, snsrc, snsrsta, left, right, snsrstatmp
		ENDC

		org 0x0000 
		goto start
		org 0x0004
		goto isr


start
		bcf STATUS, RP0 ;Bank 0
		clrf PORTA	
		clrf PORTC
		movlw 0x7
		movwf CMCON0	;Disable comparators
		bsf STATUS, RP0	;Bank 1
		movlw 0xE0
		movwf OSCCON	;8mhz
		movlw 0x50
		movwf ADCON1	;F/16 for 8mhz
		movlw B'10000001'
		movwf ADCON0	;Right justified, channel 0 enabled, AD - on
		movlw 0xCF
		movwf TRISA	; RA4, RA5 - outputs ; else inputs
		movwf TRISC ; RC4, RC5 - outputs ; else inputs
		movlw 0xEF
		movwf ANSEL	;RA4 - Digital, else analog
		bcf STATUS, RP0	;Bank 0

		bcf PIR1, T2IF	;Clear timer 2 interrupt flag
		bcf PIR1, ADIF	;Clear A/D interrupt flag
		clrf dutyp	;Clear duty cycle flags
		movlw 0x80
		movwf duty1
		movwf duty2	;Set Duty cycles to 50%
		bsf STATUS, RP0	;Bank 1
		movwf PR2	;Set timer 2 period
		movlw 0x42	
		movwf PIE1	;Turn on timer 2  and A/D interrupts
		movlw 0xC0
		movwf INTCON	;Turn global and peripheral interrupts on
		bcf STATUS, RP0	;Bank 0
		clrf TMR2
		movlw 0x4	;Turn on timer 2
		movwf T2CON
		bsf PORTC, 4	;RC4 = 1
		bsf PORTC, 5	;RC5 = 1
		goto main

val_table
		addwf PCL, f
		nop
		retlw B'10000001'	;Channel 0
		retlw B'10000101'	;Channel 1
		retlw B'10001001'	;Channel 2
		retlw B'10010001'	;Channel 4
		retlw B'10010101'	;Channel 5
		retlw B'10011001'	;Channel 6

isr
		movwf conw	;Save W
		swapf STATUS, w	;Save STATUS
		movwf cons
;Int
		btfss PIR1, ADIF	;Check A/D ready
		goto check_tmr2i	;Not A/D conv
		movlw D'250'	; ~1,22V
		bsf STATUS, RP0	;Bank 1
		subwf ADRESL, w	;ADRESL -= 250
		bcf STATUS, RP0	;Bank 0
		rlf	snsrsta, f	; Rotate snsrsta left through carry (result from subtraction)
		bsf PORTA, 5
		bcf PORTA, 5	;Next LED enable (or disable last)
		decfsz snsrc, f	;Snsrc -= 1, 0?
		goto next_sensor
		bsf sweeprdy, 0	;Indicate that sensor sweep is ready
		goto end_ad
next_sensor
		movf snsrc, w	;snsrc -> W
		sublw 0x6	;6 - W -> W
		call val_table
		movwf ADCON0
		call wait	;2us wait
		call wait	;2us wait
		bsf ADCON0, GO	;Start next conversation
end_ad
		bcf PIR1, ADIF	;Clear AD flag
check_tmr2i
		btfss PIR1, TMR2IF	;Timer 2 match int?
		goto end_int	;No, end int
		bcf T2CON, 2	;Turn off timer 2

		btfss dutyp, 2	;Start of duty cycle?
		goto check_expire	;No, check expire of duty cycle
		movf duty1,	w	;Yes, duty1 -> w
		btfss dutyp, 1	;duty1 expired first(duty1 < duty2)?
		movf duty2, w	;No, duty2 -> w
		clrf dutyp	;Clear flags
		bsf PORTC, 4	;RC4 = RC5 = 1
		bsf PORTC, 5
		goto end_tmr2	;End interrupt
check_expire	
		btfss dutyp, 0	;Duty expire?
		goto no_exp	;No, check for differences
		;Convert duty cycle
		comf duty1, w	;Complement Duty 1(255 - duty1)
		btfsc dutyp, 1	;Duty 1 expired last?
eqt
		comf duty2, w	;No, complement Duty 2(255 - duty2)
		bcf PORTC, 4	;RC4 = 0 regardless of state
		btfsc dutyp, 1	;Duty 1 expired last?
		bcf PORTC, 5	;No, RC5 = 0
		bsf dutyp, 2	;Set start
		goto end_tmr2
no_exp		
		movf duty2, w	;duty1 - duty2
		subwf duty1, w	
		btfsc STATUS, Z	;Equal?
		goto equal	;If equal, skip expire
		btfsc STATUS, C	;duty1 > duty2?
		goto gtr	;Yes, setup expire
		movwf dif	;duty1 < duty2
		comf dif, f	;negate 2's complement dif
		incf dif, w	;dif + 1 -> w
		bsf dutyp, 1	;Set duty2 expire
		bcf PORTC, 4	;RC4 = 0
		goto setexp		
gtr
		bcf PORTC,	5	;RC5 = 0
setexp
		bsf dutyp, 0	;Indicate expire
end_tmr2
		bsf STATUS, RP0	;Bank 1
		movwf PR2	;Set next period
		bcf STATUS, RP0
		bsf T2CON, 2	;Turn timer 2 back on
		bcf PIR1, T2IF	;Clear timer 2 flag
end_int
		swapf cons, w	;Restore STATUS
		movwf STATUS
		swapf conw, f	;Restore W
		swapf conw, w
		retfie
equal
		bsf dutyp, 1	;Set duty 2 expire
		goto eqt
wait
		return
main
		movlw 0x6
		movwf snsrc	;Setup counter
		clrf sweeprdy	;Clear sweep flag
		clrf snsrsta	;Clear sensor status
		bsf PORTA, 4
		bsf PORTA, 5	;Shift 1 into the register(enable LED0)
		bcf PORTA, 5
		bcf PORTA, 4	
		bsf ADCON0, GO	;Start conversation

loop
		goto loop	;Infinite loop, until interrupt
		btfss sweeprdy, 0	;Sensor check ready?
		goto loop	;No, go back to looping
check_sides
		movf snsrsta, w
		movwf snsrstatmp	;Save snsrsta
		clrf left	;Left = right = 0
		clrf right
		rrf snsrsta, f
		btfss STATUS, C	;1 = not in line;0 = in line
		incf right,f	;one indicator + in right
		rrf snsrsta, f
		btfss STATUS, C	;1 = not in line;0 = in line
		incf right,f	;one indicator + in right
		rrf snsrsta, f
		btfss STATUS, C	;1 = not in line;0 = in line
		incf right,f	;one indicator + in right
		rrf snsrsta, f
		btfss STATUS, C	;1 = not in line;0 = in line
		incf left,f	;one indicator + in left
		rrf snsrsta, f
		btfss STATUS, C	;1 = not in line;0 = in line
		incf left,f	;one indicator + in left
		rrf snsrsta, f
		btfss STATUS, C	;1 = not in line;0 = in line
		incf left,f	;one indicator + in left
		movf left, w
		subwf right, w	;right - left -> W	
		btfsc STATUS, Z	;Equal?
		goto straight	;Yes, the bot is centered!
		btfss STATUS, C	;right > left?
		goto go_right	;No, right < left (line in right), go right
		movlw 0x50
		movwf duty1	;Left motor slower than left one => turn right
		movlw 0x80
		movwf duty2
		goto main
		nop	;Int Protection
		goto main
go_right
		movlw 0x50
		movwf duty2	;Right motor slower than right one => turn left
		movlw 0x80
		movwf duty1
		goto main
		nop	;Int Protection
		goto main
straight
		movlw 0x80	;Set duty cycle to 50%
		movwf duty1
		movwf duty2
		goto main
		nop	;Int Protection
		goto main
		END