Rotina para mover servo (tentando aprender)

Saudaçoes!
Estou tentando aprender a mover um servo com o 16F628A, e estou tendo dificuldades..
Escrevi esta rotina, para que o servo se move-e ao ligar a alimentaçcao do PIC.. sabem pq nao funciona?
Abracos
;**************************************************************
;* Controle para servos
;*
;* Rafael S. Delfino 14-10-2006
;**************************************************************
;
;**************************************************************
;definições do programa
;**************************************************************
radix dec ;padrao->valores decimal
include <P16F628.INC> ;inclui arquivo
__config _WDT_OFF & _CP_OFF & _HS_OSC & _PWRTE_ON ;configura bits
;
;**************************************************************
;variaveis do programa
;**************************************************************
cblock 0x20
t1
t2
pos
endc
;
;**************************************************************
;area do reset
;**************************************************************
org 0x00 ;reset
goto inicio ;desvia do endereco 0x04 - interrupção
;
;**************************************************************
;area das interrupções
;**************************************************************
org 0x04 ;toda interrupção aponta para este endereço
retfie ;retorno de interrupção
;
;**************************************************************
;inicio do programa
;**************************************************************
inicio:
movlw 0x00 ;ajuste para os bits INTCON
movwf INTCON ;desabilita ints
clrf PORTA ;limpa portas
clrf PORTB
bsf STATUS,RP0 ;seleciona banco 1 para options e tris
movlw 0x0E ;ajusta os bits em A como saída
movwf TRISA ;exceto RA1, RA2 e RA3, entradas para chaves
movlw 0x00 ;ajusta os bits em B como saída
movwf TRISB ;não usados
bcf STATUS,RP0 ;volta ao banco 0... (padrão do reset)
movlw 0x0A ;prepara para posicionar o servo no centro
movwf POS ;guarda posição
goto servo1 ;chama subrotina para mover servo
;
;**************************************************************
;loop principal do programa
;**************************************************************
loop:
nop
nop
nop
nop
nop
nop ;perde 6 ciclos
movlw 0x0F ;carrega com valor para mover
movwf POS
servo1:
bsf PORTB,4 ;liga pulso
call Tservo ;aguarda tempo
bcf PORTA,4 ;desliga pulso
call T20ms ;aguarda 20ms
goto loop ;faz infinitamente
;**************************************************************
;Prepara temporização do servo - pulso e 20ms de espera
;**************************************************************
Tservo:
movf POS,W ;carrega W com POS
movwf T1 ;carrega T1 com W
goto car_1 ;inicia temporização
T20ms:
movlw 0x78 ;carrega W com 20ms
movwf T1 ;carrega T1 com W
goto car_1 ;inicia temporização
;
;**************************************************************
;subrotina para temporização
;**************************************************************
car_1:
movlw 0xff ;carrega W com 255
movwf T2 ;carrega T2 com 255
dec_1:
decfsz T2,1 ;decrementa T2
goto dec_1 ;255 x T1 vezes
decfsz T1,1 ;decrementa T1
goto car_1 ;volta a carregar T2
retlw 0x00 ;retorna
;
;**************************************************************
;fim do programa
;**************************************************************
end
Estou tentando aprender a mover um servo com o 16F628A, e estou tendo dificuldades..
Escrevi esta rotina, para que o servo se move-e ao ligar a alimentaçcao do PIC.. sabem pq nao funciona?
Abracos
;**************************************************************
;* Controle para servos
;*
;* Rafael S. Delfino 14-10-2006
;**************************************************************
;
;**************************************************************
;definições do programa
;**************************************************************
radix dec ;padrao->valores decimal
include <P16F628.INC> ;inclui arquivo
__config _WDT_OFF & _CP_OFF & _HS_OSC & _PWRTE_ON ;configura bits
;
;**************************************************************
;variaveis do programa
;**************************************************************
cblock 0x20
t1
t2
pos
endc
;
;**************************************************************
;area do reset
;**************************************************************
org 0x00 ;reset
goto inicio ;desvia do endereco 0x04 - interrupção
;
;**************************************************************
;area das interrupções
;**************************************************************
org 0x04 ;toda interrupção aponta para este endereço
retfie ;retorno de interrupção
;
;**************************************************************
;inicio do programa
;**************************************************************
inicio:
movlw 0x00 ;ajuste para os bits INTCON
movwf INTCON ;desabilita ints
clrf PORTA ;limpa portas
clrf PORTB
bsf STATUS,RP0 ;seleciona banco 1 para options e tris
movlw 0x0E ;ajusta os bits em A como saída
movwf TRISA ;exceto RA1, RA2 e RA3, entradas para chaves
movlw 0x00 ;ajusta os bits em B como saída
movwf TRISB ;não usados
bcf STATUS,RP0 ;volta ao banco 0... (padrão do reset)
movlw 0x0A ;prepara para posicionar o servo no centro
movwf POS ;guarda posição
goto servo1 ;chama subrotina para mover servo
;
;**************************************************************
;loop principal do programa
;**************************************************************
loop:
nop
nop
nop
nop
nop
nop ;perde 6 ciclos
movlw 0x0F ;carrega com valor para mover
movwf POS
servo1:
bsf PORTB,4 ;liga pulso
call Tservo ;aguarda tempo
bcf PORTA,4 ;desliga pulso
call T20ms ;aguarda 20ms
goto loop ;faz infinitamente
;**************************************************************
;Prepara temporização do servo - pulso e 20ms de espera
;**************************************************************
Tservo:
movf POS,W ;carrega W com POS
movwf T1 ;carrega T1 com W
goto car_1 ;inicia temporização
T20ms:
movlw 0x78 ;carrega W com 20ms
movwf T1 ;carrega T1 com W
goto car_1 ;inicia temporização
;
;**************************************************************
;subrotina para temporização
;**************************************************************
car_1:
movlw 0xff ;carrega W com 255
movwf T2 ;carrega T2 com 255
dec_1:
decfsz T2,1 ;decrementa T2
goto dec_1 ;255 x T1 vezes
decfsz T1,1 ;decrementa T1
goto car_1 ;volta a carregar T2
retlw 0x00 ;retorna
;
;**************************************************************
;fim do programa
;**************************************************************
end