;-----------------------------------------------------------
;
;	SUAC DAS [MeYonPan]	July 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
timer2	.res.w	1
timer3	.res.w	1
timer4	.res.w	1
timer5	.res.w	1
timer6	.res.w	1
timer7	.res.w	1
timer8	.res.w	1
timer9	.res.w	1
wait1	.res.w	1
wait2	.res.w	1
flag	.res.b	1
pbdata	.res.b	1
nowdata	.res.b	1
olddata	.res.b	1
phase	.res.b	1
status	.res.b	1
on_bit	.res.b	1
current	.res.b	1
target	.res.b	1

;***** 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'11111111,r0l
	mov.b	r0l,@p1ddr		; set : Port[1] all output
	mov.b	r0l,@p2ddr		; set : Port[2] all output
	mov.b	r0l,@pbddr		; set : Port[B] all output
	mov.b	#b'00000000,r0l
	mov.b	r0l,@p3ddr		; set : Port[3] all input
	mov.b	r0l,@paddr		; set : Port[A] all input
	mov.b	r0l,@p4dr
	mov.b	r0l,@p5dr
	mov.b	#b'00000011,r0l
	mov.b	r0l,@pbdata
	mov.b	r0l,@pbdr
	mov.b	#b'00000000,r0l
	mov.b	r0l,@p1dr		; motor off
	mov.b	r0l,@p2dr		; LED off
	jsr	@wait_500msec
	bset	#0,@tstr		; timer start !
	andc.b	#b'01111111,ccr		; interrupt enable
main_loop:
	jsr	@timer_check
	jsr	@sensor_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	@led_check
	jsr	@motor_1msec
	jsr	@release
	rts
release:
	mov.w	@timer6,r0
	cmp.w	#0,r0
	bne	_rel_1
	rts
_rel_1:
	dec.w	#1,r0
	mov.w	r0,@timer6
	cmp.w	#0,r0
	beq	_rel_2
	rts
_rel_2:
	mov.b	#b'00000000,r0l
	mov.b	r0l,@p1dr
	rts

motor_1msec:
	mov.w	@timer4,r0
	inc.w	#1,r0
	mov.w	r0,@timer4
	cmp.w	#45,r0			; speed (msec/step)
	beq	waiter
	rts
waiter:
	mov.w	#0,r0
	mov.w	r0,@timer4
	mov.b	@status,r0l
	cmp.b	#0,r0l
	bne	_motor_go
	rts
_motor_go:
	dec.b	r0l
	mov.b	r0l,@status
	cmp.b	#0,r0l
	bne	_motor_next
	jsr	@sound_off
	mov.w	#2500,r0		; Release Timer (msec)
	mov.w	r0,@timer6
_motor_next:
	mov.b	@phase,r0l
	btst.b	#7,r0l
	bne	normal
reverse:
	inc.b	r0l
	and.b	#b'00000011,r0l
	mov.b	r0l,@phase
_mixmix:
	cmp.b	#0,r0l
	beq	_n_ph0
	cmp.b	#1,r0l
	beq	_n_ph1
	cmp.b	#2,r0l
	beq	_n_ph2
	cmp.b	#3,r0l
	beq	_n_ph3
_n_ph0:
	mov.b	#b'10011001,r0l
	mov.b	r0l,@p1dr
	rts
_n_ph1:
	mov.b	#b'00110011,r0l
	mov.b	r0l,@p1dr
	rts
_n_ph2:
	mov.b	#b'01100110,r0l
	mov.b	r0l,@p1dr
	rts
_n_ph3:
	mov.b	#b'11001100,r0l
	mov.b	r0l,@p1dr
	rts
normal:
	dec.b	r0l
	and.b	#b'00000011,r0l
	mov.b	r0l,r2l
	or.b	#b'10000000,r0l
	mov.b	r0l,@phase
	mov.b	r2l,r0l
	bra	_mixmix
led_check:
	mov.w	@timer1,r0
	inc.w	#1,r0
	mov.w	r0,@timer1
	cmp.w	#1000,r0
	beq	t1000msec
	rts
t1000msec:
	mov.w	#0,r0
	mov.w	r0,@timer1
	mov.w	@timer2,r0
	inc.w	#1,r0
	mov.w	r0,@timer2
	mov.b	@pbdata,r1l
	btst.b	#0,r0l
	beq	_led_on
	or.b	#b'10000000,r1l
	bra	_led_mix
_led_on:
	and.b	#b'01111111,r1l
_led_mix:
	mov.b	r1l,@pbdata
	mov.b	r1l,@pbdr
	rts

sensor_check:
	mov.w	@timer3,r0
	inc.w	#1,r0
	mov.w	r0,@timer3
	cmp.w	#100,r0
	beq	t100wait
	rts
t100wait:
	mov.w	#0,r0
	mov.w	r0,@timer3
	mov.b	@status,r0l
	cmp.b	#0,r0l
	beq	_sense_go
	rts
_sense_go:
	mov.b	@padr,r0l
	xor.b	#b'11111111,r0l
	mov.b	r0l,@nowdata
	mov.b	@olddata,r1l
	xor.b	r0l,r1l
	bne	_event1
	rts
_event1:
	mov.b	@nowdata,r0l
	mov.b	r0l,@olddata
	mov.b	r0l,@p2dr
	mov.b	r1l,r3l		; R3L <-- event bit = 1
	mov.b	@nowdata,r2l
	and	r3l,r2l
	mov.b	r2l,@on_bit
	cmp.b	#0,r2l
	bne	sensor_on	; ON event
	rts
sound_on:
	mov.b	@pbdata,r0l
	and.b	#b'11111110,r0l
	mov.b	r0l,@pbdata
	rts
sound_off:
	mov.b	@pbdata,r0l
	or.b	#b'00000001,r0l
	mov.b	r0l,@pbdata
	rts
sensor_on:
	jsr	@sound_on
	mov.b	@on_bit,r2l	; [R2L] on event sensor bit
	btst.b	#0,r2l
	bne	_sensor_0
	btst.b	#1,r2l
	bne	_sensor_1
	btst.b	#2,r2l
	bne	_sensor_2
	btst.b	#3,r2l
	bne	_sensor_3
	btst.b	#4,r2l
	bne	_sensor_4
	btst.b	#5,r2l
	bne	_sensor_5
	btst.b	#6,r2l
	bne	_sensor_6
	btst.b	#7,r2l
	bne	_sensor_7
_sensor_0:
	mov.b	#0,r1l
	bra	_sensor_mix
_sensor_1:
	mov.b	#1,r1l
	bra	_sensor_mix
_sensor_2:
	mov.b	#2,r1l
	bra	_sensor_mix
_sensor_3:
	mov.b	#3,r1l
	bra	_sensor_mix
_sensor_4:
	mov.b	#4,r1l
	bra	_sensor_mix
_sensor_5:
	mov.b	#5,r1l
	bra	_sensor_mix
_sensor_6:
	mov.b	#6,r1l
	bra	_sensor_mix
_sensor_7:
	mov.b	#7,r1l
_sensor_mix:
	mov.b	r1l,@target
	mov.b	@current,r0l
	and.b	#b'00000111,r0l
	shal.b	r0l
	shal.b	r0l
	shal.b	r0l
	or.b	r0l,r1l		; [R1L] table pointer
	mov.b	#0,r1h
	mov.w	#0,e1
	mov.b	@(database1,er1),r0l
	mov.b	r0l,@status
	mov.b	@(database2,er1),r0l
	xor.b	#b'11111111,r0l
	rotr.b	r0l
	mov.b	@phase,r1l
	and.b	#b'01111111,r1l
	or.b	r1l,r0l
	mov.b	r0l,@phase
	mov.b	@target,r0l
	mov.b	r0l,@current
	rts
database1:
	.data   0,25,50,75,100,75,50,25		; current = 	1
	.data	25,0,25,50,75,100,75,50		;		2
	.data	50,25,0,25,50,75,100,75		;		3
	.data	75,50,25,0,25,50,75,100		;		4
	.data	100,75,50,25,0,25,50,75		;		5
	.data	75,100,75,50,25,0,25,50		;		6
	.data	50,75,100,75,50,25,0,25		;		7
	.data	25,50,75,100,75,50,25,0		;		8
database2:
	.data   0,0,0,0,1,1,1,1			; current = 	1
	.data	1,0,0,0,0,1,1,1			;		2
	.data	1,1,0,0,0,0,1,1			;		3
	.data	1,1,1,0,0,0,0,1			;		4
	.data	1,1,1,1,0,0,0,0			;		5
	.data	0,1,1,1,1,0,0,0			;		6
	.data	0,0,1,1,1,1,0,0			;		7
	.data	0,0,0,1,1,1,1,0			;		8

	.end