PICの条件分岐について
お世話になります。PICについて質問があります。ボタンを押せば、PORTBのLEDに対する出力が変わるという物ですが、意図したようになりません。PORTに対する設定は間違っていないと思うのですが、チャタリングを考慮したプログラムになっており、上手く条件分岐してくれません。条件というのは、PORTAに入力があれば、25ms待機して再度PORTAを見に行きます。そのときにもPORTAに入力があれば、変数に格納した数値を一つずつ減らしていき、そのときに0であればPORTBを設定したとおりに出力するというものです。以下がソースですが、ここがおかしいということだけでも教えてくださると助かります。よろしくお願いします。
list p=16f628a
#include<p16f628a.inc>
__CONFIG _CP_OFF & _WDT_OFF & _BODEN_OFF & _PWRTE_ON & _INTRC_OSC_NOCLKOUT & _MCLRE_OFF & _LVP_OFF
SWI equ 0x21
SWI2 equ 0x22
CNT1 EQU 0x20
CNT2 EQU 0x21
ORG 0x000
goto main
ORG 0x004
main
bcf STATUS,RP0
bcf STATUS,RP1
clrf INTCON
clrf PORTA
movlw 0x07
movwf CMCON
bsf STATUS,RP0
bsf PCON,OSCF
clrf TRISB
movlw 0x03
movwf TRISA
bcf STATUS,RP0
clrf PORTA
clrf PORTB
clrf SWI
main_loop
btfss PORTA,1
goto flase_wait_routine
goto trues_wait_routine
flase_wait_routine
goto main_loop
trues_wait_routine
call DLY_25
btfss PORTA,1
goto main_loop
incf SWI,f
movf SWI,W
movwf SWI2
decfsz SWI2,f
goto a_loop_1
bsf PORTB,0
bcf PORTB,1
bcf PORTB,2
bcf PORTB,3
goto main_loop
a_loop_1
decfsz SWI2,f
goto a_loop_2
bcf PORTB,0
bsf PORTB,1
bcf PORTB,2
bcf PORTB,3
goto main_loop
a_loop_2
decfsz SWI2,f
goto a_loop_3
bcf PORTB,0
bcf PORTB,1
bsf PORTB,2
bcf PORTB,3
goto main_loop
a_loop_3
decfsz SWI2,f
goto a_loop_4
bcf PORTB,0
bcf PORTB,1
bcf PORTB,2
bsf PORTB,3
clrf SWI
goto main_loop
a_loop_4
goto main_loop
;Delay Routine
DLY_25 ; 25ms
movlw d'25'
movwf CNT1
DLP1;1ms
movlw d'20'
movwf CNT2
DLP2
nop
nop
decfsz CNT2,f
goto DLP2
decfsz CNT1,f
goto DLP1
return
END
補足
どのように訂正したらいいですか? countをどのように加えたらいいですか?