TITLE  "photo2.asm"            	; sunflower robot
 List P=12F675, R=DEC
 INCLUDE "p12F675.inc"

; data segment
 CBLOCK 0x20                   
 del  					; variables for passing the parameters
 n1, n2					; sensor values
 ENDC

; code segment
 PAGE
 __CONFIG _BODEN_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
	clrf	GPIO			; initialize GPIO
	movlw	0x07
	movwf	CMCON			; comparator OFF
	bcf	ADCON0, 0		; ADC OFF
  	bsf    	STATUS, RP0      	; change to BANK 1
	clrf	ANSEL			; turn off ADC
	clrf	WPU
	movlw	0x0A
	movwf	TRISIO 			; enable GP<3:1> for input
  	bcf    	STATUS, RP0        	; back to BANK 0

loop
	call 	measure2		; get n1
	call 	measure1		; get n2

	bcf	GPIO, 0 		; motor stop
	bcf	GPIO, 5

	movf	n2, w			; w = n1 - n2
	subwf	n1, w			
	btfss	STATUS, C
	sublw	0			; w = |n1 - n2|
	sublw	15			; w = 15 - w
	btfsc	STATUS, C			
	goto 	L2			; diff is small - continue

	movf	n2, w
	subwf	n1, w
	btfss	STATUS, C
	goto 	L1	
	bsf	GPIO, 0
	goto 	L2
L1	bsf	GPIO, 5

L2	movlw	50			; 50 msec delay
	movwf	del
	call	delay
  	goto 	loop			; endless loop


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

	decfsz	del, f
	goto 	delay
	return

measure1
	clrf	n1		
	bsf	GPIO, 4			; take cap high
		
	incf	n1, f
 	movlw	255
	subwf	n1, w
	btfsc	STATUS, Z
	goto	$+3
	btfsc	GPIO, 3			; is input low ?
	goto	$-6			; back to loop

	bcf	GPIO, 4			; discharge the cap
	return

measure2
	clrf	n2
	bsf	GPIO, 2			; take cap high
	
	incf	n2, f
 	movlw	255
	subwf	n2, w
	btfsc	STATUS, Z
	goto	$+3
	btfsc	GPIO, 1			; is input low ?
	goto	$-6			; back to loop

	bcf	GPIO, 2			; discharge the cap
	return

 end