;*******************************************************************
;Dbt_US_tst
;Uses Derbot ultrasound sensor for simple distance measurement. 
;Distance to target measured using US, and displayed on HC lcd.
;Resolution is 1cm.
;Sound travels 1mm/3us. For range of 1m require timing up to 6ms,
;2m up to 12ms. 
;TJW 10.9.05								        Tested 13.9.05
;*******************************************************************
;Clock is 4MHz
;
	list 	p=16F873A
	#include p16f873A.inc
;
;Set Configuration Word: crystal oscillator HS, WDT off, 
;     power-up timer on, code protect off, LV Program off.
	__CONFIG _HS_OSC & _WDT_OFF & _PWRTE_ON & _LVP_OFF
;
;Specify RAM
	cblock 20
delcntr1 	;used in delay5 & delayADC SRs
delcntr2 
temp	  	;a temp location, to be used only in consecutive instructions
int_cntr 	;counts incoming interrupts
pulse_cntr  ;counts how many pwm pulses sent to servo, before moving to next posn.
pw_cntr	    ;counter used to set width of pulse to servo
servo_dirn  ;little counter which determines direction servo points, used as look-up table pointer	
past_pulse  ;holds value of most recent pulse width, used with demand value to determine actual
echo_time	;counter measuring time for echo to return
I2C_RX_word 	;holds most recent I2C word recd
I2C_add	 	  	;holds address used in I2C message
I2C_TX_word 	;holds word to be transmitted on I2C
;for Binary to BCD routine
R0 				; output bytes 10k’s digits
R1 				; 1k’s, 100’s digits
R2 				; 10’s, 1’s digits
TEMPL 			; input bytes (2 byte binary)
TEMPH
COUNT 			; internal variable in Bin2BCD16
	endc
;
	org 00
	goto begin
;
;Initialise
;set up SFRs in Bank 1
begin	bcf	status,rp1
	bsf	status,rp0 		;select memory bank 1
	movlw	B'00011011'	;set port A bits, bit 4 ip for opto
	movwf	trisa		
	movlw	B'11110001'
	movwf	trisb		;also port B bits, bit 3 is servo PWM op
	movlw	B'11011001' ;Port C bits: bit 0 ip for opto, bit 6 for us echo, 
	movwf	trisc		;I2C bits both set as ip
	movlw	B'01000100' 
	movwf	adcon1		;select port A bits 0,1,3 for analog input
	movlw	B'11101000'
	movwf	option_reg	
	movlw 	D'250'			;set Timer 2 Overflow period
	movwf	pr2
	movlw	09 		;set up I2C for 100kbps baud rate
	movwf	sspadd
;set up SFRs in Bank 0	
	bcf	status,rp0 		;select bank 0
	movlw	B'01111100'	;switch on Timer2, no prescale, /16 postscale
						  ;giving 4ms interrupt prd (with pr2=250)
	movwf	t2con
	movlw	B'00101000'	;SSPCON1:MSSP on, I2C Master 
	movwf	sspcon
;
;Switch all outputs off
	movlw 00
	movwf porta
	movwf portb
	movwf portc
us_loop clrf echo_time
	bsf		portc,5		;output us pulse. 10us minimum required
	call 	delay20u
	bcf	 	portc,5
	call 	delay300u	;pause for op to be set high; ie blank for 5cm
;this loop takes 30us per cycle, ie 10mm there and back, or 5mm one way;
;hence 8-bit range is 255x5mm = 1275mm. Max duration in loop is 30x255 = 7.6ms
echo call 	delay24u
	incf	echo_time,1
	btfsc	status,z	;test for overflow, which indicates no target found
	goto	no_tgt
	btfsc	portc,6
	goto	echo
;here if target detected
	bsf		portb,1	;indicate with a bleep
	call	delay200	
	bcf		portb,1
	call	delay200
;now process and send data, resolving in cms. Binary word formed in temph, templ
;divide by 2 to get cms
	bcf		status,c
	rrf		echo_time,0	;W now holds distance in cms
;add in blanked 3cms (this derived experimentally)
	addlw	3	
	movwf	templ
	clrf	temph
	call	Bin2BCD16
	call 	four_dig_disp
	goto	us_loop
no_tgt	
	bsf		portb,1
	call	delay500
	call	delay500	
	bcf		portb,1
	call	delay200	
	goto	us_loop
;

;**********************************************************
;SUBROUTINES
;**********************************************************
;sends BCD digits to LCD. Digits are outputs of Bin2BCD16 SR.
four_dig_disp
;Start I2C message
	movlw	0a4			;this is slave address, R/W is write
	movwf	I2C_add
	call    I2C_send_add
;send ms digit	
	swapf	R1,0
	andlw	B'00001111'	;suppress other nibble
	iorlw	30			;turn to ASCII
	movwf	I2C_TX_word ;send the character	
	call	I2C_send_word
	call delay1	
	call delay1
	call delay1	;lcd line feed may have occurred here, give extra delay	
	call delay1
;send second digit
	movf	R1,0
	andlw	B'00001111'	;suppress other nibble
	iorlw	30			;turn to ASCII
	movwf	I2C_TX_word ;send the character	
	call	I2C_send_word
	call delay1	
	call delay1
;send middle digit
	swapf	R2,0	;swap the nibbles
	andlw	B'00001111'	;suppress other nibble
	iorlw	30			;turn to ASCII
	movwf	I2C_TX_word ;send the character	
	call	I2C_send_word
	call delay1	
	call delay1
;send ls digit
	movf	R2,0	;
	andlw	B'00001111'	;suppress other nibble
	iorlw	30			;turn to ASCII
	movwf	I2C_TX_word ;send the character	
	call	I2C_send_word
	call delay1	
	call delay1
;	movlw ' ' 		;send a space
;	movwf	I2C_TX_word ;send the character	
;	call	I2C_send_word
	call 	I2C_send_stop
	bcf		status,rp0
	call delay1	
	call delay1
	return
;
;initiates I2C message, by sending the word found in I2C_add, which 
;must include R/W bit.
;Waits for all acknowledgement and completion states.
I2C_send_add
	bsf		status,rp0
	bsf		sspcon2,sen	;force start bit
	btfsc	sspcon2,sen	;check for its completion
	goto	$-1
	bcf		status,rp0
	movf	I2C_add,0	;load address and data dirn bit
	movwf	sspbuf		;and send 
	bcf		pir1,sspif	;will test this soon
	bsf		status,rp0
	btfsc 	sspstat,bf ;test for write complete
	goto	$-1
	btfsc 	sspcon2,ackstat	;wait for 0 acknowledge bit 
	goto 	$-1
	bcf		status,rp0
	btfss	pir1,sspif ;test for int flag to show completion
	goto	$-1
	bcf		pir1,sspif
	return
;
;Receives (XXXfinal) word from I2C bus, and stores in I2C_RX_word. Returns Ack of 1.
I2C_rec_word bsf status,rp0
	bsf		sspcon2,rcen	;set receive enable bit 
	btfss	sspstat,bf		;wait for buffer full
	goto 	$-1
	bcf		status,rp0		;read the data
	movf	sspbuf,0
	movwf	I2C_RX_word		;store it for use somewhere
	bcf		pir1,sspif		;preclear int flag, as we are about to use it
	bsf		status,rp0
	bsf		sspcon2,ackdt 	;set required acknowledge state, 1 as it's last byte
	bsf		sspcon2,acken	;and enable it
	bcf		status,rp0
	btfss	pir1,sspif		;use interrupt flag to test for end of ack
	goto	$-1
	return
;
;Sends word on I2C bus, and awaits acknowledgement
I2C_send_word bcf status,rp0
	movf	I2C_TX_word,0	;get the word	
	movwf	sspbuf			;this starts the transfer
	bsf		status,rp0
	btfsc 	sspstat,r_w 	;test for write complete
	goto	$-1
	btfsc 	sspcon2,ackstat	;wait for 0 acknowledge bit 
	goto 	$-1
	bcf status,rp0
	return
;
;***************************************************************************
; Bin2BCD16 - Converts a 16-bit binary number in TEMPH:TEMPL into a 5-digit  
; 3 byte packed BCD number in R0:R1:R2. R0 holds the 10 thousands digit, R1 
; holds the hundreds and thousands digits, R2 holds the ones and tens digits.
;From Microchip Application Note AN220.
;***************************************************************************
; Inputs: TEMPH, TEMPL (16-bit binary number)
; Outputs: R0, R1, R2 (5 digit BCD number)
; Used: COUNT
Bin2BCD16
; Initialize variables
	bcf STATUS,C 	;clear carry bit
	movlw D'16'
	movwf COUNT 	;init bit counter
	clrf R0 		;clear output
	clrf R1 		;clear output
	clrf R2 		;clear output
Loop16a2 rlf TEMPL,F ;mult by 2, shift MSb to TEMPH
	rlf TEMPH,F 	;mult by 2, shift MSb to R2
	rlf R2,F 		;mult by 2, shift MSb to R1
	rlf R1,F 		;mult by 2, shift MSb to R0
	rlf R0,F 		;mult by 2, shift MSb to Carry
	decfsz COUNT,F 	;decrement bit counter
	goto AdjDec2
	return
AdjDec2	movlw R2
	movwf FSR 		;point to R2
	call AdjBCD2
	movlw R1
	movwf FSR 		;point to R1
	call AdjBCD2
	movlw R0
	movwf FSR 		;point to R0
	call AdjBCD2
 	goto Loop16a2
;
AdjBCD2
	movlw 3
	addwf INDF,W 	;W = 3 + Rn
	movwf TEMP 		;TEMP = 3+Rn
	btfsc TEMP,3 
	movwf INDF 		;Rn=TEMP
	movlw 30 		;decimal adjust?
	addwf INDF,W 	;W=30+Rn
	movwf TEMP 		;TEMP=30+Rn
	btfsc TEMP,7
	movwf INDF 		;Rn=TEMP
	return
;
;Sends I2C stop bit, and awaits completion
I2C_send_stop bsf status,rp0
	bsf		sspcon2,pen		;force stop bit. 
	btfss	sspstat,p 		;test for stop bit completion
	goto	$-1

;introduces delay of 20us approx
delay20u  movlw 	D'5'	;5 cycles called, 
;			each taking 3us, plus call, return (2 ea), and 2 move insts
;			less one cycle lost when last goto is hopped
        movwf  	delcntr1
dela    decfsz 	delcntr1,1 ;3 inst cycles in this loop, ie 3us
        goto 	dela
        return
;introduces delay of 24us 
delay24u  movlw 6	;6 cycles called, each taking 3us, 
;plus call (2), & 2 opening insts (2) + 2 at end
;			less one cycle lost when last goto is hopped
        movwf  	delcntr1
del21    decfsz 	delcntr1,1 ;3 inst cycles in this loop, ie 3us
        goto 	del21
		nop
	    return
;
;introduces delay of 200us. 98 cycles called, each taking 3us, 
;plus call (2), & 2 opening insts (2) + 2 at end
delay300u  movlw D'98'	;
        movwf  	delcntr1
del32   decfsz 	delcntr1,1 
		goto 	del32
		nop	
        return
;introduces delay of 1ms approx
delay1  movlw 	D'250'	;250 cycles called, 
;			each taking 4us
        movwf  	delcntr1
del1    nop	;4 inst cycles in this loop, ie 4us
        decfsz 	delcntr1,1
        goto 	del1
        return
;
;10ms delay (approx)	;10 calls to delay1
delay10  movlw D'10'
        movwf  	delcntr2
del10    call 	delay1
        decfsz  delcntr2,1
        goto 	del10
        return  
;200ms delay (approx)	;200 calls to delay1
delay200  movlw D'200'
        movwf  	delcntr2
del22    call 	delay1
        decfsz  delcntr2,1
        goto 	del22
        return  
;
;introduces delay of 500ms approx, thru 500 calls to delay1
delay500  movlw D'250'
        movwf  	delcntr2
del5    call 	delay1
		call 	delay1
        decfsz  delcntr2,1
        goto 	del5
        return  
;
	end
