list	p=pic16f84
	include	"p16f84.inc"
;	__config	_CP_OFF & _WDT_OFF & _XT_OSC & _PWRTE_ON
;PORTA
SW_M		equ	0
xout		equ	4
SW_Pport	equ	PORTB
SW_P		equ	7

NAVE	equ	16		;readacc average
Kh		equ	0x02
Kl		equ	0xd0
TH		equ	30

	cblock	0ch
	T1Xh,T1Xl,T2h,T2l,ZXacth,ZXactl,Xacc,Count1,TMR0h
	ZXcalh,ZXcall,T2calh,T2call
	endc

	org		0
	goto	start

	org		04h				;0.8us x 256=204.8us
	incf	TMR0h,F
	bcf		INTCON,T0IF
	retfie

	include	"piclib.asm"
	include	"mathlib.asm"

start
	set_tris	10h,80h,00h	;PORTA:xxxioooo, PORTB:iooooooo, PBpu, x2(0.8us)
	clrf	PORTA
	bsf		INTCON,GIE
	call	cali
	call	t_500ms
	call	cali

main
	cbc		SW_Pport,SW_P,cali	;if(SW_P==0) call cali
	call	readacc
	call	findzact
	call	calcacc
	jbs		Xacc,7,main
	cgt		Xacc,TH+10,mouseon	;if(Xacc>=0 && Xacc>TH+10) call	mouseon
	goto	main

cali
	wbc		SW_Pport,SW_P
	call	readacc
	mov2	T2calh,T2call, T2h,T2l		;T2cal=T2
	mov2	ZXcalh,ZXcall, T1Xh,T1Xl	;ZXcal=T1X
	return

mouseon
	bsf		PORTA,SW_M
	call	readacc
	call	findzact
	call	calcacc
	jbs		Xacc,7,mouseon1
	jgt		Xacc,TH,mouseon
mouseon1
	bcf		PORTA,SW_M
	return

;get T1X and T2
readacc
	clrf2	T1Xh,T1Xl
	clrf2	T2h,T2l
	mvi		Count1,NAVE
raloop
	wbs		PORTA,xout				;xout~��
	wbc		PORTA,xout				;xout~~|__��
	clrf2	TMR0h,TMR0				;(TMR0h TMR0)=0(Ta)
	bcf		INTCON,T0IF				;Clear  TMR0 overflow flag
	bsf		INTCON,T0IE				;Enable TMR0 overflow int
	wbs		PORTA,xout				;xout      |~~��
	bcf		INTCON,T0IE				;Disable TMR0 overflow int
;	mov2	T1Xh,T1Xl, TMR0h,TMR0	;T1X(Tb)=(TMR0h TMR0)
	movf	TMR0,W
	addwf	T1Xl,F
	movf	TMR0h,W
	btfsc	STATUS,C
	incf	TMR0h,W
	addwf	T1Xh,F

	bsf		INTCON,T0IE				;Enable TMR0 overflow int
	wbc		PORTA,xout				;xout      |~~~|___��
	bcf		INTCON,T0IE				;Disable TMR0 overflow int
;	mov2	T2h,T2l, TMR0h,TMR0		;T2(Te)=(TMR0h TMR0)
	movf	TMR0,W
	addwf	T2l,F
	movf	TMR0h,W
	btfsc	STATUS,C
	incf	TMR0h,W
	addwf	T2h,F

	djnz	Count1,raloop
	return

;calc ZXact from ZXcal, T2cal and T2
findzact
	mov2	ACCh,ACCl, ZXcalh,ZXcall
	mov2	ARGh,ARGl, T2h,T2l
	call	Mul16x16					;(PROD3..PROD0)=ZXcal*T2
	mov2	DIV1,DIV0, T2calh,T2call
	call	Div32x16					;(ANS1 ANS0)=(PROD3..PROD0)/(DIV1 DIV0)
	mov2	ZXacth,ZXactl, ANS1,ANS0	;ZXact=(ZXcal*T2)/T2cal
	return

;calc Xacc from ZXact, T1X and T2
calcacc
	movf	ZXacth,W
	subwf	T1Xh,W
	jnc		ca1							;if(T1X<ZXact)	goto	ca1
	jnz		ca2							;if(T1X>ZXact)	goto	ca2
	movf	ZXactl,W
	subwf	T1Xl,W
	jnc		ca1
ca2
	mov2	ACCh,ACCl, T1Xh,T1Xl
	mov2	ARGh,ARGl, ZXacth,ZXactl
	call	Sub16x16					;ACC=ACC-ARG=T1X-ZXact
	mvi2	ARGh,ARGl, Kh,Kl
	call	Mul16x16					;(PROD3..PROD0)=K*(T1X-ZXact)
	mov2	DIV1,DIV0, T2h,T2l
	call	Div32x16
	comf	ANS0,W
	addlw	1
	movwf	Xacc						;Xacc=-K*(ZXact-T1X)/T2act
	return
ca1
	mov2	ACCh,ACCl, ZXacth,ZXactl
	mov2	ARGh,ARGl, T1Xh,T1Xl
	call	Sub16x16					;ACC=ACC-ARG=ZXact-T1X
	mvi2	ARGh,ARGl, Kh,Kl
	call	Mul16x16					;(PROD3..PROD0)=K*(ZXact-T1X)
	mov2	DIV1,DIV0,T2h,T2l
	call	Div32x16
	mov		Xacc,ANS0					;Xacc=K*(T1X-ZXact)/T2act
	return

	end