| CODE |
| ;------------------------------------------------------------------------------ ; 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 |