*========================
*
* icb file:  "sonar-rev21-test.asm"
*
*   Test the sonar board
*   Uses motor 2 control line for INIT, motor 3 for BINH
*
* Modified 3/1996 Kent Farnsworth (akent@zilker.net)
* 
* The major mods from the original version of this code is the change from
* a continuous ping to a single ping. If no echo is heard within the ping
* on time, sonar_time will read very low (0 to 2").
* Just call 'ping(0);' to init a ping, wait about 35ms, and read sonar_time.
*

#include "../6811regs.asm"
* program equates

* #define motorexp 1	/* uncomment for 6.270 expansion board pinging */

#if !motorexp
* motor 2,3 equates
SONAR_OUTPUT	EQU	$7f00		/* Using motor control lines for output */
SONAR_BYTE	EQU	$0E		/* Using upper half of motor byte */
SONAR_INIT	EQU	%01000000	/* INIT is motor 2 on line */
SONAR_INIT_MASK	EQU	%00111111	/* INIT is motor 2 on line */
SONAR_BINH	EQU	%10000000	/* BINH is motor 3 on line */
SONAR_INPUT	EQU 	PORTA
#else
SONAR_OUTPUT	EQU	$4000		/* Using motor control lines for output */
SONAR_BYTE	EQU	$00		/* Using upper half of motor byte */
SONAR_INIT	EQU	%01000000	/* INIT is motor 2 on line */
SONAR_INIT_MASK	EQU	%00000000	/* INIT is motor 2 on line */
SONAR_BINH	EQU	%10000000	/* BINH is motor 3 on line */
SONAR_INPUT	EQU 	PORTA
#endif

SONAR_ECHO	EQU	%00000001	/* ECHO is A0 line */
ON_PERIOD	EQU	30		/* listen for 30ms */
CAPTURE_MASK	EQU	%11111110	/* TFLG1 mask */
CAPTURE_BIT	EQU	1		/* TFLG1 and TCTL2 bit */
CAPTURE_REG	EQU	TIC3

	ORG	MAIN_START

* variables

/* C variables */
variable_sonar_time
sonar_time		FDB	0 /* counter time from beginning of INIT */

variable_sonar_enable	FCB	0
sonar_enable		FCB	0

/* internal variable */
sonar_count		FCB	0
sonar_start_time	FDB	0
	
subroutine_initialize_module:
#include "../ldxibase.asm"

* X now has base pointer to interrupt vectors ($FF00 or $BF00)

* get current vector; poke such that when we finish, we go there
	LDD	TOC4INT,X		; SystemInt on TOC4
	STD	interrupt_code_exit+1

* install ourself as new vector
	LDD	#interrupt_code_start
	STD	TOC4INT,X
	rts	


/************************************************************************/
subroutine_ping:
	LDAA 	#1			/* load 1 into D */
	STAA	sonar_enable		/* enable servo */
	CLR	sonar_count		/* start cycle */
	ldd	#0
	std	sonar_time
	LDX	#BASE
	BSET	TCTL2-BASE,X CAPTURE_BIT	* set to captur input mode
	RTS

interrupt_code_start:
	LDAB	sonar_enable		/* check if sonar is on */
	BEQ	interrupt_code_exit

	LDAA	sonar_count		/* check if at beginning of cycle */
	BEQ	reset_count

	DECA
	STAA	sonar_count
	BRA	interrupt_code_exit

reset_count
	LDX	#BASE
	CMPB	#1
	BEQ	set_init	/* we're at the beginning of cycle */	

	CMPB	#2
	BEQ	set_binh	/* override the internal blanking interval */

	LDAA	SONAR_BYTE	/* turn off INIT line */
	ANDA	#SONAR_INIT_MASK
	STAA	SONAR_BYTE	/* turn it off for posterity */
	STAA	SONAR_OUTPUT	/* turn it off now */

	BrCLR	TFLG1-BASE,X CAPTURE_BIT interrupt_code_exit /* no echo yet if exit */
	BCLR	TFLG1-BASE,X CAPTURE_MASK	/* clear the capture bit */
	LDD	CAPTURE_REG				/* get the time of capture */
	SUBD	sonar_start_time	/* minus the initial turn on time */
	LSRD						/* always positive, in us */
	STD	sonar_time				/* for C code to use */

	CLR	variable_sonar_enable+1	/* turn off the interrupt */
	BRA	interrupt_code_exit

set_init
	LDAA	SONAR_BYTE	/* turn on INIT line */
	ORAA	#SONAR_INIT
	STAA	SONAR_BYTE
	STAA	SONAR_OUTPUT	/* turn it on now */

	LDD	TCNT
	STD	sonar_start_time
	LDAA	#2		/* set for binh on next time */
	STAA	sonar_enable
	BRA	interrupt_code_exit

set_binh
	LDAA	SONAR_BYTE	/* turn on BINH line */
	ORAA	#SONAR_BINH
	STAA	SONAR_BYTE
	STAA	SONAR_OUTPUT	/* turn it on now */

	LDAA	#ON_PERIOD
	STAA	sonar_count
	LDAA	#3
	STAA	sonar_enable

interrupt_code_exit:
	JMP	$0000	; this value poked in by init routine