TITLE  "trackbot2.asm"        		; Trackbot, v.2  
 List P=PIC16F684, R=DEC
 INCLUDE "p16f684.inc"

; data segment
 CBLOCK 0x20                   
 del  					; variable used for delay
 pulse, pulses2Send, pulsesReceived	
 ENDC

; code segment
 PAGE
 __CONFIG _FCMEN_OFF & _IESO_OFF & _BOD_OFF & _CPD_OFF & _CP_OFF & _MCLRE_OFF & _PWRTE_ON & _WDT_OFF & _INTRC_OSC_NOCLKOUT

  org 0                      		; start program at the beginning of mem
	bcf	STATUS, RP0		; activate BANK 0
	clrf	PORTA			; initialize PORT A
	clrf	PORTC			; initialize PORT C
	movlw	0x07
	movwf	CMCON0			; comparators OFF
	bsf    	STATUS, RP0      	; change to BANK 1

 	; ADC and PWM configuration
	movlw	b'10000'
	movwf	ANSEL ^ 0x80		; make RC0/AN4 analog input
	movlw 	b'01110000' 		; set ADC Frc clock
	movwf 	ADCON1 ^ 0x80
	movlw	0xFF	
	movwf	PR2 ^ 0x80		; PWM period 244Hz
	movlw	b'101101'
	movwf	TRISA ^ 0x80		; enable RA1 and RA4 for output
	movlw	1
	movwf	TRISC ^ 0x80		; enable output on RC<5:1>
	bcf    	STATUS, RP0        	; back to BANK 0

	movlw	b'10001'
	movwf 	ADCON0 			; Left justify, Vdd Vref, AN4, On	
	movlw	7
	movwf	T2CON			; enable Timer 2 with 1:16 prescaler
	movlw	0x0C			; enable single output PWM
	movwf	CCP1CON 	

	movlw	b'100000'
	movwf	PORTC			; enable motors

loop
	movlw	10
	movwf	del
	call	delay			; 10 msec delay

	; process pulses from the right sensor
	clrf	pulse			; start with IR pulse value 0
	clrf	pulsesReceived	
	movlw	32
	movwf	pulses2Send

send_pulses1
	goto	$+1
	goto 	$+1
        goto    $+1
        goto    $+1
	btfss	PORTA, 0		; is there IR reflected waveform?
	incf	pulsesReceived, f

	movlw	b'10'
	xorwf	pulse, w		; compose a new pulse value
	movwf	pulse
	movwf	PORTA			; output the new pulse value
	decfsz	pulses2Send, f
	goto	send_pulses1		; end of a 26 cycle (38KHz=26.3mksec)

	movlw	16
	subwf	pulsesReceived, w	; C=1 if more than 2 pulses received
	movf	PORTC, w
	andlw	b'11100111'		; left motor forwards
	iorlw	b'00010000'
	btfsc	STATUS, C
	xorlw	b'11000'		; left motor reverse
	movwf	PORTC

	movlw	10
	movwf	del
	call	delay			; 10 msec delay

	; process pulses from the left sensor
	clrf	pulse			; start with IR pulse value 0
	clrf	pulsesReceived	
	movlw	32
	movwf	pulses2Send

send_pulses2
	goto	$+1
	goto 	$+1
        goto    $+1
        goto    $+1
	btfss	PORTA, 5		; is there IR reflected waveform?
	incf	pulsesReceived, f

	movlw	b'10000'
	xorwf	pulse, w		; compose a new pulse value
	movwf	pulse
	movwf	PORTA			; output the new pulse value
	decfsz	pulses2Send, f
	goto	send_pulses2		; end of a 26 cycle (38KHz=26.3mksec)

	movlw	16
	subwf	pulsesReceived, w	; C=1 if more than 2 pulses received
	movf	PORTC, w
	andlw	b'11111001'		; right motor forwards	
	iorlw	b'00000010'
	btfsc	STATUS, C
	xorlw	b'110'			; right motor reverse
	movwf	PORTC

    ; setup the motors speed
	bsf	ADCON0, GO		; start ADC operation
	btfsc	ADCON0, GO		; and wait for its completion	
	goto 	$-1	
	movf	ADRESH, w		; get higher 8 bits of ADC
	movwf	CCPR1L			; setup PWM duty cycle ratio

  	goto 	loop			; endless loop


; procedures
delay					; a delay for del milliseconds
	movlw 	200
	
	sublw	1			; this loop takes 5us*200 = 1ms
	sublw	0			; for PIC16F684 @ 4 Mhz
	btfss	STATUS, Z
	goto 	$-3

	decfsz	del, f
	goto 	delay
	return

 end