
;------------------------------------------------------------------------------
; Title       : bsa.asm
; Description : Brick Spine for AVR
;               The original is a "Brick Spine 2" which Mr. Hirota developed.
; Author      : KAWAMOTO "nanashino" Yasuhisa
; Email       : nanashi@yk.rim.or.jp
; Date        : 09/22/2000
;------------------------------------------------------------------------------

;------------------------------------------------------------------------------
; History     : 09/22/2000  TOV0フラグのクリア忘れ修正
;               09/04/2000  初期バージョン
;------------------------------------------------------------------------------

.include "2313def.inc"

;-- Defined constant --
.equ	N0    = 125		; 250us@4.0MHz and R0=2
.equ	R0    = 2		; 1/8
.equ	N1    = 10000		; 2.5ms@4.0MHz and R1=1
.equ	PWMP1 = 1000		; 0.25ms@4.0MHz and R1=1
.equ	PWMW1 = 533		; 133us@4.0MHz and R1=1
.equ	R1    = 1		; 1/1

;-- Global register --
.def	t_tmp		= r16	; Scratch register(in interrupt handler)
.def	t_tmp2		= r17	; Scratch register2(in interrupt handler)
.def	t_tmp3		= r18	; Scratch register3(in interrupt handler)
.def	t_sr		= r19	; SREG storage(in interrupt handler)
.def	t_reld0		= r20	; Reload register0(in tim0_ovf)
.def	rcx_wc		= r1	; RCX width counter
.def	rcx_c0		= r2	; RCX counter0
.def	rcx_c1		= r3	; RCX counter1
.def	rcx_c2		= r4	; RCX counter2
.def	rcx_c3		= r5	; RCX counter3
.def	rcx_bf0		= r6	; RCX buffer0
.def	rcx_bf1		= r7	; RCX buffer1
.def	rcx_st		= r21	; RCX status
.def	t_reld1h	= r22	; Reload high register1(in tim1_ovf)
.def	t_reld1l	= r23	; Reload low register1(in tim1_ovf)
.def	pwm_oc		= r24	; PWM output channel
.def	tmp		= r25	; Scratch register
.def	tmp2		= r26	; Scratch register2
; r28-r29 is Y-Register
; r30-r31 is Z-Register

;-- Bit position in Status register --
.equ	RCBF		= 7	; RCX command buffer full

	.dseg
	.org $60
;-- Servo PWM paramter --
pwm_in:
	.byte 8
pwm_out:
	.byte 8
pwm_r1:
	.byte 8
pwm_r2:
	.byte 8

	.cseg
;-- Reset and interrupt vector --
	.org $0000
	rjmp	main		; Reset Handler
	.org INT0addr
	reti			; IRQ0 Handler(Not Used)
	.org INT1addr
	reti			; IRQ1 Handler(Not Used)
	.org ICP1addr
	reti			; Timer1 Capture Handler(Not Used)
	.org OC1addr
	rjmp	tim_comp1	; Timer1 Compare Handler
	.org OVF1addr
	rjmp	tim1_ovf	; Timer1 Overflow Handler
	.org OVF0addr
	rjmp	tim0_ovf	; Timer0 Overflow Handler
	.org URXCaddr
	reti			; UART RX Complete Handler(Not Used)
	.org UDREaddr
	reti			; UDR Empty Handler(Not Used)
	.org UTXCaddr
	reti			; UART TX Complete Handler(Not Used)
	.org ACIaddr
	reti			; Analog Comparator Handler(Not Used)

;-- RCX signal receive(in tim0_ovf) --
rcx:
	sbis	PIND,0			; RCX-A FWD
	inc	rcx_c0
	sbis	PIND,1			; RCX-A RVS
	inc	rcx_c1
	sbis	PIND,2			; RCX-B FWD
	inc	rcx_c2
	sbis	PIND,3			; RCX-B RVS
	inc	rcx_c3

	dec	rcx_wc
	brne	rcx_exit

	sbrc	rcx_st,RCBF		; Check clear RCBF
	rjmp	rcx_skip

	inc	rcx_c0			; rcx_c0 = (rcx_c0+2)/4
	inc	rcx_c0
	lsr	rcx_c0
	lsr	rcx_c0
	inc	rcx_c1			; rcx_c1 = (rcx_c1+2)/4
	inc	rcx_c1
	lsr	rcx_c1
	lsr	rcx_c1
	inc	rcx_c2			; rcx_c2 = (rcx_c2+2)/4
	inc	rcx_c2
	lsr	rcx_c2
	lsr	rcx_c2
	inc	rcx_c3			; rcx_c3 = (rcx_c3+2)/4
	inc	rcx_c3
	lsr	rcx_c3
	lsr	rcx_c3

	tst	rcx_c0			; if rcx_c0 != 0 goto rcx_00
	brne	rcx_00
	tst	rcx_c1			; if rcx_c1 == 0 goto rcx_00
	breq	rcx_00
	ldi	t_tmp,8			; rcx_c0 = rcx_c1+8
	add	rcx_c1,t_tmp
	mov	rcx_c0,rcx_c1
rcx_00:
	tst	rcx_c2			; if rcx_c2 != 0 goto rcx_01
	brne	rcx_01
	tst	rcx_c3			; if rcx_c3 == 0 goto rcx_01
	breq	rcx_01
	ldi	t_tmp,8			; rcx_c2 = rcx_c3+8
	add	rcx_c3,t_tmp
	mov	rcx_c2,rcx_c3
rcx_01:
	cp	rcx_bf0,rcx_c0		; if rcx_bf0 == rcx_c0 goto rcx_02
	breq	rcx_02
	mov	rcx_bf0,rcx_c0
	rjmp	rcx_skip
rcx_02:
	cp	rcx_bf1,rcx_c2		; if rcx_bf1 == rcx_c2 goto rcx_03
	breq	rcx_03
	mov	rcx_bf1,rcx_c2
	rjmp	rcx_skip
rcx_03:
	tst	rcx_bf0			; if rcx_bf0 == 0 goto rcx_skip
	breq	rcx_skip
	tst	rcx_bf1			; if rcx_bf1 == 0 goto rcx_skip
	breq	rcx_skip

	sbr	rcx_st,(1<<RCBF)	; Set RCBF
rcx_skip:
	ldi	t_tmp,32		; 250us x 32 = 8ms
	mov	rcx_wc,t_tmp
	clr	rcx_c0
	clr	rcx_c1
	clr	rcx_c2
	clr	rcx_c3
rcx_exit:
	ret

;-- PWM output low(in tim1_ovf) --
pwml:
	clr	t_tmp
	out	PORTB,t_tmp		; PWM signal low output

	lsl	pwm_oc
	brne	pwm_00
	ldi	t_tmp,0b00000001
	mov	pwm_oc,t_tmp
pwm_00:
	sbrc	pwm_oc,0
	lds	t_tmp,pwm_out
	sbrc	pwm_oc,1
	lds	t_tmp,pwm_out+1
	sbrc	pwm_oc,2
	lds	t_tmp,pwm_out+2
	sbrc	pwm_oc,3
	lds	t_tmp,pwm_out+3
	sbrc	pwm_oc,4
	lds	t_tmp,pwm_out+4
	sbrc	pwm_oc,5
	lds	t_tmp,pwm_out+5
	sbrc	pwm_oc,6
	lds	t_tmp,pwm_out+6
	sbrc	pwm_oc,7
	lds	t_tmp,pwm_out+7
	ldi	t_tmp3,high(65536-PWMP1+9)
	ldi	t_tmp2,low(65536-PWMP1+9)
pwm_01:
	cpi	t_tmp,0
	breq	pwm_02
	subi	t_tmp2,low(PWMW1)
	sbci	t_tmp3,high(PWMW1)
	dec	t_tmp
	rjmp	pwm_01
pwm_02:
	out	OCR1AH,t_tmp3		; Set compare register
	out	OCR1AL,t_tmp2
	ret

;-- PWM output high(in tim_comp1) --
pwmh:
	out	PORTB,pwm_oc		; PWM signal high output
	ret

;-- Timer0 Overflow Handler --
tim0_ovf:
	in	t_sr,SREG		; Store SREG
	out	TCNT0,t_reld0		; Set reload timer0
	rcall	rcx			; RCX signal receive
	out	SREG,t_sr		; Restore SREG
	reti

;-- Timer1 Overflow Handler --
tim1_ovf:
	in	t_sr,SREG		; Store SREG
	out	TCNT1H,t_reld1h		; Set reload timer1
	out	TCNT1L,t_reld1l
	rcall	pwml			; PWM output low
	out	SREG,t_sr		; Restore SREG
	reti

;-- Timer1 Compare Handler --
tim_comp1:
	in	t_sr,SREG		; Store SREG
	rcall	pwmh			; PWM output high
	out	SREG,t_sr		; Restore SREG
	reti

;-- Init hardware --
init_hw:
	ldi	tmp,(256-N0+8/8)	; Set reload value
	mov	t_reld0,tmp
	ldi	rcx_st,0b00000000
	ldi	tmp,32			; 250us x 32 = 8ms
	mov	rcx_wc,tmp
	clr	rcx_c0
	clr	rcx_c1
	clr	rcx_c2
	clr	rcx_c3
	clr	rcx_bf0
	clr	rcx_bf1

	ldi	tmp,high(65536-N1+9)	; Set reload value
	mov	t_reld1h,tmp
	ldi	tmp,low(65536-N1+9)
	mov	t_reld1l,tmp

	ldi	ZH,high(pwm_in)		; Init pwm_in and pwm_out, pwm_r1, pwm_r2
	ldi	ZL,low(pwm_in)
	ldi	tmp,7
	ldi	tmp2,8*4
init_loop1:
	st	Z+,tmp
	dec	tmp2
	brne	init_loop1

	ldi	tmp,0b00000001
	mov	pwm_oc,tmp

	ldi	tmp,0b11111111		; PB7-PB0(output)
	out	DDRB,tmp
	ldi	tmp,0b11111111
	out	PORTB,tmp
	ldi	tmp,0b01110000		; PD6-PD4(output),PD3-PD0(input)
	out	DDRD,tmp
	ldi	tmp,0b01110000
	out	PORTD,tmp

	ldi	tmp,R0
	out	TCCR0,tmp		; Start timer0 and set clock source
	out	TCNT0,t_reld0

	ldi	tmp,$00
	out	TCCR1A,tmp
	ldi	tmp,R1
	out	TCCR1B,tmp		; Start timer1 and set clock source
	out	TCNT1H,t_reld1h
	out	TCNT1L,t_reld1l

	ldi	tmp,((1<<TOV1)|(1<<OCF1A)|(1<<TOV0)) ; Clear flag
	out	TIFR,tmp
	ldi	tmp,((1<<TOIE1)|(1<<OCIE1A)|(1<<TOIE0)) ; Enable interrupt
	out	TIMSK,tmp

	ret

;-- Main --
main:
	ldi	tmp,low(RAMEND)		; Set stack-pointer
	out	SPL,tmp
	rcall	init_hw			; Init hardware
	sei				; Enable interrupt
loop:
	sbrs	rcx_st,RCBF		; Check set RCBF
	rjmp	loop
	mov	tmp,rcx_bf0
	mov	tmp2,rcx_bf1
	cbr	rcx_st,(1<<RCBF)	; Clear RCBF
	dec	tmp
	dec	tmp2
pos0:
	cpi	tmp2,0
	brne	pos1
	sts	pwm_in,tmp		; pwm_in <- servo position
	rjmp	loop
pos1:
	cpi	tmp2,1
	brne	pos2
	sts	pwm_in+1,tmp		; pwm_in + 1 <- servo position
	rjmp	loop
pos2:
	cpi	tmp2,2
	brne	pos3
	sts	pwm_in+2,tmp		; pwm_in + 2 <- servo position
	rjmp	loop
pos3:
	cpi	tmp2,3
	brne	pos4
	sts	pwm_in+3,tmp		; pwm_in + 3 <- servo position
	rjmp	loop
pos4:
	cpi	tmp2,4
	brne	pos5
	sts	pwm_in+4,tmp		; pwm_in + 4 <- servo position
	rjmp	loop
pos5:
	cpi	tmp2,5
	brne	pos6
	sts	pwm_in+5,tmp		; pwm_in + 5 <- servo position
	rjmp	loop
pos6:
	cpi	tmp2,6
	brne	pos7
	sts	pwm_in+6,tmp		; pwm_in + 6 <- servo position
	rjmp	loop
pos7:
	cpi	tmp2,7
	brne	cmd
	sts	pwm_in+7,tmp		; pwm_in + 7 <- servo position
	rjmp	loop
cmd:
	cpi	tmp2,14
	brne	loop
cmd0:
	cpi	tmp,0
	brne	cmd1
	ldi	ZH,high(pwm_in)		; pwm_out <- pwm_in
	ldi	ZL,low(pwm_in)
	ldi	YH,high(pwm_out)
	ldi	YL,low(pwm_out)
	ldi	tmp,8
loop0:
	ld	tmp2,Z+
	st	Y+,tmp2
	dec	tmp
	brne	loop0
	rjmp	loop
cmd1:
	cpi	tmp,1
	brne	cmd2
	ldi	ZH,high(pwm_in)		; pwm_r1 <- pwm_in
	ldi	ZL,low(pwm_in)
	ldi	YH,high(pwm_r1)
	ldi	YL,low(pwm_r1)
	ldi	tmp,8
loop1:
	ld	tmp2,Z+
	st	Y+,tmp2
	dec	tmp
	brne	loop1
	rjmp	loop
cmd2:
	cpi	tmp,2
	brne	cmd5
	ldi	ZH,high(pwm_in)		; pwm_r2 <- pwm_in
	ldi	ZL,low(pwm_in)
	ldi	YH,high(pwm_r2)
	ldi	YL,low(pwm_r2)
	ldi	tmp,8
loop2:
	ld	tmp2,Z+
	st	Y+,tmp2
	dec	tmp
	brne	loop2
	rjmp	loop
cmd5:
	cpi	tmp,5
	brne	cmd6
	ldi	ZH,high(pwm_r1)		; pwm_in <- pwm_r1
	ldi	ZL,low(pwm_r1)
	ldi	YH,high(pwm_in)
	ldi	YL,low(pwm_in)
	ldi	tmp,8
loop5:
	ld	tmp2,Z+
	st	Y+,tmp2
	dec	tmp
	brne	loop5
	rjmp	loop
cmd6:
	cpi	tmp,6
	brne	cmde
	ldi	ZH,high(pwm_r2)		; pwm_in <- pwm_r2
	ldi	ZL,low(pwm_r2)
	ldi	YH,high(pwm_in)
	ldi	YL,low(pwm_in)
	ldi	tmp,8
loop6:
	ld	tmp2,Z+
	st	Y+,tmp2
	dec	tmp
	brne	loop6
cmde:
	rjmp	loop
