* icb file

***************************************
* Sonar Sensor Routines
***************************************

* operated off of IC2 and IC3 interrupt

*  Brian Van Mondfrans
*  Mon Feb 21 1994

#include "6811regs.asm"
#include "pcode_eq.asm"
#include "r21_eq.asm"

	ORG MAIN_START

subroutine_initialize_module:

#include "ldxibase.asm"
	LDD #tic3_code_start
	STD TIC3INT,X   ; TIC3 interrupt

	LDAA #%00000001
	STAA TCTL2

	LDX #BASE

	BCLR TFLG1,X $FE
	BSET TMSK1,X $01

	CLI
	RTS

****************************************
*
* TIC3 code: store time
*
*
tic3_code_start:
	LDX #BASE
		INC sonar_edge
		BNE falling_edge

		LDD TIC3
		BCLR TFLG2,X $7F
		LSRD
		STD variable_sonar_start_time
		LDAA #$00
		STAA variable_tof

		BCLR TCTL2,X $03
		BSET TCTL2,X $02
		BRA tic3_code_exit

overflow:
	LDAA #1
	STAA variable_tof
	BRA finish_up

falling_edge:
		LDD TIC3
		LSRD
		STD variable_sonar_end_time
		BRSET TFLG2,X $80 overflow
finish_up:        
		BCLR TCTL2,X $03
		LDAA #$FF
		STAA sonar_edge
		LDAA #1
		STAA variable_sonar_end

tic3_code_exit:
		BCLR TFLG1,X $FE
		RTI

variable_sonar_start_time:  FDB 0
sonar_edge:                 FCB $FF

variable_sonar_end_time:    FDB 0
variable_sonar_end:         FDB 0
variable_tof:               FDB 0