RoYbot - Robot bípedo con ASM y PIC16F84

[Fulguitron]

   Muy buenas, desde hace un tiempo estoy liado con robotillo que ya comienza a dar los primeros pasos. Esta hecho totalmente por mi, tanto chasis, como hardware y software (bueno cogi la idea de otro lado y la amplié hasta todo lo que hay hoy en día). Pero actualmente estoy muy parado por un asunto que no logro descifrar. RoY camina de la siguiente forma

Código: [Seleccionar]

Condicion

movf cont_1,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z ;STATUS "Z" si son iguales va a la siguiente línea, si ContC y
goto Movimiento_1 ;y cont_1 no son iguales se salta la siguiente línea.
movf cont_2,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_2
movf cont_3,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_3
movf cont_4,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_4
movf cont_5,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_5
goto CargaMemoria
Cada Movimiento_X es un ciclo en que los seis servos que mueven al robot van de un angulo al otro, una vez los seis servos han llegado al angulo deseado, hacen una señal y cuando todos han llegado, salta al otro mivimiento.

Código: [Seleccionar]

Movimiento_1_2

movf FinAngServ0,W
addwf FinMovimiento,F
movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F

movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal

CargaMovimiento_2
movlw d'1'
movwf cont_1
movlw d'1'
movwf FinAngServ1
movlw d'1'
movwf FinAngServ2
movlw d'1'
movwf FinAngServ3
movlw d'1'
movwf FinAngServ4
movlw d'1'
movwf FinAngServ5
movlw d'1'
movwf FinAngServ6
movlw d'0'
movwf FinMovimiento


Mi problema está en que cada vez que pasa de un movimiento a otro, hay un lapso de tiempo que no logro eliminar y no sé porque sale. Y este es mi problema, necesito eliminar ese lapso de tiempo para que el movimiento sea fluido.
Os cuelgo el código completo para que lo podais chafardear y un video del movimiento y sus paros (todavía tengo que pulir los movimientos pero eso no me preocupa tanto)

Código: [Seleccionar]

list      p=16F84A ; definir procesador

include <p16F84A.inc> ; definiciones de variables específicas del procesador


__CONFIG _CP_OFF &  _WDT_OFF & _PWRTE_ON & _XT_OSC

#define SERVO0 PORTB,0 ; puerto de salida de servo 0
#define SERVO1 PORTB,1 ; puerto de salida de servo 1
#define SERVO2 PORTB,2 ; puerto de salida de servo 2
#define SERVO3 PORTB,3 ; puerto de salida de servo 3
#define SERVO4 PORTB,4 ; puerto de salida de servo 4
#define SERVO5 PORTB,5 ; puerto de salida de servo 5
#define SERVO6 PORTB,6 ; puerto de salida de servo 6

CBLOCK 0x20
Posic ; posición servo
AngServ0 ; memoria del ángulo del servo 0
AngServ1 ; memoria del ángulo del servo 1
AngServ2
AngServ3
AngServ4
AngServ5
AngServ6
FinAngServ0
FinAngServ1
FinAngServ2
FinAngServ3
FinAngServ4
FinAngServ5
FinAngServ6
FinMovimiento
TotalFinMov
grad_50
grad_65
grad_70
grad_75
grad_90
grad_95
grad_105
grad_110
grad_115
grad_120
grad_140
acum_A
cont_0
cont_1
cont_2
cont_3
cont_4
cont_5
cont_A
cont_B
cont_C
contA
ENDC

;***********************************************************************************

org 0x000
goto configuracion

;***********************************************************************************
; Principal
;***********************************************************************************

configuracion



bsf STATUS,RP0 ; Apunta a banco 1
clrf TRISB

movlw b'00011111' ; PORTA
movwf TRISA ; entradas


movlw b'00000010' ; Configuración para TMR0
movwf OPTION_REG ; preescaler 2, 1:8 con CLK interno (que es
; de 1 MHz si el oscilador es de 4 MHz).
; El contador cuenta cada 8 useg

    bcf STATUS,RP0          ; Apunta a banco 0

clrf TMR0 ; inicia registro de timer en 0
; ======================= CARGA DE LOS TIEMPOS PARA LOS DIFERENTES ANGULOS ==================

movlw d'156' ; 50 grados son 800 ms.
movwf grad_50
movlw d'137' ; 65 grados son 950 ms.
movwf grad_65
movlw d'131' ; 70 grados son 1000 ms.
movwf grad_70
movlw d'125' ; 75 grados son 1050 ms.
movwf grad_75
movlw d'106' ; 90 grados son 1200 ms.
movwf grad_90
movlw d'100' ; 90 grados son 1200 ms.
movwf grad_95
movlw d'87' ; 105 grados son 1350 ms.
movwf grad_105
movlw d'81' ; 110 grados son 1400 ms.
movwf grad_110
movlw d'75' ; 110 grados son 1400 ms.
movwf grad_115
movlw d'68' ; 120 grados son 1500 ms.
movwf grad_120


; =====================================================================================

movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ0
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ1
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ2
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ3
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ4
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ5
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ6


; =============================== CARGA DE LAS MEMORIAS INICIALES ====================================

movlw d'2'
movwf cont_1
movlw d'2'
movwf cont_2
movlw d'2'
movwf cont_3
movlw d'2'
movwf cont_4
movlw d'2'
movwf cont_5
movlw d'2'
movwf cont_A
movlw d'1'
movwf cont_C
movlw d'1'
movwf FinAngServ0
movlw d'1'
movwf FinAngServ1
movlw d'1'
movwf FinAngServ2
movlw d'1'
movwf FinAngServ3
movlw d'1'
movwf FinAngServ4
movlw d'1'
movwf FinAngServ5
movlw d'1'
movwf FinAngServ6
movlw d'0'
movwf FinMovimiento
movlw d'0'
movwf TotalFinMov

principal

; Los servos actúan con un pulso de control que va entre 0,5 ms y 2,5 ms.
; En el bloque que sigue se inicia en 1 el pulso de control y se espera un
; retardo fijo de aproximadamente 0,5 ms, que es el valor mínimo del pulso.
; Con este valor de longitud de pulso y una longitud cero en la parte variable,
; el servo está parado al máximo hacia la izquierda (sentido antihorario).

bsf SERVO0 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'218' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer

movf AngServ0,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO0 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer

bsf SERVO1 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'218' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer

movf AngServ1,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO1 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer

bsf SERVO2 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'218' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer

movf AngServ2,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO2 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer

bsf SERVO3 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'218' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer

movf AngServ3,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO3 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer

bsf SERVO4 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'218' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer

movf AngServ4,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO4 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer

bsf SERVO5 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'218' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer

movf AngServ5,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO5 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer

bsf SERVO6 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'218' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer

movf AngServ6,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO6 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer

; ============================================================================


; Retardo de 20 ms, que es el tiempo estándar que debe separar los pulsos
; de control para los servos comunes de RC

movlw d'9' ; cantidad de veces: 9 x 2 ms = 18 ms + 2 ms
movwf acum_A
retardo

bcf INTCON,T0IF ; borra el flag de timer
movlw d'6' ; (256-6 = 250) 250 * 8 us = 2 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
decfsz acum_A,1 ; decrementa cantidad de veces
goto retardo

; En este paso iremos seleccionando la posción a seguir, cuando acaba un movimiento los cont_1, cont_2, ...
; estos son diferentes a cont_C y por lo tanto va al siguiente movimiento.

Condicion

movf cont_1,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z ;STATUS "Z" si son iguales va a la siguiente línea, si ContC y
goto Movimiento_1 ;y cont_1 no son iguales se salta la siguiente línea.
movf cont_2,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_2
movf cont_3,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_3
movf cont_4,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_4
movf cont_5,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_5
goto CargaMemoria

Movimiento_1

movf AngServ0,W
subwf grad_90,W ;revisa el valor entre AngServo_0 y grad_90
btfss STATUS,Z ;si no son iguales ves a incf, si lo son salta la línea
incf AngServ0,1 ;incrementa AngServ0 hasta que llegue a grad_90, o sea hasta que
;AngServ0 llegue a 90 grados.
movf AngServ1,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ1,1

movf AngServ2,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ2,1

movf AngServ3,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ3,1

movf AngServ4,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ4,1

movf AngServ5,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ5,1

movf AngServ6,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ6,1


movf AngServ0,W
subwf grad_90,W ;revisa el valor entre AngSer0 y grad90, si ha llegado y son iguales
btfsc STATUS,Z ;decrementa FinAngServ0 para que no se siga movimendo el servo.
decf FinAngServ0,1

movf AngServ1,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ1,1

movf AngServ2,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ2,1

movf AngServ3,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ3,1

movf AngServ4,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ4,1

movf AngServ5,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ5,1

movf AngServ6,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ6,1

; ------------ Condicion fin del movimiento uno --------------------
; se comprueba que todos los  servos han llegado al final de recorrido para pasar al segundo movimiento.
; si alguno no ha llegado volverá al princpio hasta que lleguen todos. Una vez llegados

Movimiento_1_2

movf FinAngServ0,W
addwf FinMovimiento,F
movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F

movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal
; ---------------------- COMIENZO EL MOVIMIENTO DOS --------------------------------------------
; volvemos a cargar todas las memorias, decrementamos cont_1, de esta forma cuando se vuelva a principal
; ya no pasaremos al movimiento uno y empezará el movimiento 2.

CargaMovimiento_2
movlw d'1'
movwf cont_1
movlw d'1'
movwf FinAngServ1
movlw d'1'
movwf FinAngServ2
movlw d'1'
movwf FinAngServ3
movlw d'1'
movwf FinAngServ4
movlw d'1'
movwf FinAngServ5
movlw d'1'
movwf FinAngServ6
movlw d'0'
movwf FinMovimiento

Movimiento_2

movf AngServ1,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ1,1

movf AngServ2,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ2,1

movf AngServ3,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ3,1

movf AngServ4,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ4,1

movf AngServ5,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ5,1

movf AngServ6,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ6,1


movf AngServ1,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ1,1

movf AngServ2,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ2,1

movf AngServ3,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ3,1

movf AngServ4,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ4,1

movf AngServ5,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ5,1

movf AngServ6,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ6,1


Movimiento_2_2

movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F

movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal


; ------------ Condicion fin del movimiento dos --------------------

; _______________________________________________________________________________________
; _______________________________________________________________________________________


CargaMovimiento_3
movlw d'1'
movwf cont_2

movlw d'1'
movwf FinAngServ3
movlw d'1'
movwf FinAngServ4

movlw d'0'
movwf FinMovimiento
Movimiento_3

movf AngServ3,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ3,1

movf AngServ4,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ4,1

movf AngServ3,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ3,1

movf AngServ4,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ4,1

Movimiento_3_2


movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F


movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal

; ----------------------- COMIENZO DEL MOVIMIENTO CUATRO -------------------------------


CargaMovimiento_4

movlw d'1'
movwf cont_3
movlw d'1'
movwf FinAngServ1
movlw d'1'
movwf FinAngServ2
movlw d'1'
movwf FinAngServ5
movlw d'1'
movwf FinAngServ6
movlw d'0'
movwf FinMovimiento

Movimiento_4

movf AngServ1,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ1,1

movf AngServ2,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ2,1

movf AngServ5,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ5,1

movf AngServ6,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ6,1

movf AngServ1,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ1,1

movf AngServ2,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ2,1

movf AngServ5,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ5,1

movf AngServ6,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ6,1

Movimiento_4_2


movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F

movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal
; ----------------------- COMIENZO DEL MOVIMIENTO CINCO -------------------------------


CargaMovimiento_5
movlw d'1'
movwf cont_4
movlw d'1'
movwf FinAngServ1
movlw d'1'
movwf FinAngServ2
movlw d'1'
movwf FinAngServ3
movlw d'1'
movwf FinAngServ4
movlw d'0'
movwf FinMovimiento

Movimiento_5

; movf AngServ1,W
; subwf grad_90,W
; btfss STATUS,Z
; incf AngServ1,1

; movf AngServ2,W
; subwf grad_90,W
; btfss STATUS,Z
; incf AngServ2,1

movf AngServ3,W
subwf grad_110,W
btfss STATUS,Z
decf AngServ3,1

movf AngServ4,W
subwf grad_110,W
btfss STATUS,Z
decf AngServ4,1

; movf AngServ1,W
; subwf grad_90,W
; btfsc STATUS,Z
; decf FinAngServ1,1

; movf AngServ2,W
; subwf grad_90,W
; btfsc STATUS,Z
; decf FinAngServ2,1

movf AngServ3,W
subwf grad_110,W
btfsc STATUS,Z
decf FinAngServ3,1

movf AngServ4,W
subwf grad_110,W
btfsc STATUS,Z
decf FinAngServ4,1


Movimiento_5_2

movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F

movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal

movlw d'1'
movwf cont_5
goto Condicion

CargaMemoria

movlw d'2'
movwf cont_2
movlw d'2'
movwf cont_3
movlw d'2'
movwf cont_4
movlw d'2'
movwf cont_5
movlw d'1'
movwf FinAngServ1
movlw d'1'
movwf FinAngServ2
movlw d'1'
movwf FinAngServ5
movlw d'1'
movwf FinAngServ6
movlw d'0'
movwf FinMovimiento
goto Movimiento_2


END


Y el video

   Como siempre, muy agradecido, a ver si me echais una mano y puedo seguir con el proyecto.

[willynovi]

Vamos adelante con ese bipedo, no te vayas a rendir 

No estudié con detenimiento el programilla, pero a simple vista veo que usas mucho la instrucción "goto", no digo que esté mal, pero cuando hago algo con ASM la intento usar lo menos posible y tratar de usar "call", que veo no la has usado nunca 

El uso de goto hace los programas mas complicados de leer y tambien al usarlo es como que pierdes un poco el control, no digo que tu programa no funcione por el uso de goto, pero al usarlo tienes mas posibilidades de cometer errores. Trata de usarlo lo menos posible y solo cuando sea estrictamente necesario, como ser el

goto configuracion

Otro tema es el manejo de los servos, has intentado realizar los pulsos usando la interrupción por TMR0?
Creo que vas a tener muchos beneficios ya que no bloqueas el transcurso del programa dedicandote a formar el pulso de cada servo.

Viendo el video, es como que se traba siempre en la misma posición, entonces puede ser que tengas algún bloqueo en esa parte del programa. Pudiste identificar en que parte del programa se traba?

Si te resulta, yo en una epoca agregaba una salida a un LED para indicarme en que parte del programa estaba, digamos que hacia como un debuging 

[willynovi]

A mediados del 2009 en otro hilo se habia tratado algo similar, hay tienes algo de info de como atender los servos con interrupciones.

http://www.todopic.com.ar/foros/index.php?topic=25898.0