* icb file:  "sonar-rev21-test.asm"
*
*   Test the sonar board
*   Uses motor 2 control line for INIT, motor 3 for BINH
*   Uses PA0/TIC3 for input capture
*   Result stored in variable sonar_time, in us flight time (max 16ms)
*   Enabled by calling sonar_on()
*   Disabled by calling sonar_off()
*   ON/OFF period using EQU's

#include <6811regs.asm>
* program equates
SONAR_OUTPUT	EQU	$7000		/* 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
SONAR_ECHO	EQU	%00000001	/* ECHO is A0 line */
ON_PERIOD	EQU	30		/* listen for 30ms */
OFF_PERIOD	EQU	80		/* rest for 80ms */
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
	
* fall through to sonar_off subroutine


/************************************************************************/
subroutine_sonar_off:
	CLR	variable_sonar_enable+1
	LDAA	SONAR_BYTE
	ANDA	#SONAR_INIT_MASK
	STAA	SONAR_BYTE
	LDX	#BASE
	BCLR	TFLG1-BASE,X CAPTURE_MASK	* clear capture bit
	BCLR	TCTL2-BASE,X CAPTURE_BIT	* set to normal input mode
	RTS
/************************************************************************/


/************************************************************************/
subroutine_sonar_on:
	LDAA 	#1			/* load 1 into D */
	STAA	sonar_enable		/* enable sonar */
	CLR	sonar_count		/* start cycle */
	LDX	#BASE
	BSET	TCTL2-BASE,X CAPTURE_BIT	* set to capture 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	/* suppress the inhibit... */

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

	LDAA	#OFF_PERIOD	/* time to rest */
	STAA	sonar_count
	LDAA	#1		/* set for init next time */
	STAA	sonar_enable

	BRCLR	TFLG1-BASE,X CAPTURE_BIT interrupt_code_exit
	BCLR	TFLG1-BASE,X CAPTURE_MASK	/* clear the bit */
	LDD	CAPTURE_REG
	SUBD	sonar_start_time
	LSRD			/* always positive, in us */
	STD	sonar_time
	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 rest 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