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