;-----------------------------------------------------------
;
;	SUAC DAS [Pool Cleaner Robot]	May. 2001
;
;-----------------------------------------------------------

;##### Port Defines #####
smr	.equ	h'0fffb0
brr	.equ	h'0fffb1
scr	.equ	h'0fffb2
tdr	.equ	h'0fffb3
ssr	.equ	h'0fffb4
rdr	.equ	h'0fffb5
p1ddr	.equ	h'0fffc0
p1dr	.equ	h'0fffc2
p2ddr	.equ	h'0fffc1
p2dr	.equ	h'0fffc3
p3ddr	.equ	h'0fffc4
p3dr	.equ	h'0fffc6
p4ddr	.equ	h'0fffc5
p4dr	.equ	h'0fffc7
p5ddr	.equ	h'0fffc8
p5dr	.equ	h'0fffca
p7dr	.equ	h'0fffce
paddr	.equ	h'0fffd1
padr	.equ	h'0fffd3
pbddr	.equ	h'0fffd4
pbdr	.equ	h'0fffd6
iprb	.equ	h'0ffff8
tstr	.equ	h'0fff60
tsnc	.equ	h'0fff61
tmdr	.equ	h'0fff62
tfcr	.equ	h'0fff63
toer	.equ	h'0fff90
tocr	.equ	h'0fff91
tcr0	.equ	h'0fff64
tior0	.equ	h'0fff65
tier0	.equ	h'0fff66
tsr0	.equ	h'0fff67
tcnt0	.equ	h'0fff68
gra0	.equ	h'0fff6a

;##### Vector Defines #####
	.section vector,data,locate=h'000000
	.data.l	start
	.org	h'000060
	.data.l	int_timer0

;##### Work RAM Data Defines #####
	.section ram,data,locate=h'0fef10
timer1	.res.w	1
stat0	.res.w	1
stat1	.res.w	1
stat2	.res.w	1
ctr0	.res.w	1
ctr1	.res.w	1
ctr2	.res.w	1
mode	.res.b	1
flag	.res.b	1
status	.res.b	1
statold	.res.b	1
inp	.res.b	4

;***** Reset --> Initialize --> Main Loop *****
	.section program,code,locate=h'001000
start:
	mov.l	#h'0fff0f,er7		; stack pointer set
	mov.l	#h'0fef10,er2
	mov.w	#h'0fe0,r1
	mov.b	#0,r0l
_ram_clear:
	mov.b	r0l,@er2
	inc.l	#1,er2
	dec.w	#1,r1
	bne	_ram_clear
	mov.b	#b'00100011,r0l
	mov.b	r0l,@tcr0		; Timer0 init : 2MHz clock
	bclr	#0,@tsr0		; Clear IMFA
	bset	#0,@tier0		; timer INT set
	mov.w	#2000,r0		; 1msec INT
	mov.w	r0,@gra0
	mov.b	#b'00011111,r0l
	mov.b	r0l,@p1ddr		; set : Port[1] output/input
	mov.b	#b'11111111,r0l
	mov.b	r0l,@p4ddr		; set : Port[4] all output
	mov.b	r0l,@p5ddr		; set : Port[5] all output
	mov.b	r0l,@paddr		; set : Port[A] all output
	mov.b	#b'00000000,r0l
	mov.b	r0l,@p2ddr		; set : Port[2] all input
	mov.b	r0l,@p3ddr		; set : Port[3] all input
	mov.b	r0l,@pbddr		; set : Port[B] all input
	mov.b	r0l,@padr		; Port[A] all off
	mov.b	r0l,@p4dr
	mov.b	r0l,@p5dr
	mov.b	#b'00000111,r0l
	mov.b	r0l,@p1dr
	jsr	@wait_500msec
	bset	#0,@tstr		; timer start !
	andc.b	#b'01111111,ccr		; interrupt enable
main_loop:
	jsr	@timer_check
	jsr	@status_check
	jmp	@main_loop

;***** Timer / Counter Routines *****
wait_500msec:
	mov.l	#500,er1
_wait_1:
	jsr	@wait_1msec
	sub.l	#1,er1
	bne	_wait_1
	rts
wait_1msec:
	mov.l	#2048,er2
_wait_2:
	sub.l	#1,er2
	bne	_wait_2
	rts
int_timer0:
	push.w	r0
	mov.b	#1,r0l
	mov.b	r0l,@flag
	btst	#0,@tsr0
	bclr	#0,@tsr0		; Clear IMFA
	pop.w	r0
	rte
timer_check:
	mov.b	@flag,r0l
	cmp.b	#0,r0l
	bne	_timer_event
	rts
_timer_event:
	mov.b	#0,r0l
	mov.b	r0l,@flag
	jsr	@every_count
	mov.w	@timer1,r0
	inc.w	#1,r0
	mov.w	r0,@timer1
	cmp.w	#1750,r0
	beq	_1750msec
	rts
_1750msec:
	mov.w	#0,r0
	mov.w	r0,@timer1
	mov.b	@mode,r0l
	inc.b	r0l
	mov.b	r0l,@mode
	cmp.b	#1,r0l
	beq	_mode_1
	cmp.b	#2,r0l
	beq	_mode_2
_mode_0:
	mov.b	#0,r0l
	mov.b	r0l,@mode
	mov.b	#b'00000010,r0l
	mov.b	r0l,@padr
	mov.w	#0,r0
	mov.w	r0,@stat0
	bra	_mode_exit
_mode_1:
	mov.b	#b'00000100,r0l
	mov.b	r0l,@padr
	mov.w	#0,r0
	mov.w	r0,@stat1
	bra	_mode_exit
_mode_2:
	mov.b	#b'00001000,r0l
	mov.b	r0l,@padr
	mov.w	#0,r0
	mov.w	r0,@stat2
_mode_exit:
	mov.b	@mode,r0l
	shll.b	r0l
	shll.b	r0l
	shll.b	r0l
	shll.b	r0l
	and.b	#b'00110000,r0l
	mov.b	r0l,@p4dr
	rts
every_count:
	mov.b	@mode,r0l
	cmp.b	#0,r0l
	beq	mode_0
	cmp.b	#1,r0l
	beq	mode_1
	cmp.b	#2,r0l
	beq	mode_2
	rts
mode_0:
	mov.b	@p1dr,r0l
	and.b	#b'00100000,r0l		; input sensor [Low]
	cmp.b	#0,r0l
	beq	_mode_0_clear
	mov.w	@stat0,r0
	inc.w	#1,r0
	mov.w	r0,@stat0
	cmp.w	#270,r0			; TIMER
	bpl	_mode_0_on
_mode_0_off:
	mov.b	@status,r0l
	and.b	#b'11111110,r0l
_mode_0_exit:
	and	#b'00000111,r0l
	mov.b	r0l,@status
	mov.b	r0l,@p1dr
	rts
_mode_0_clear:
	mov.w	#0,r0
	mov.w	r0,@stat0
	bra	_mode_0_off
_mode_0_on:
	mov.b	@status,r0l
	or.b	#b'00000001,r0l
	bra	_mode_0_exit
	rts
mode_1:
	mov.b	@p1dr,r0l
	and.b	#b'01000000,r0l		; input sensor [Mid]
	cmp.b	#0,r0l
	beq	_mode_1_clear
	mov.w	@stat1,r0
	inc.w	#1,r0
	mov.w	r0,@stat1
	cmp.w	#270,r0			; TIMER
	bpl	_mode_1_on
_mode_1_off:
	mov.b	@status,r0l
	and.b	#b'11111101,r0l
_mode_1_exit:
	and	#b'00000111,r0l
	mov.b	r0l,@status
	mov.b	r0l,@p1dr
	rts
_mode_1_clear:
	mov.w	#0,r0
	mov.w	r0,@stat1
	bra	_mode_1_off
_mode_1_on:
	mov.b	@status,r0l
	or.b	#b'00000010,r0l
	bra	_mode_1_exit
	rts
mode_2:
	mov.b	@p1dr,r0l
	and.b	#b'10000000,r0l		; input sensor [Top]
	cmp.b	#0,r0l
	beq	_mode_2_clear
	mov.w	@stat2,r0
	inc.w	#1,r0
	mov.w	r0,@stat2
	cmp.w	#270,r0			; TIMER
	bpl	_mode_2_on
_mode_2_off:
	mov.b	@status,r0l
	and.b	#b'11111011,r0l
_mode_2_exit:
	and	#b'00000111,r0l
	mov.b	r0l,@status
	mov.b	r0l,@p1dr
	rts
_mode_2_clear:
	mov.w	#0,r0
	mov.w	r0,@stat2
	bra	_mode_2_off
_mode_2_on:
	mov.b	@status,r0l
	or.b	#b'00000100,r0l
	bra	_mode_2_exit
	rts

status_check:
	mov.b	@status,r2l
	mov.b	@statold,r1l
	cmp.b	r2l,r1l
	bne	_stat_1
	rts
_stat_1:
	mov.b	r2l,@statold
	mov.b	#0,r0l
	mov.b	r0l,@p5dr		; Stop
	mov.b	@p7dr,r1l	; SW1
	mov.b	@pbdr,r2l	; SW2
	mov.b	@p2dr,r3l	; SW3
	mov.b	@p3dr,r4l	; SW4
	mov.b	@status,r0l
_stat_2:
	cmp.b	#0,r0l
	beq	_stat_3
	dec.b	r0l
	shlr.b	r1l
	shlr.b	r2l
	shlr.b	r3l
	shlr.b	r4l
	bra	_stat_2
_stat_3:
	and.b	#b'00000001,r1l
	shll.b	r2l
	and.b	#b'00000010,r2l
	shll.b	r3l
	shll.b	r3l
	and.b	#b'00000100,r3l
	shll.b	r4l
	shll.b	r4l
	shll.b	r4l
	and.b	#b'00001000,r4l
	or.b	r4l,r1l
	or.b	r3l,r1l
	or.b	r2l,r1l
	mov.b	r1l,@p5dr
	rts

	.end