Controlar servo con pic16f84a

[Miquel_S]

Excelente explicacion tapi8 me has abierto los ojos con lo de la velocidad de los pwm.
De todos modos posteo otra solucion para conseguir un movimiento suave en los servos,solucion que me la dio un compañero del foro:
 
La idea es así;
Posicion Salida = Posicion Actual + Error * Cte
Posicion Salida = es el valor del pulso que tienes que indicarle al servo
Error = Posicion Deseada - Posicion Actual
Cte = 0,5 (para suavisar la salida)

Ejemplo para ir de un extremo a la posicion central
Posicion Actual = 850us
Posicion Deseada = 1500us
Error = 1500us - 850us = 650us
Posicion Salida = 850us + (650us * 0,5) = 1175us

Cuando hace este loop debes actualizar los datos para cuando lo ejecute nuevamente:
Posicion Actual = 1175us
Posicion Deseada = 1500us
Error = 1500us - 1175us = 325us
Posicion Salidad = 1175us + (325us * 0,5) = 1338us

   Saludos
   Miquel

[tapi8]

Si Miquel yo estoy tratando de hacerlo parecido pero con temporizaciones para cada posicion, es decir un movimiento entre 90º y 180º, con pasos intermedios seria asi:
1.5ms->90º
1.75->115º
2.00->135º
2.25->160º
2.5->180º
Para las de los extremos creo se le podria dar algo mas de tiempo, por ejemplo 1seg. y para las intermedias 1/2seg. de modo que en los extremos se harian repeticiones de 50 veces y en los intermedios de 25 veces ( todo esto lo pienso yo en teoria ) creo que asi deberia ir suave, si son necesarias mas paradas intermedias se pueden hacer.
Quiero hacer esto con interrupcion de TIMER0 para la de 20ms, y que no este liado el procesador haciendo temporizaciones todo el tiempo, y cuando lo pongais en el robot podais atender sensores y otras cosas, pero me esta dando algunos problemas que no me esperaba, a ver si soy tan listo como creia o no lo soy 

tapi8

[tapi8]

A ver si este pograma funciona, Miquel le puse tres pasos intermedios entre 90º y 180º para ver si el movimiento es mas suave, a ver si alguien que sepa mas que yo le hecha un vistazo y le saca defectos que seguro que los tendra, porque nunca he hecho nada para servos, de hecho no sabia como funcionaban hasta hace muy poco.


;Programa para control de 8 servos conectados en PORTA y PORTB en el nible menos significativo
;RA0 a RA3 y RB0 a RB3, se mueven todos al mismo tiempo y los mismos grados paro se puede modificar
;para diferentes movimientos independientes de cada servo


list p=16F84A
#include P16F84A.inc
__CONFIG   _CP_OFF & _WDT_OFF & _PWRTE_ON & _XT_OSC
;**** Definicion de variables ****
CBLOCK   0X20      ; CBLOCK en 20h para que valga para toda la serie 16f

PDel0         ; Contadores para demoras
pdel1
PDel2
PDel3
PDel4
PDel5
PDel6
W_Temp         ; Registro para guardar temporalmente W.-
STATUS_Temp      ; Registro para guardar temporalmete STATUS
CONTADOR_0      ; para repeticiones de trenes de pulsos
CONTADOR_1
CONTADOR_2
CONTADOR_3
CONTADOR_4


ENDC

;**** Inicio del Micro ****
Reset      
   org   0x00      ; Aquí comienza el micro.-
   goto   Inicio      ; Salto a inicio de mi programa.-
;**** Vector de Interrupcion ****
   org   0x04      ; Atiendo Interrupcion.-
   goto   Inicio_ISR

; **** Programa Principal ****
;**** Configuracion de puertos ***
   org   0x05      ; Origen del código de programa.-
Inicio      
   bsf   STATUS,RP0    ; Pasamos de Banco 0 a Banco 1.-
   movlw   b'11000000'   ; RB0 a RB6 como salida para control de servoS
   movwf   TRISB
   movlw   b'00000      ; PORTA salidas
   mowf   PORTA
   movlw   b'00000110'   ; Se selecciona TMR0 y preescaler de 128-
   movwf   OPTION_REG   ; para que al desbordar nos de 19,968 ms
   bcf   STATUS,RP0   ; Paso del Banco 1 al Banco 0
   clrf   PORTA      ; empezamos con servos en 0
   clrf   PORTB
   movlw   .100      ; Cargamos 100 en TMR0 para lograr aprox. 20ms.-
   movwf   TMR0
   movlw   b'10100000'   ; Habilitamos GIE y T0IE (interrupción del TMR0)
   movwf   INTCON
   clrf   CONTADOR_0   ; empieza con contadores a 0
   clrf   CONTADOR_1
   clrf   CONTADOR_2
   clrf   CONTADOR_3
   clrf   CONTADOR_4



;*************control de velocidad de los servos, variando el Nº de repeticiones ********************
;********se varia la velocidad de movimiento, cuanto menor es el Nº mas rapido***********************

MAIN
   
   
   btfss   INTCON,TOIF   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,TOIF   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_1   ; la primera vez no es necesario pero despues si   
   incf   CONTADOR_0   ; cuenta nº veces se desborda TIMER0
   movlw    .50      ; EN 90º Y 180º seran dobles por ser los extremos del movimiento
   subwf   CONTADOR_0,0
   btfsc   STATUS,Z   ; mira si son 50 desbordes
   goto   MAIN_1      ; Z=1 va a siguiente posicion
   call    CENT_SERVOS   ; todavia no, sigue en 90º
   goto   MAIN      ; y vuelve para esperar desborde de TIMER0
   
MAIN_1   


   btfss   INTCON,TOIF   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_1      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,TOIF   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_0   ; se pone a 0 para proxima vez
   incf   CONTADOR_1   ; pasaron 50 veces empieza para 115º
   movlw    .25      ; 25 es el numero de repeticiones para 115º
   subwf   CONTADOR_1,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN_2      ; si, va a siguiente posicion
   call    MOV_1      : no sigue en 115º
   goto   MAIN_1
   
   
MAIN_2

   
   btfss   INTCON,TOIF   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_2      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,TOIF   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_1
   incf   CONTADOR_2   ; si, empieza en 135º
   movlw   .25
   subwf   CONTADOR_2,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_3      ; SI, va a siguiente posicion
   call   MOV_2      ; no, sigue en 135º
   goto   MAIN_2

MAIN_3

   btfss   INTCON,TOIF   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_3      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,TOIF   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_2   
   incf   CONTADOR_3   ; si, empieza en 160º
   movlw   .25
   subwf   CONTADOR_3,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_4      ; SI, va a siguiente posicion
   call   MOV_3      ; no, sigue en 160º
   goto   MAIN_3   
   
   
MAIN_4

   btfss   INTCON,TOIF   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_4      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,TOIF   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_3
   incf   CONTADOR_4   ; si, empieza en 180º
   movlw   .50      ; aqui repite doble de veces por ser final de recorrido
   subwf   CONTADOR_4,0   ; ya se vera si asi va bien
   btfsc   STATUS,Z   ; van 50?
   goto   MAIN_5
   call   MOV_4      ; no, sigue en 180º
   goto   MAIN_4   

RETROCEDER
   
MAIN_5   

   btfss   INTCON,TOIF   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_5      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,TOIF   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_4
   incf   CONTADOR_3   ; si, empieza en 160º
   movlw   .25
   subwf   CONTADOR_3,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_6
   call   MOV_2      ; no, sigue en 160º
   goto   MAIN_5
   
MAIN_6   

   btfss   INTCON,TOIF   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_6      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,TOIF   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_3
   incf   CONTADOR_2   ; si, empieza en 135º
   movlw   .25
   subwf   CONTADOR_2,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_7
   call   MOV_2      ; no, sigue en 135º
   goto   MAIN_6

   
MAIN_7   

   btfss   INTCON,TOIF   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_7      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,TOIF   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion

   clrf   CONTADOR_2
   incf   CONTADOR_1   ; pasaron 50 veces empieza para 115º
   movlw    .25      ; 25 es el numero de repeticiones para 115º
   subwf   CONTADOR_1,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN      ; vuelve al principio a 90º
   call    MOV_1      : no sigue en 115º
   goto   MAIN_7


***************control movimiento angulos de los servos*************************************************


CENT_SERVOS
   movlw   0x0F      ; ponemos dutty a 1
   movwf   PORTA      ; el RA4 lo pase a RA5, RA4 colector abierto
   movlw   0x0F      ; LOS 5 servos de portb a 1
   movwf   PORTB
   call   DEMORA_CENT   ; durante 1.5 ms para centrado servos
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
            ; aqui se ponen todos al mismo tiempo y en la misma posicion
   clrf   PORTB      ; si se quieren posiciones diferentes para cada servo
   return         ; se puedne hacer "bcf" con diferentes tiempos para cada uno



MOV_1   
   movlw   0x0F      ; ponemos dutty a 1 para avance de 90º a 115º
   movwf   PORTA      ; para los 5servos
   movlw   0x0F      ; LOS 5 servos de portb a 1
   movwf   PORTB
   call   DEMORA_1   ; llama demora 1.75ms
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
   clrf   PORTB
   goto   MAIN




MOV_2
   movlw   0x0F      ; ponemos dutty a 1 para avance de 115º A 135º
   movwf   PORTA      ; para los 5servos
   movlw   0x0F      ; LOS 5 servos de portb a 1
   movwf   PORTB
   call   DEMORA_2   ; llama demora 2m
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
   clrf   PORTB
   goto   MAIN
   


MOV_3
   movlw   0x0F      ; ponemos dutty a 1 para avance de 135º a 160º
   movwf   PORTA      ; para los 5servos
   movlw   0x0F      ; LOS 5 servos de portb a 1
   movwf   PORTB
   call   DEMORA_3   ; llama demora 2.25m   
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
   clrf   PORTB
   goto   MAIN
       



MOV_4
   movlw   0x0F      ; ponemos dutty a 1 para avance de 160º a 180º
   movwf   PORTA      ; para los 5servos
   movlw   0x0F      ; LOS 5 servos de portb a 1
   movwf   PORTB
   call   DEMORA_4   ; llama demora 2.5m   
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
   clrf   PORTB
   goto   MAIN



;************ Rutina de servicio de Interrupcion ************************

;********Aqui haremos copia de respaldo porque este programa ira integrado*********
;*****************en otro mayor y puede afectar a dicho programa********************

;copia de registro W y STATUS.-
Inicio_ISR
   movwf   W_Temp      ; Copiamos W a un registro Temporal.
   swapf   STATUS, W   ; Invertimos los nibles del registro STATUS.
   movwf   STATUS_Temp   ; Guardamos STATUS en un registro temporal.
;**** Interrupcion por TMR0 ****
ISR
   btfss   INTCON,T0IF   ; Consultamos si es por TMR0.-
   goto   Fin_ISR      ; No, entonces restauramos valores.-
         

Actualizo_TMR0         ; Actualizo TMR0 para obtener una temporizacion de 20 ms.
   movlw   .100      ; d'100'
   movwf   TMR
   goto   Fin_ISR      ; Restauro valores.-

; Restauramos los valores de W y STATUS.-
Fin_ISR
   swapf   STATUS_Temp,W   ; Invertimos lo nibles de STATUS_Temp.-
   movwf   STATUS
   swapf   W_Temp, f   ; Invertimos los nibles y lo guardamos en el mismo registro.-
   swapf   W_Temp,W   ; Invertimos los nibles nuevamente y lo guardamos en W.-
   retfie         ; Salimos de interrupción.-
;..........................................


DEMORA_CENT          ; temp. de 1.5ms para centrado 90º 

   movlw     .249      ; 1 set numero de repeticion
        movwf     PDel0     ; 1 |
PLoop0  clrwdt              ; 1 clear watchdog
PDelL1  goto PDelL2         ; 2 ciclos delay
PDelL2 
        decfsz    PDel0, 1  ; 1 + (1) es el tiempo 0  ?
        goto      PLoop0    ; 2 no, loop
        clrwdt              ; 1 ciclo delay
        return              ; 2+2 Fin.

DEMORA_1          ; temp.de 1.75ms para 115º 

   movlw     .249      ; 1 set numero de repeticion
        movwf     PDel1     ; 1 |
PLoop1  clrwdt              ; 1 clear watchdog
PDelL3  goto PDelL2         ; 2 ciclos delay
PDelL4  clrwdt              ; 1 ciclo delay
        decfsz    PDel1, 1  ; 1 + (1) es el tiempo 0  ?
        goto      PLoop0    ; 2 no, loop
PDelL5  goto PDelL4         ; 2 ciclos delay
PDelL6 
        return              ; 2+2 Fin.

DEMORA_2          ; temp.de2ms para 135º   

   movlw     .249      ; 1 set numero de repeticion
        movwf     PDel2     ; 1 |
PLoop2  clrwdt              ; 1 clear watchdog
PDelL7  goto PDelL2         ; 2 ciclos delay
PDelL8  goto PDelL3         ; 2 ciclos delay
PDelL9 
        decfsz    PDel2, 1  ; 1 + (1) es el tiempo 0  ?
        goto      PLoop0    ; 2 no, loop
PDelL10  goto PDelL5         ; 2 ciclos delay
PDelL11  clrwdt              ; 1 ciclo delay
        return              ; 2+2 Fin.

DEMORA_3           ; temp. de 2.25ms para 160º

   movlw     .3        ; 1 set numero de repeticion  (B)
        movwf     PDel3     ; 1 |
PLoop3  movlw     .186      ; 1 set numero de repeticion  (A)
        movwf     PDel4     ; 1 |
PLoop4  clrwdt              ; 1 clear watchdog
        decfsz    PDel4, 1  ; 1 + (1) es el tiempo 0  ? (A)
        goto      PLoop4    ; 2 no, loop
        decfsz    PDel3,  1 ; 1 + (1) es el tiempo 0  ? (B)
        goto      PLoop3    ; 2 no, loop
        clrwdt              ; 1 ciclo delay
        return              ; 2+2 Fin.

DEMORA_4          ; temp. de 2.5ms para 180º 

   movlw     .7        ; 1 set numero de repeticion  (B)
        movwf     PDel5     ; 1 |
PLoop5  movlw     .88       ; 1 set numero de repeticion  (A)
        movwf     PDel6     ; 1 |
PLoop6  clrwdt              ; 1 clear watchdog
        decfsz    PDel6, 1  ; 1 + (1) es el tiempo 0  ? (A)
        goto      PLoop6    ; 2 no, loop
        decfsz    PDel5,  1 ; 1 + (1) es el tiempo 0  ? (B)
        goto      PLoop5    ; 2 no, loop
PDelL12  goto PDelL2         ; 2 ciclos delay
PDelL13  clrwdt              ; 1 ciclo delay
        return              ; 2+2 Fin.

[Miquel_S]

Gracias por el programa Tapi8, en cuanto tenga un momento ya que estoy muy liado con el trabajo lo mirare y posteare mi opinion.

Un saludo. 

[Miquel_S]

Hola Tapi8, he estado mirando muy por encima ya te comente que andaba muy mal de tiempo tu programa, y hay una cosa que no entiendo, que no quiere decir que este mal, ya que tampoco soy tan bueno para encontrar fallos a los maestros.
¿Porque haces una temporizacion de 20 ms?
Los 20 ms son contando las dos temporizaciones nivel alto nivel bajo,tu mismo lo explicas un poco mas arriba.

 Saludos.

[tapi8]

Si Miquel los tiempos son de 20ms (19.968ms para ser mas exactos) y los tiempos en alto de 1.5 a 2.5 van incluidos dentro de ese tiempo de 20ms, todos los tiempos y las repeticiones para la velocidad se pueden modificar sin problema, al igual que los angulos del movimiento yo los hice entre 90º y 180º pero si va mejor se pueden hacer entre 45º y 135º por ejemplo.
Cuando le puse los tiempos a fulgitron se complementaban los dos tiempos para hacer los 20ms porque eran los dos por bucle, y creo que no hay otra manera, pero si el de 20ms se hace por TIMER0, los bucles en alto van dentro del tiempo de los 20ms, el procesador solo atiende a los tiempos en alto (1.5 a 2.5) y el resto en bajo no hace nada solo espera a que el TIMER se desborde, si los dos tiempos, alto y bajo, son por bucle tiene que hacerlos el procesador,por tanto tiene que complementar el uno con el otro.
A ver si le echan un ojo los que si son maestros, Brunu, suki, pibe etc y nos dan su opinion, que seguro sera mas valida que la mia.

tapi8

[tapi8]

Mirando el programa vi algunos fallos, que en el MPLAB tenia corregidos pero en el block de notas donde lo habia hecho no, y claro pegue el del block de notas, el que estaba mal como es logico 
Si lo pruebas usa este el otro tiene errores Miquel, este lo saque directamente del MPLAB y en apariencia funciona. Digo en apariencia porque no se como simular el TIMER y para que me salga bien de la interrupcion le tengo que cambiar el RETFIE por un RETURN normal, cosa rara pero es asi.


list p=16F84A
#include P16F84A.inc
__CONFIG   _CP_OFF & _WDT_OFF & _PWRTE_ON & _XT_OSC
;**** Definicion de variables ****
CBLOCK   0X20      ; CBLOCK en 20h para que valga para toda la serie 16f

PDel0         ; Contadores para demoras
PDel1
PDel2
PDel3
PDel4
PDel5
PDel6
W_Temp         ; Registro para guardar temporalmente W.-
STATUS_Temp      ; Registro para guardar temporalmete STATUS
CONTADOR_0      ; para repeticiones de trenes de pulsos
CONTADOR_1
CONTADOR_2
CONTADOR_3
CONTADOR_4


ENDC

;**** Inicio del Micro ****
Reset      
   org   0x00      ; Aquí comienza el micro.-
   goto   Inicio      ; Salto a inicio de mi programa.-
;**** Vector de Interrupcion ****
   org   0x04      ; Atiendo Interrupcion.-
   goto   Inicio_ISR

; **** Programa Principal ****
;**** Configuracion de puertos ***
   org   0x05      ; Origen del código de programa.-
Inicio      
   bsf   STATUS,RP0    ; Pasamos de Banco 0 a Banco 1.-
   movlw   b'11000000'   ; RB0 a RB6 como salida para control de servoS
   movwf   TRISB
   movlw   b'00000'      ; PORTA salidas
   moVwf   PORTA
   movlw   b'00000110'   ; Se selecciona TMR0 y preescaler de 128-
   movwf   OPTION_REG   ; para que al desbordar nos de 19,968 ms
   bcf   STATUS,RP0   ; Paso del Banco 1 al Banco 0
   clrf   PORTA      ; empezamos con servos en 0
   clrf   PORTB
   movlw   .100      ; Cargamos 100 en TMR0 para lograr aprox. 20ms.-
   movwf   TMR0
   movlw   b'10100000'   ; Habilitamos GIE y T0IE (interrupción del TMR0)
   movwf   INTCON
   clrf   CONTADOR_0   ; empieza con contadores a 0
   clrf   CONTADOR_1
   clrf   CONTADOR_2
   clrf   CONTADOR_3
   clrf   CONTADOR_4



;*************control de velocidad de los servos, variando el Nº de repeticiones ********************
;********se varia la velocidad de movimiento, cuanto menor es el Nº mas rapido***********************

MAIN
   
   
   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_1   ; la primera vez no es necesario pero despues si   
   incf   CONTADOR_0   ; cuenta nº veces se desborda TIMER0
   movlw    .50      ; EN 90º Y 180º seran dobles por ser los extremos del movimiento
   subwf   CONTADOR_0,0
   btfsc   STATUS,Z   ; mira si son 50 desbordes
   goto   MAIN_1      ; Z=1 va a siguiente posicion
   call    CENT_SERVOS   ; todavia no, sigue en 90º
   goto   MAIN      ; y vuelve para esperar desborde de TIMER0
   
MAIN_1   


   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_1      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_0   ; se pone a 0 para proxima vez
   incf   CONTADOR_1   ; pasaron 50 veces empieza para 115º
   movlw    .25      ; 25 es el numero de repeticiones para 115º
   subwf   CONTADOR_1,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN_2      ; si, va a siguiente posicion
   call    MOV_1      ; no sigue en 115º
   goto   MAIN_1
   
   
MAIN_2

   
   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_2      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_1
   incf   CONTADOR_2   ; comienza cuentaen 135º
   movlw   .25
   subwf   CONTADOR_2,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_3      ; SI, va a siguiente posicion
   call   MOV_2      ; no, sigue en 135º
   goto   MAIN_2

MAIN_3

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_3      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_2   
   incf   CONTADOR_3   ; si, empieza en 160º
   movlw   .25
   subwf   CONTADOR_3,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_4      ; SI, va a siguiente posicion
   call   MOV_3      ; no, sigue en 160º
   goto   MAIN_3   
   
   
MAIN_4

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_4      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_3
   incf   CONTADOR_4   ; si, empieza en 180º
   movlw   .50      ; aqui repite doble de veces por ser final de recorrido
   subwf   CONTADOR_4,0   ; ya se vera si asi va bien
   btfsc   STATUS,Z   ; van 50?
   goto   MAIN_5
   call   MOV_4      ; no, sigue en 180º
   goto   MAIN_4   

            ;RETROCEDER
   
MAIN_5   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_5      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_4
   incf   CONTADOR_3   ; si, empieza en 160º
   movlw   .25
   subwf   CONTADOR_3,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_6
   call   MOV_2      ; no, sigue en 160º
   goto   MAIN_5
   
MAIN_6   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_6      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_3
   incf   CONTADOR_2   ; si, empieza en 135º
   movlw   .25
   subwf   CONTADOR_2,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_7
   call   MOV_2      ; no, sigue en 135º
   goto   MAIN_6

   
MAIN_7   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_7      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion

   clrf   CONTADOR_2
   incf   CONTADOR_1   ; pasaron 25 veces empieza para 115º
   movlw    .25      ; 25 es el numero de repeticiones para 115º
   subwf   CONTADOR_1,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN      ; vuelve al principio a 90º
   call    MOV_1      ; no sigue en 115º
   goto   MAIN_7


;***************control movimiento angulos de los servos*************************************************


CENT_SERVOS
   movlw   0x0F      ; ponemos dutty a 1 de
   movwf   PORTA      ;
   movlw   0x0F      ; LOS servos de portb a 1
   movwf   PORTB
   call   DEMORA_CENT   ; durante 1.5 ms para centrado servos
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
            ; aqui se ponen todos al mismo tiempo y en la misma posicion
   clrf   PORTB      ; si se quieren posiciones diferentes para cada servo
   return         ; se puedne hacer "bcf" con diferentes tiempos para cada uno



MOV_1   
   movlw   0x0F      ; ponemos dutty a 1 para avance de 90º a 115º
   movwf   PORTA      ; para los 5servos
   movlw   0x0F      ; LOS servos de portb a 1
   movwf   PORTB
   call   DEMORA_1   ; llama demora 1.75ms
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
   clrf   PORTB
   return




MOV_2
   movlw   0x0F      ; ponemos dutty a 1 para avance de 115º A 135º
   movwf   PORTA      ; para los 5servos
   movlw   0x0F      ; LOS 5 servos de portb a 1
   movwf   PORTB
   call   DEMORA_2   ; llama demora 2m
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
   clrf   PORTB
   return
   


MOV_3
   movlw   0x0F      ; ponemos dutty a 1 para avance de 135º a 160º
   movwf   PORTA      ; para los 5servos
   movlw   0x0F      ; LOS 5 servos de portb a 1
   movwf   PORTB
   call   DEMORA_3   ; llama demora 2.25m   
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
   clrf   PORTB
   return
       



MOV_4
   movlw   0x0F      ; ponemos dutty a 1 para avance de 160º a 180º
   movwf   PORTA      ; para los 5servos
   movlw   0x0F      ; LOS 5 servos de portb a 1
   movwf   PORTB
   call   DEMORA_4   ; llama demora 2.5m   
   clrf   PORTA      ; pone a 0 servos y espera final de interrupcion
   clrf   PORTB
   return



;************ Rutina de servicio de Interrupcion ************************

;********Aqui haremos copia de respaldo porque este programa ira integrado*********
;*****************en otro mayor y puede afectar a dicho programa********************

;copia de registro W y STATUS.-
Inicio_ISR
   movwf   W_Temp      ; Copiamos W a un registro Temporal.
   swapf   STATUS, W   ; Invertimos los nibles del registro STATUS.
   movwf   STATUS_Temp   ; Guardamos STATUS en un registro temporal.
;**** Interrupcion por TMR0 ****
ISR
   btfss   INTCON,T0IF   ; Consultamos si es por TMR0.-
   goto   Fin_ISR      ; No, entonces restauramos valores.-
         

Actualizo_TMR0         ; Actualizo TMR0 para obtener una temporizacion de 20 ms.
   movlw   .100      ; d'100'
   movwf   TMR0
   goto   Fin_ISR      ; Restauro valores.-

; Restauramos los valores de W y STATUS.-
Fin_ISR
   swapf   STATUS_Temp,W   ; Invertimos lo nibles de STATUS_Temp.-
   movwf   STATUS
   swapf   W_Temp, f   ; Invertimos los nibles y lo guardamos en el mismo registro.-
   swapf   W_Temp,W   ; Invertimos los nibles nuevamente y lo guardamos en W.-
   retfie         ; Salimos de interrupción.-
;..........................................


DEMORA_CENT          ; temp. de 1.5ms para centrado 90º 

   movlw     .249      ; 1 set numero de repeticion
        movwf     PDel0     ; 1 |
PLoop0  clrwdt              ; 1 clear watchdog
PDelL1  goto PDelL2         ; 2 ciclos delay
PDelL2 
        decfsz    PDel0, 1  ; 1 + (1) es el tiempo 0  ?
        goto      PLoop0    ; 2 no, loop
        clrwdt              ; 1 ciclo delay
        return              ; 2+2 Fin.

DEMORA_1          ; temp.de 1.75ms para 115º 

   movlw     .249      ; 1 set numero de repeticion
        movwf     PDel1     ; 1 |
PLoop1  clrwdt              ; 1 clear watchdog
PDelL3  goto PDelL2         ; 2 ciclos delay
PDelL4  clrwdt              ; 1 ciclo delay
        decfsz    PDel1, 1  ; 1 + (1) es el tiempo 0  ?
        goto      PLoop0    ; 2 no, loop
PDelL5  goto PDelL4         ; 2 ciclos delay
PDelL6 
        return              ; 2+2 Fin.

DEMORA_2          ; temp.de2ms para 135º   

   movlw     .249      ; 1 set numero de repeticion
        movwf     PDel2     ; 1 |
PLoop2  clrwdt              ; 1 clear watchdog
PDelL7  goto PDelL2         ; 2 ciclos delay
PDelL8  goto PDelL3         ; 2 ciclos delay
PDelL9 
        decfsz    PDel2, 1  ; 1 + (1) es el tiempo 0  ?
        goto      PLoop0    ; 2 no, loop
PDelL10  goto PDelL5         ; 2 ciclos delay
PDelL11  clrwdt              ; 1 ciclo delay
        return              ; 2+2 Fin.

DEMORA_3           ; temp. de 2.25ms para 160º

   movlw     .3        ; 1 set numero de repeticion  (B)
        movwf     PDel3     ; 1 |
PLoop3  movlw     .186      ; 1 set numero de repeticion  (A)
        movwf     PDel4     ; 1 |
PLoop4  clrwdt              ; 1 clear watchdog
        decfsz    PDel4, 1  ; 1 + (1) es el tiempo 0  ? (A)
        goto      PLoop4    ; 2 no, loop
        decfsz    PDel3,  1 ; 1 + (1) es el tiempo 0  ? (B)
        goto      PLoop3    ; 2 no, loop
        clrwdt              ; 1 ciclo delay
        return              ; 2+2 Fin.

DEMORA_4          ; temp. de 2.5ms para 180º 

   movlw     .7        ; 1 set numero de repeticion  (B)
        movwf     PDel5     ; 1 |
PLoop5  movlw     .88       ; 1 set numero de repeticion  (A)
        movwf     PDel6     ; 1 |
PLoop6  clrwdt              ; 1 clear watchdog
        decfsz    PDel6, 1  ; 1 + (1) es el tiempo 0  ? (A)
        goto      PLoop6    ; 2 no, loop
        decfsz    PDel5,  1 ; 1 + (1) es el tiempo 0  ? (B)
        goto      PLoop5    ; 2 no, loop
PDelL12  goto PDelL2         ; 2 ciclos delay
PDelL13  clrwdt              ; 1 ciclo delay
        return              ; 2+2 Fin.

   END

[tapi8]

Bueno Miquel a ver si este te gusta, le hice unas modificaciones ahora deberian moverse de manera independiente primero los 4 servos del PORTA de 90º a 140º pasando por 100>110>120>130>140  esto se puede modificar como se quiera. Le puse una rutina de demora variable aprovechando que los movimientos son de 10 en 10º lo que equivale a 111us y mascaras para que no afecte a los pines que no uso, para que si se pone en robot con mas cosas no les afecte.
Suki, Bruno, Leon etc, los que sabeis, a ver si podeis hacerme el favor e echarle un vistazo, en el MPLAB en apariencia funciona, digo esto porque no soy capaz de simular correctamente el desbordamiento del TIMER0 y lo hago poniendo "artifialmente" el flag TOIF a 1, pero tengo que salir de la interrupcion con un return porque con el retfie no va bien

Código: Text

1.  

[tapi8]

Código: Text

1.  

[tapi8]

Segui las instrucciones de Leon pero no fui capaz de ponerlo en el geshi, soy muy torpe lo siento

list p=16F84A
#include P16F84A.inc
__CONFIG   _CP_OFF & _WDT_OFF & _PWRTE_ON & _XT_OSC
;**** Definicion de variables ****
CBLOCK   0X20      ; CBLOCK en 20h para que valga para toda la serie 16f

PDel0         ; Contadores para demor
PDel1
W_Temp         ; Registro para guardar temporalmente W.-
STATUS_Temp      ; Registro para guardar temporalmete STATUS
CONTADOR_0      ; para repeticiones de trenes de pulsos
CONTADOR_1      ; seguramente se pueda hacer con uno solo pero no fui capaz
CONTADOR_2
CONTADOR_3
CONTADOR_4
CONTADOR_5
CONTADOR_6
CONTADOR_7
CONTADOR_8
CONTADOR_9
CONTADOR_10
CONTADOR_11
CONTADOR_12
CONTADOR_13
CONTADOR_14
CONTADOR_15
CONTADOR_16
CONTADOR_17
CONTADOR_18
CONTADOR_19

ENDC

;**** Inicio del Micro ****
Reset      
   org   0x00      ; Aquí comienza el micro.-
   goto   Inicio      ; Salto a inicio de mi programa.-
;**** Vector de Interrupcion ****
   org   0x04      ; Atiendo Interrupcion.-
   goto   Inicio_ISR

; **** Programa Principal ****
;**** Configuracion de puertos ***
   org   0x05      ; Origen del código de programa.-
Inicio      
   bsf   STATUS,RP0    ; Pasamos de Banco 0 a Banco 1.-
   movlw   b'11110000'   ; RB0 a RB3 como salida para control de servoS
   movwf   TRISB      ;
   movlw   b'10000'   ; PORTA, RA0>RA3 salidas
   movwf   PORTA
   movlw   b'00000110'   ; Se selecciona TMR0 y preescaler de 128-
   movwf   OPTION_REG   ; para que al desbordar nos de 19,968 ms
   bcf   STATUS,RP0   ; Paso del Banco 1 al Banco 0
   clrf   PORTA      ; empezamos con servos en 0
   clrf   PORTB
   movlw   .100      ; Cargamos 100 en TMR0 para lograr aprox. 20ms.-
   movwf   TMR0
   movlw   b'10100000'   ; Habilitamos GIE y T0IE (interrupción del TMR0)
   movwf   INTCON
   clrf   CONTADOR_0   ; empieza con contador_0 a 0 por ser el primero que cuenta
            ; los otros se iran borrando en el programa



;*************control de velocidad de los servos, variando el Nº de repeticiones ********************
;********se varia la velocidad de movimiento, cuanto menor es el Nº mas rapido va el servo***********************

MAIN
   
   
   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_19      
   incf   CONTADOR_0,f   ; cuenta nº veces se desborda TIMER0
   movlw    .50      ; EN 90º Y 180º seran dobles por ser los extremos del movimiento
   subwf   CONTADOR_0,0
   btfsc   STATUS,Z   ; mira si son 50 desbord
   goto   MAIN_A100   ; Z=1 va a siguiente posicion
   call    CENT_SERVOS   ; todavia no, sigue en 90º
   goto   MAIN      ; y vuelve para esperar desborde de TIMER0
   

;***********comienzo avance servos PORTA****************************************************

MAIN_A100   


   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_A100   ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_0   
   incf   CONTADOR_1,f   ; pasaron 50 veces empieza para 100º
   movlw    .25      ; 25 es el numero de repeticiones para 100º
   subwf   CONTADOR_1,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN_A110   ; si, va a siguiente posicion
   call    MOV_A100   ; no sigue en 100º
   goto   MAIN_A100
   
   
MAIN_A110

   
   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_A110   ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupci
   clrf   CONTADOR_1
   incf   CONTADOR_2,f   ; si, empieza en 110º
   movlw   .25
   subwf   CONTADOR_2,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_A120   ; SI, va a siguiente posicion
   call   MOV_A110   ; no, sigue en 110º
   goto   MAIN_A110

MAIN_A120

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_A120   ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_2      
   incf   CONTADOR_3,f   ; si, empieza en 160º
   movlw   .25
   subwf   CONTADOR_3,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_A130   ; SI, va a siguiente posicion
   call   MOV_A120   ; no, sigue en 120º
   goto   MAIN_A120   
   
   
MAIN_A130

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_A130   ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_3
   incf   CONTADOR_4,f   ; si, empieza en 130º
   movlw   .25      ; aqui repite doble de veces por ser final de recorrido
   subwf   CONTADOR_4,0   ; ya se vera si asi va bien
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_A140
   call   MOV_A130   ; no, sigue en 130º
   goto   MAIN_A130   

      
   
MAIN_A140   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_A140      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_4   
   incf   CONTADOR_5,f   ; si, empieza en 160º
   movlw   .25
   subwf   CONTADOR_5,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_RA130
   call   MOV_A140   ; no, sigue en 140º
   goto   MAIN_A140
   
;**********retrceder servos PORTA de 140º a 90º**************************************************

MAIN_RA130   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_RA130      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_5   
   incf   CONTADOR_6,f   ; si, empieza en 130º
   movlw   .25
   subwf   CONTADOR_6,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_RA120
   call   MOV_A130   ; no, sigue en 130º
   goto   MAIN_RA130

   
MAIN_RA120   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_RA120   ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion

   clrf   CONTADOR_6
   incf   CONTADOR_7,f   ; pasaron 50 veces empieza para 115º
   movlw    .25      ; 25 es el numero de repeticiones para 115º
   subwf   CONTADOR_7,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN_RA110   ; SI, va a siguiente
   call    MOV_A120   ; no sigue en 120º
   goto   MAIN_RA120


MAIN_RA110

   
   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_RA110      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_7
   incf   CONTADOR_8,f   ; si, empieza en 110º
   movlw   .25
   subwf   CONTADOR_8,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_RA100   ; SI, va a siguiente posicion
   call   MOV_A110   ; no, sigue en 110º
   goto   MAIN_RA110

MAIN_RA100   


   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_RA100      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_8   ; se pone a 0 para proxima vez
   incf   CONTADOR_9   ; pasaron 50 veces empieza para 100º
   movlw    .25      ; 25 es el numero de repeticiones para 100º
   subwf   CONTADOR_9,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN_CENT      ; si, va a siguiente posicion
   call    MOV_A100   ; no sigue en 10º
   goto   MAIN_RA100


MAIN_CENT         ; aqui centra servos despues de movimiento completo PORTA   
            ; para empezar avance de PORTB
   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_CENT      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_9   ; la primera vez no es necesario pero despues si   
   incf   CONTADOR_10,f   ; cuenta nº veces se desborda TIMER0
   movlw    .50      ; EN 90º Y 180º seran dobles por ser los extremos del movimiento
   subwf   CONTADOR_10,0
   btfsc   STATUS,Z   ; mira si son 50 desbordes
   goto   MAIN_A100   ; Z=1 va a siguiente posicion
   call    CENT_SERVOS   ; todavia no, sigue en 90º
   goto   MAIN      ; y vuelve para esperar desborde de TIMER0



;********************movimientos avance  SERVOS PORTB*****************************************



MAIN_B100   


   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_B100      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_10   ; se pone a 0 para proxima vez
   incf   CONTADOR_11,f   ; pasaron 50 veces empieza para 100º
   movlw    .25      ; 25 es el numero de repeticiones para 100º
   subwf   CONTADOR_11,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN_B110   ; si, va a siguiente posicion
   call    MOV_B100   ; no sigue en 1100º
   goto   MAIN_B100
   
   
MAIN_B110

   
   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_B110      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_11
   incf   CONTADOR_12,f   ; si, empieza en 110º
   movlw   .25
   subwf   CONTADOR_12,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_B120   ; SI, va a siguiente posicion
   call   MOV_B110   ; no, sigue en 110º
   goto   MAIN_B110

MAIN_B120

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_B120      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_12   
   incf   CONTADOR_13,f   ; si, empieza en 160º
   movlw   .25
   subwf   CONTADOR_13,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_B130   ; SI, va a siguiente posicion
   call   MOV_B120   ; no, sigue en 120º
   goto   MAIN_B120   
   
   
MAIN_B130

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_B130      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_13
   incf   CONTADOR_14,f   ; si, empieza en 180º
   movlw   .50      ; aqui repite doble de veces por ser final de recorrido
   subwf   CONTADOR_14,0   ; ya se vera si asi va bien
   btfsc   STATUS,Z   ; van 50?
   goto   MAIN_B140
   call   MOV_B130      ; no, sigue en 130º
   goto   MAIN_B130   

      
   
MAIN_B140   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_B140      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_14
   incf   CONTADOR_15,f   ; si, empieza en 160º
   movlw   .25
   subwf   CONTADOR_15,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_RB130
   call   MOV_B140   ; no, sigue en 140º
   goto   MAIN_B140
   
;**********retroceder servos PORTA de 140º a 90º**************************************************

MAIN_RB130   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_RB130      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_15
   incf   CONTADOR_16,f   ; si, empieza en 135º
   movlw   .25
   subwf   CONTADOR_16,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_RA120
   call   MOV_B130   ; no, sigue en 130º
   goto   MAIN_RB130

   
MAIN_RB120   

   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_RB120      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion

   clrf   CONTADOR_16
   incf   CONTADOR_17,f   ; pasaron 50 veces empieza para 115º
   movlw    .25      ; 25 es el numero de repeticiones para 115º
   subwf   CONTADOR_17,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN_RB110   ; SI, va a siguiente
   call    MOV_B120   ; no sigue en 120º
   goto   MAIN_RB120


MAIN_RB110

   
   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_RB110   ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_17
   incf   CONTADOR_18,f   ; si, empieza en 110º
   movlw   .25
   subwf   CONTADOR_18,0
   btfsc   STATUS,Z   ; van 25?
   goto   MAIN_RB100   ; SI, va a siguiente posicion
   call   MOV_B110   ; no, sigue en 110º
   goto   MAIN_RB110

MAIN_RB100   


   btfss   INTCON,2   ; chequea si acabo la interrupcion, si pasaron 20ms
   goto   MAIN_RB100      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
            ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
            ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
   
   bcf   INTCON,2   ; aqui borra bandera interrupcion, se deja sin borrar en la
            ; interrupcion para poder detectar en MAIN la interrupcion
   clrf   CONTADOR_18   ; se pone a 0 para proxima vez
   incf   CONTADOR_19,f   ; pasaron 50 veces empieza para 100º
   movlw    .25      ; 25 es el numero de repeticiones para 100º
   subwf   CONTADOR_19,0
   btfsc   STATUS,Z   ; van 25 veces?
   goto   MAIN      ; si, va a posicion inicial, centra y empieza siguiente paso
   call    MOV_B100   ; no sigue en 10º
   goto   MAIN_RB100

;************DUTTY DE movimiento de centrado a 90º PORTA, PORTB******************************************


CENT_SERVOS
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB los servos estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta
   
      movlw   .12      ; cargamos 12 para que la demora sea de 1336us>90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
      andwf   PORTA,f      ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTB,f      ; y nos deja el valor del nibble alto como estaba
      return         ; retorna a esperar el final de los 20ms

;**************DUTTYS para movimientos servos PORTA de 10 en 10 grados***********************************

MOV_A100   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º, 9X0.111ms de los 90º +3x0.111ms de 0º segun MPLAB 1.336ms
               ; "creo" que es aceptable el desfase de 36us, sino se corrige
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTB,f      ; y nos deja el valor del nibble alto como estaba en PORTB
      movlw   .1
      call   DEMORA      ; hacemos un retardo mas para PORTA, asi se movera a 100º
      movlw   0XF0      ; mascara para PORTA
      andwf   PORTA,f      ; nibble alto como estaba y bajo a 0   
      return         ;


MOV_A110   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.110 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTB,f      ; y nos deja el valor del nibble alto como estaba en PORTB
      movlw   .2      ; cargamos 2 en w para tiempo adicional de PORTA de 0.222ms>20º
      call   DEMORA      ; hacemos 2 retardoS mas para PORTA, asi se movera a 100º
      movlw   0XF0      ; mascara para PORTA
      andwf   PORTA,f      ; nibble alto como estaba y bajo a 0   
      return         ;



MOV_A120   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTB,f      ; y nos deja el valor del nibble alto como estaba en PORTB
      movlw   .3      ; cargamos 3 en w para tiempo adicional de PORTA de 0.333ms>30º
      call   DEMORA      ; hacemos un retardo mas para PORTA, asi se movera a 120º
      movlw   0XF0      ; mascara para PORTA
      andwf   PORTA,f      ; nibble alto como estaba y bajo a 0   
      return         ;

MOV_A130   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTB,f      ; y nos deja el valor del nibble alto como estaba en PORTB
      movlw   .4      ; cargamos 4 en w para tiempo adicional de PORTA de 0.444ms>40º
      call   DEMORA      ; hacemos un retardo mas para PORTA, asi se movera a 130º
      movlw   0XF0      ; mascara para PORTA
      andwf   PORTA,f      ; nibble alto como estaba y bajo a 0   
      return         ;
MOV_A140   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0xF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTB,f      ; y nos deja el valor del nibble alto como estaba en PORTB
      movlw   .5      ; cargamos 5 en w para tiempo adicional de PORTA de 0.555ms>50º
      call   DEMORA      ; hacemos un retardo mas para PORTA, asi se movera a 140º
      movlw   0XF0      ; mascara para PORTA
      andwf   PORTA,f      ; nibble alto como estaba y bajo a 0   
      return         ;


;*****************DUTTYS para movimientos servos PORTB de 10 en 10 grados**********************************

MOV_B100   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTA,f      ; y nos deja el valor del nibble alto como estaba en PORTA
      movlw   .1      ; aunque solo es una repeticion hay que cargar W porque tiene otro valor
      call   DEMORA      ; hacemos un retardo mas para PORTB, asi se movera a 100º
      movlw   0XF0      ; mascara para PORTB
      andwf   PORTB,f      ; nibble alto como estaba y bajo a 0   
      return         ;

MOV_B110   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTA,f      ; y nos deja el valor del nibble alto como estaba en PORTA
      movlw   .2      ; cargamos 2 en w para tiempo adicional de PORTA de 0.222ms>20º
      call   DEMORA      ; hacemos 2 retardoS mas para PORTB, asi se movera a 110º
      movlw   0XF0      ; mascara para PORTA
      andwf   PORTB,f      ; nibble alto como estaba y bajo a 0   
      return         ;

MOV_B120   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTA,f      ; y nos deja el valor del nibble alto como estaba en PORTA
      movlw   .3      ; cargamos 3 en w para tiempo adicional de PORTA de 0.330ms>30º
      call   DEMORA      ; hacemos un retardo mas para PORTB, asi se movera a 120º
      movlw   0XF0      ; mascara para PORTA
      andwf   PORTB,f      ; nibble alto como estaba y bajo a 0   
      return         ;

MOV_B130   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTA,f      ; y nos deja el valor del nibble alto como estaba en PORTA
      movlw   .4      ; cargamos 4 en w para tiempo adicional de PORTA de 0.444ms>40º
      call   DEMORA      ; hacemos un retardo mas para PORTA, asi se movera a 130º
      movlw   0XF0      ; mascara para PORTB
      andwf   PORTB,f      ; nibble alto como estaba y bajo a 0   
      return         ;

MOV_B140   
      movlw   0x0F      ; ponemos dutty a 1 del PORTA y PORTB estan conectados
      iorwf   PORTA,f      ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
      iorwf   PORTB,f      ; y al alto le aplicamos mascara para dejarlo como esta

   
      movlw   .12      ; para 90º
      call   DEMORA      ; durante 0.111 ms para movimientos de 10º aprox
      
         
      movlw   0XF0      ; mascara para que no nos afecte al nibble alto
               ; nos pone a 0 el nibble bajo donde tenemos los servos
      andwf   PORTA,f      ; y nos deja el valor del nibble alto como estaba en PORTA
      movlw   .5      ; cargamos 5 en w para tiempo adicional de PORTA de 0.555ms>50º
      call   DEMORA      ; hacemos un retardo mas para PORTA, asi se movera a 140º
      movlw   0XF0      ; mascara para PORTB
      andwf   PORTB,f      ; nibble alto como estaba y bajo a 0   
      return         ;



;************ Rutina de servicio de Interrupcion ************************

;********Aqui haremos copia de respaldo porque este programa ira integrado*********
;*****************en otro mayor y puede afectar a dicho programa********************

;copia de registro W y STATUS.-
Inicio_ISR
   movwf   W_Temp      ; Copiamos W a un registro Temporal.
   swapf   STATUS, W   ; Invertimos los nibles del registro STATUS.
   movwf   STATUS_Temp   ; Guardamos STATUS en un registro temporal.
;**** Interrupcion por TMR0 ****
ISR
   btfss   INTCON,T0IF   ; Consultamos si es por TMR0.-
   goto   Fin_ISR      ; No, entonces restauramos valores.-
         

Actualizo_TMR0         ; Actualizo TMR0 para obtener una temporizacion de 20 ms.
   movlw   .100      ; d'100'
   movwf   TMR0
   goto   Fin_ISR      ; Restauro valores.-

; Restauramos los valores de W y STATUS.-
Fin_ISR
   swapf   STATUS_Temp,W   ; Invertimos lo nibles de STATUS_Temp.-
   movwf   STATUS
   swapf   W_Temp, f   ; Invertimos los nibles y lo guardamos en el mismo registro.-
   swapf   W_Temp,W   ; Invertimos los nibles nuevamente y lo guardamos en W.-
   retfie         ; Salimos de interrupción.-
;..........................................



DEMORA             ; haremos demoras de 0.111ms equivalentes 10º del servo aprox
   movwf   PDel1      ; la demora multiplicara 0.111 por el numero que traiga w

TOP1   movlw     .26          ; 1 set numero de repeticion
        movwf     PDel0        ; 1 |
PLoop0  clrwdt                ; 1 clear watchdog
        decfsz    PDel0,1     ; 1 + (1) es el tiempo 0  ?
        goto      PLoop0      ; 2 no, loop
PDelL1  goto PDelL2           ; 2 ciclos delay
PDelL2  clrwdt                 ; 1 ciclo delay
   decfsz   PDel1,1
   goto   TOP1
        return                 ; 2+2 Fin.

   END

[tapi8]

Bueno he modificado el codigo anterior, que no funcionaba, cosa rara estando hecho por mi  como ya ponia no era capaz de simular el desborde del TIMER0, ahora que si lo he simulado me di cuenta de un fallo que no caia en el, y que ademas es logico, al dsbordar se setea INTCON,TOIF, y como no lo borro en la interrupcion no puedo salir de la interrupcion con un RETFIE, prque vuelve a entrar en ella, asi que salgo con un return, borro el TOIF y desues hbilito el GIE, ES LA UNICA FOMA DE HACERLO, de hecho funcionaria mejor sin la interupcion pero despues de la lata que me dio la va a llevar, esto lo descubri despues de darle muchas vultas al programa y al coco, tambie afine un poco los tiempos del DUTTY y lo puse el triple de rapido, ahora deberia ir bien, en el MPLAB anda sin problemas. antes hacia un ciclo completo de avance en 11 seg y ahora en 3.53 seg, no se si es muy rapido, no tengo los servos para prbarlo, bueno aqui os lo dejo por si os interesa.

Código: ASM

1. list p=16F84A
2. #include P16F84A.inc
3. __CONFIG   _CP_OFF & _WDT_OFF & _PWRTE_ON & _XT_OSC
4. ;**** Definicion de variables ****
5. CBLOCK  0X20            ; CBLOCK en 20h para que valga para toda la serie 16f
6.  
7. PDel0                   ; Contadores para demor
8. PDel1
9. W_Temp                  ; Registro para guardar temporalmente W.-
10. STATUS_Temp             ; Registro para guardar temporalmete STATUS
11. CONTADOR_0              ; para repeticiones de trenes de pulsos
12. CONTADOR_1              ; seguramente se pueda hacer con uno solo pero no fui capaz
13. CONTADOR_2
14. CONTADOR_3
15. CONTADOR_4
16. CONTADOR_5
17. CONTADOR_6
18. CONTADOR_7
19. CONTADOR_8
20. CONTADOR_9
21. CONTADOR_10
22. CONTADOR_11
23. CONTADOR_12
24. CONTADOR_13
25. CONTADOR_14
26. CONTADOR_15
27. CONTADOR_16
28. CONTADOR_17
29. CONTADOR_18
30. CONTADOR_19
31.  
32. ENDC
33.  
34. ;**** Inicio del Micro ****
35. Reset          
36.         org     0x00            ; Aquí comienza el micro.-
37.         goto    Inicio          ; Salto a inicio de mi programa.-
38. ;**** Vector de Interrupcion ****
39.         org     0x04            ; Atiendo Interrupcion.-
40.         goto    Inicio_ISR
41.  
42. ; **** Programa Principal ****
43. ;**** Configuracion de puertos ***
44.         org     0x05            ; Origen del código de programa.-
45. Inicio         
46.         bsf     STATUS,RP0      ; Pasamos de Banco 0 a Banco 1.-
47.         movlw   b'11110000'     ; RB0 a RB3 como salida para control de servoS
48.         movwf   TRISB           ;
49.         movlw   b'10000'        ; PORTA, RA0>RA3 salidas
50.         movwf   PORTA
51.         movlw   b'00000110'     ; Se selecciona TMR0 y preescaler de 128-
52.         movwf   OPTION_REG      ; para que al desbordar nos de 19,968 ms
53.         bcf     STATUS,RP0      ; Paso del Banco 1 al Banco 0
54.         clrf    PORTA           ; empezamos con servos en 0
55.         clrf    PORTB
56.         movlw   .100            ; Cargamos 100 en TMR0 para lograr aprox. 20ms.-
57.         movwf   TMR0
58.         movlw   b'10100000'     ; Habilitamos GIE y T0IE (interrupción del TMR0)
59.         movwf   INTCON
60.         clrf    CONTADOR_0      ; empieza con contador_0 a 0 por ser el primero que cuenta
61.                                                 ; los otros se iran borrando en el programa
62.                        
63.         clrf                    CONTADOR_1     
64.         clrf                    CONTADOR_2
65.         clrf                    CONTADOR_3
66.         clrf                    CONTADOR_4
67.         clrf                    CONTADOR_5
68.         clrf                    CONTADOR_6
69.         clrf                    CONTADOR_7
70.         clrf                    CONTADOR_8
71.         clrf                    CONTADOR_9
72.         clrf                    CONTADOR_10
73.         clrf                    CONTADOR_11
74.         clrf                    CONTADOR_12
75.         clrf                    CONTADOR_13
76.         clrf                    CONTADOR_14
77.         clrf                    CONTADOR_15
78.         clrf                    CONTADOR_16
79.         clrf                    CONTADOR_17
80.         clrf                    CONTADOR_18
81.         clrf                    CONTADOR_19
82.  
83.  
84. ;*************control de velocidad de los servos, variando el Nº de repeticiones ********************
85. ;********se varia la velocidad de movimiento, cuanto menor es el Nº mas rapido va el servo***********************
86.  
87. MAIN
88.        
89.  
90.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
91.         goto    MAIN            ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
92.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
93.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
94.                 bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
95.                                 ; interrupcion para poder detectar en MAIN la interrupcion     
96.         bsf             INTCON,7       
97.         clrf    CONTADOR_19            
98.         incf    CONTADOR_0,f    ; cuenta nº veces se desborda TIMER0
99.         movlw   .25             ; EN 90º seran dobles por ser el centrado del movimiento
100.         subwf   CONTADOR_0,0
101.         btfsc   STATUS,Z        ; mira si son 25 desbord
102.         goto    MAIN_A100       ; Z=1 va a siguiente posicion
103.         call    CENT_SERVOS     ; todavia no, sigue en 90º
104.         goto    MAIN            ; y vuelve para esperar desborde de TIMER0
105.        
106.  
107. ;***********comienzo avance servos PORTA****************************************************
108.  
109. MAIN_A100      
110.  
111.  
112.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
113.         goto    MAIN_A100       ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
114.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
115.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
116.        
117.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
118.                                 ; interrupcion para poder detectar en MAIN la interrupcion
119.         bsf             INTCON,7
120.         clrf    CONTADOR_0     
121.         incf    CONTADOR_1,f    ; pasaron 8 veces empieza para 100º
122.         movlw   .8              ; 25 es el numero de repeticiones para 100º
123.         subwf   CONTADOR_1,0
124.         btfsc   STATUS,Z        ; van 8 veces?
125.         goto    MAIN_A110       ; si, va a siguiente posicion
126.         call    MOV_A100        ; no sigue en 100º
127.         goto    MAIN_A100
128.        
129.        
130. MAIN_A110
131.  
132.        
133.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
134.         goto    MAIN_A110       ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
135.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
136.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
137.                
138.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
139.                                 ; interrupcion para poder detectar en MAIN la interrupci
140.         bsf             INTCON,7
141.         clrf    CONTADOR_1
142.         incf    CONTADOR_2,f    ; si, empieza en 110º
143.         movlw   .8
144.         subwf   CONTADOR_2,0
145.         btfsc   STATUS,Z        ; van 8?
146.         goto    MAIN_A120       ; SI, va a siguiente posicion
147.         call    MOV_A110        ; no, sigue en 110º
148.         goto    MAIN_A110
149.  
150. MAIN_A120
151.  
152.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
153.         goto    MAIN_A120       ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
154.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
155.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
156.        
157.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
158.         bsf             INTCON,7                                ; interrupcion para poder detectar en MAIN la interrupcion
159.  
160.         clrf    CONTADOR_2             
161.         incf    CONTADOR_3,f    ; si, empieza en 160º
162.         movlw   .8
163.         subwf   CONTADOR_3,0
164.         btfsc   STATUS,Z        ; van 8?
165.         goto    MAIN_A130       ; SI, va a siguiente posicion
166.         call    MOV_A120        ; no, sigue en 120º
167.         goto    MAIN_A120      
168.        
169.        
170. MAIN_A130
171.  
172.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
173.         goto    MAIN_A130       ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
174.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
175.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
176.        
177.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
178.                                 ; interrupcion para poder detectar en MAIN la interrupcion
179.         bsf             INTCON,7
180.         clrf    CONTADOR_3
181.         incf    CONTADOR_4,f    ; si, empieza en 130º
182.         movlw   .8              ; aqui repite doble de veces por ser final de recorrido
183.         subwf   CONTADOR_4,0    ; ya se vera si asi va bien
184.         btfsc   STATUS,Z        ; van 8?
185.         goto    MAIN_A140
186.         call    MOV_A130        ; no, sigue en 130º
187.         goto    MAIN_A130      
188.  
189.                
190.        
191. MAIN_A140      
192.  
193.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
194.         goto    MAIN_A140       ; sino acabo espera, aqui espera mucho tiempo entre 17.7 y 19.7ms
195.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
196.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
197.        
198.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
199.                                 ; interrupcion para poder detectar en MAIN la interrupcion
200.         bsf             INTCON,7
201.         clrf    CONTADOR_4     
202.         incf    CONTADOR_5,f    ; si, empieza cuenta en en 140º
203.         movlw   .8
204.         subwf   CONTADOR_5,0
205.         btfsc   STATUS,Z        ; van 8?
206.         goto    MAIN_RA130
207.         call    MOV_A140        ; no, sigue en 140º
208.         goto    MAIN_A140
209.        
210. ;**********retrceder servos PORTA de 140º a 90º**************************************************
211.  
212. MAIN_RA130     
213.  
214.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
215.         goto    MAIN_RA130              ; sino acabo espera, aqui espera mucho tiempo entre 17.7 y 19.7ms
216.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
217.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
218.        
219.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
220.                                 ; interrupcion para poder detectar en MAIN la interrupcion
221.         bsf             INTCON,7
222.         clrf    CONTADOR_5     
223.         incf    CONTADOR_6,f    ; si, empieza en 130º
224.         movlw   .8
225.         subwf   CONTADOR_6,0
226.         btfsc   STATUS,Z        ; van 8?
227.         goto    MAIN_RA120
228.         call    MOV_A130        ; no, sigue en 130º
229.         goto    MAIN_RA130
230.  
231.        
232. MAIN_RA120     
233.  
234.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
235.         goto    MAIN_RA120      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
236.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
237.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
238.        
239.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
240.                                 ; interrupcion para poder detectar en MAIN la interrupcion
241.         bsf             INTCON,7
242.  
243.         clrf    CONTADOR_6
244.         incf    CONTADOR_7,f    ; pasaron 50 veces empieza para 115º
245.         movlw   .8              ; 8 es el numero de repeticiones para 115º
246.         subwf   CONTADOR_7,0
247.         btfsc   STATUS,Z        ; van 8 veces?
248.         goto    MAIN_RA110      ; SI, va a siguiente
249.         call    MOV_A120        ; no sigue en 120º
250.         goto    MAIN_RA120
251.  
252.  
253. MAIN_RA110
254.  
255.        
256.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
257.         goto    MAIN_RA110              ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
258.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
259.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
260.        
261.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
262.                                 ; interrupcion para poder detectar en MAIN la interrupcion
263.         bsf             INTCON,7
264.         clrf    CONTADOR_7
265.         incf    CONTADOR_8,f    ; si, empieza en 110º
266.         movlw   .8
267.         subwf   CONTADOR_8,0
268.         btfsc   STATUS,Z        ; van 8?
269.         goto    MAIN_RA100      ; SI, va a siguiente posicion
270.         call    MOV_A110        ; no, sigue en 110º
271.         goto    MAIN_RA110
272.  
273. MAIN_RA100     
274.  
275.  
276.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
277.         goto    MAIN_RA100              ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
278.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
279.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
280.        
281.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
282.                                 ; interrupcion para poder detectar en MAIN la interrupcion
283.         bsf             INTCON,7
284.         clrf    CONTADOR_8      ; se pone a 0 para proxima vez
285.         incf    CONTADOR_9      ; pasaron 50 veces empieza para 100º
286.         movlw   .8              ; 8 es el numero de repeticiones para 100º
287.         subwf   CONTADOR_9,0
288.         btfsc   STATUS,Z        ; van 8 veces?
289.         goto    MAIN_CENT               ; si, va a siguiente posicion
290.         call    MOV_A100        ; no sigue en 10º
291.         goto    MAIN_RA100
292.  
293.  
294. MAIN_CENT                       ; aqui centra servos despues de movimiento completo PORTA      
295.                                 ; para empezar avance de PORTB
296.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
297.         goto    MAIN_CENT               ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
298.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
299.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
300.        
301.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
302.                                 ; interrupcion para poder detectar en MAIN la interrupcion
303.         bsf             INTCON,7
304.         clrf    CONTADOR_9      ; la primera vez no es necesario pero despues si       
305.         incf    CONTADOR_10,f   ; cuenta nº veces se desborda TIMER0
306.         movlw   .8              ;
307.         subwf   CONTADOR_10,0
308.         btfsc   STATUS,Z        ; mira si son 8 desbordes
309.         goto    MAIN_B100       ; Z=1 va a siguiente posicion
310.         call    CENT_SERVOS     ; todavia no, sigue en 90º
311.         goto    MAIN_CENT               ; y vuelve para esperar desborde de TIMER0
312.  
313.  
314.  
315. ;********************movimientos avance  SERVOS PORTB*****************************************
316.  
317.  
318.  
319. MAIN_B100      
320.  
321.  
322.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
323.         goto    MAIN_B100               ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
324.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
325.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
326.        
327.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
328.                                 ; interrupcion para poder detectar en MAIN la interrupcion
329.         bsf             INTCON,7
330.         clrf    CONTADOR_10     ; se pone a 0 para proxima vez
331.         incf    CONTADOR_11,f   ; pasaron 50 veces empieza para 100º
332.         movlw   .8              ; 25 es el numero de repeticiones para 100º
333.         subwf   CONTADOR_11,0
334.         btfsc   STATUS,Z        ; van 8 veces?
335.         goto    MAIN_B110       ; si, va a siguiente posicion
336.         call    MOV_B100        ; no sigue en 110º
337.         goto    MAIN_B100
338.        
339.        
340. MAIN_B110
341.  
342.        
343.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
344.         goto    MAIN_B110               ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
345.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
346.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
347.        
348.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
349.                                 ; interrupcion para poder detectar en MAIN la interrupcion
350.         bsf             INTCON,7
351.         clrf    CONTADOR_11
352.         incf    CONTADOR_12,f   ; si, empieza en 110º
353.         movlw   .8
354.         subwf   CONTADOR_12,0
355.         btfsc   STATUS,Z        ; van 8?
356.         goto    MAIN_B120       ; SI, va a siguiente posicion
357.         call    MOV_B110        ; no, sigue en 110º
358.         goto    MAIN_B110
359.  
360. MAIN_B120
361.  
362.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
363.         goto    MAIN_B120               ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
364.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
365.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
366.        
367.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
368.                                 ; interrupcion para poder detectar en MAIN la interrupcion
369.         bsf             INTCON,7
370.         clrf    CONTADOR_12    
371.         incf    CONTADOR_13,f   ; si, empieza en 120º
372.         movlw   .8
373.         subwf   CONTADOR_13,0
374.         btfsc   STATUS,Z        ; van 8?
375.         goto    MAIN_B130       ; SI, va a siguiente posicion
376.         call    MOV_B120        ; no, sigue en 120º
377.         goto    MAIN_B120      
378.        
379.        
380. MAIN_B130
381.  
382.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
383.         goto    MAIN_B130               ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
384.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
385.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
386.        
387.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
388.                                 ; interrupcion para poder detectar en MAIN la interrupcion
389.         bsf             INTCON,7
390.         clrf    CONTADOR_13
391.         incf    CONTADOR_14,f   ; si, empieza en 130º
392.         movlw   .8              ; aqui repite doble de veces por ser final de recorrido
393.         subwf   CONTADOR_14,0   ; ya se vera si asi va bien
394.         btfsc   STATUS,Z        ; van 8?
395.         goto    MAIN_B140
396.         call    MOV_B130        ; no, sigue en 130º
397.         goto    MAIN_B130      
398.  
399.                
400.        
401. MAIN_B140      
402.  
403.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
404.         goto    MAIN_B140               ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
405.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
406.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
407.        
408.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
409.                                 ; interrupcion para poder detectar en MAIN la interrupcion
410.         bsf             INTCON,7
411.         clrf    CONTADOR_14
412.         incf    CONTADOR_15,f   ; si, empieza en 140º
413.         movlw   .8
414.         subwf   CONTADOR_15,0
415.         btfsc   STATUS,Z        ; van 8?
416.         goto    MAIN_RB130
417.         call    MOV_B140        ; no, sigue en 140º
418.         goto    MAIN_B140
419.        
420. ;**********retroceder servos PORTB de 140º a 90º**************************************************
421.  
422. MAIN_RB130     
423.  
424.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
425.         goto    MAIN_RB130              ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
426.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
427.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
428.        
429.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
430.                                 ; interrupcion para poder detectar en MAIN la interrupcion
431.         bsf             INTCON,7
432.         clrf    CONTADOR_15
433.         incf    CONTADOR_16,f   ; si, empieza en 135º
434.         movlw   .8
435.         subwf   CONTADOR_16,0
436.         btfsc   STATUS,Z        ; van 8?
437.         goto    MAIN_RB120
438.         call    MOV_B130        ; no, sigue en 130º
439.         goto    MAIN_RB130
440.  
441.        
442. MAIN_RB120     
443.  
444.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
445.         goto    MAIN_RB120              ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
446.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
447.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
448.        
449.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
450.                                 ; interrupcion para poder detectar en MAIN la interrupcion
451.         bsf             INTCON,7
452.         clrf    CONTADOR_16
453.         incf    CONTADOR_17,f   ; pasaron 50 veces empieza para 115º
454.         movlw   .8              ; 25 es el numero de repeticiones para 115º
455.         subwf   CONTADOR_17,0
456.         btfsc   STATUS,Z        ; van 8 veces?
457.         goto    MAIN_RB110      ; SI, va a siguiente
458.         call    MOV_B120        ; no sigue en 120º
459.         goto    MAIN_RB120
460.  
461.  
462. MAIN_RB110
463.  
464.        
465.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
466.         goto    MAIN_RB110      ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
467.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
468.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
469.        
470.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
471.                                 ; interrupcion para poder detectar en MAIN la interrupcion
472.         bsf             INTCON,7
473.         clrf    CONTADOR_17
474.         incf    CONTADOR_18,f   ; si, empieza en 110º
475.         movlw   .8
476.         subwf   CONTADOR_18,0
477.         btfsc   STATUS,Z        ; van 8?
478.         goto    MAIN_RB100      ; SI, va a siguiente posicion
479.         call    MOV_B110        ; no, sigue en 110º
480.         goto    MAIN_RB110
481.  
482. MAIN_RB100     
483.  
484.  
485.         btfss   INTCON,2        ; chequea si acabo la interrupcion, si pasaron 20ms
486.         goto    MAIN_RB100              ; sino acabo espera, aqui espera mucho tiempo entre 17.5 y 19.5ms
487.                                 ; de cada 20ms, se pueden hacer otras cosas, entre un 85% y un 95%
488.                                 ; del tiempo esta sin hacer nada para que se puedan hacer otras cosas
489.        
490.         bcf     INTCON,2        ; aqui borra bandera interrupcion, se deja sin borrar en la
491.                                 ; interrupcion para poder detectar en MAIN la interrupcion
492.         bsf             INTCON,7
493.         clrf    CONTADOR_18     ; se pone a 0 para proxima vez
494.         incf    CONTADOR_19,f   ; pasaron 50 veces empieza para 100º
495.         movlw   .8              ; 25 es el numero de repeticiones para 100º
496.         subwf   CONTADOR_19,0
497.         btfsc   STATUS,Z        ; van 8 veces?
498.         goto    MAIN            ; si, va a posicion inicial, centra y empieza siguiente paso
499.         call    MOV_B100        ; no sigue en 10º
500.         goto    MAIN_RB100
501.  
502. ;************DUTTY DE movimiento de centrado a 90º PORTA, PORTB******************************************
503.  
504.  
505. CENT_SERVOS
506.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB los servos estan conectados
507.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
508.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
509.        
510.                 movlw   .13             ; cargamos 12 para que la demora sea de 1336us>90º
511.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
512.                
513.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
514.                 andwf   PORTA,f         ; nos pone a 0 el nibble bajo donde tenemos los servos
515.                 andwf   PORTB,f         ; y nos deja el valor del nibble alto como estaba
516.                 return                  ; retorna a esperar el final de los 20ms
517.  
518. ;**************DUTTYS para movimientos servos PORTA de 10 en 10 grados***********************************
519.  
520. MOV_A100       
521.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
522.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
523.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
524.  
525.        
526.                 movlw   .13             ; para 90º, 9X0.111ms de los 90º +3x0.111ms de 0º segun MPLAB 1.336ms
527.                                         ; "creo" que es aceptable el desfase de 36us, sino se corrige
528.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
529.                
530.                        
531.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
532.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
533.                 andwf   PORTB,f         ; y nos deja el valor del nibble alto como estaba en PORTB
534.                 movlw   .1
535.                 call    DEMORA          ; hacemos un retardo mas para PORTA, asi se movera a 100º
536.                 movlw   0XF0            ; mascara para PORTA
537.                 andwf   PORTA,f         ; nibble alto como estaba y bajo a 0   
538.                 return                  ;
539.  
540.  
541. MOV_A110       
542.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
543.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
544.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
545.  
546.        
547.                 movlw   .13             ; para 90º
548.                 call    DEMORA          ; durante 0.110 ms para movimientos de 10º aprox
549.                
550.                        
551.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
552.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
553.                 andwf   PORTB,f         ; y nos deja el valor del nibble alto como estaba en PORTB
554.                 movlw   .2              ; cargamos 2 en w para tiempo adicional de PORTA de 0.222ms>20º
555.                 call    DEMORA          ; hacemos 2 retardoS mas para PORTA, asi se movera a 100º
556.                 movlw   0XF0            ; mascara para PORTA
557.                 andwf   PORTA,f         ; nibble alto como estaba y bajo a 0   
558.                 return                  ;
559.  
560.  
561.  
562. MOV_A120       
563.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
564.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
565.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
566.  
567.        
568.                 movlw   .13             ; para 90º
569.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
570.                
571.                        
572.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
573.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
574.                 andwf   PORTB,f         ; y nos deja el valor del nibble alto como estaba en PORTB
575.                 movlw   .3              ; cargamos 3 en w para tiempo adicional de PORTA de 0.333ms>30º
576.                 call    DEMORA          ; hacemos un retardo mas para PORTA, asi se movera a 120º
577.                 movlw   0XF0            ; mascara para PORTA
578.                 andwf   PORTA,f         ; nibble alto como estaba y bajo a 0   
579.                 return                  ;
580.  
581. MOV_A130       
582.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
583.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
584.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
585.  
586.        
587.                 movlw   .13             ; para 90º
588.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
589.                
590.                        
591.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
592.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
593.                 andwf   PORTB,f         ; y nos deja el valor del nibble alto como estaba en PORTB
594.                 movlw   .4              ; cargamos 4 en w para tiempo adicional de PORTA de 0.444ms>40º
595.                 call    DEMORA          ; hacemos un retardo mas para PORTA, asi se movera a 130º
596.                 movlw   0XF0            ; mascara para PORTA
597.                 andwf   PORTA,f         ; nibble alto como estaba y bajo a 0   
598.                 return                  ;
599. MOV_A140       
600.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
601.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
602.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
603.  
604.        
605.                 movlw   .13             ; para 90º
606.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
607.                
608.                        
609.                 movlw   0xF0            ; mascara para que no nos afecte al nibble alto
610.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
611.                 andwf   PORTB,f         ; y nos deja el valor del nibble alto como estaba en PORTB
612.                 movlw   .5              ; cargamos 5 en w para tiempo adicional de PORTA de 0.555ms>50º
613.                 call    DEMORA          ; hacemos un retardo mas para PORTA, asi se movera a 140º
614.                 movlw   0XF0            ; mascara para PORTA
615.                 andwf   PORTA,f         ; nibble alto como estaba y bajo a 0   
616.                 return                  ;
617.  
618.  
619. ;*****************DUTTYS para movimientos servos PORTB de 10 en 10 grados**********************************
620.  
621. MOV_B100       
622.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
623.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
624.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
625.  
626.        
627.                 movlw   .13             ; para 90º
628.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
629.                
630.                        
631.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
632.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
633.                 andwf   PORTA,f         ; y nos deja el valor del nibble alto como estaba en PORTA
634.                 movlw   .1              ; aunque solo es una repeticion hay que cargar W porque tiene otro valor
635.                 call    DEMORA          ; hacemos un retardo mas para PORTB, asi se movera a 100º
636.                 movlw   0XF0            ; mascara para PORTB
637.                 andwf   PORTB,f         ; nibble alto como estaba y bajo a 0   
638.                 return                  ;
639.  
640. MOV_B110       
641.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
642.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
643.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
644.  
645.        
646.                 movlw   .13             ; para 90º
647.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
648.                
649.                        
650.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
651.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
652.                 andwf   PORTA,f         ; y nos deja el valor del nibble alto como estaba en PORTA
653.                 movlw   .2              ; cargamos 2 en w para tiempo adicional de PORTA de 0.222ms>20º
654.                 call    DEMORA          ; hacemos 2 retardoS mas para PORTB, asi se movera a 110º
655.                 movlw   0XF0            ; mascara para PORTA
656.                 andwf   PORTB,f         ; nibble alto como estaba y bajo a 0   
657.                 return                  ;
658.  
659. MOV_B120       
660.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
661.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
662.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
663.  
664.        
665.                 movlw   .13             ; para 90º
666.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
667.                
668.                        
669.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
670.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
671.                 andwf   PORTA,f         ; y nos deja el valor del nibble alto como estaba en PORTA
672.                 movlw   .3              ; cargamos 3 en w para tiempo adicional de PORTA de 0.330ms>30º
673.                 call    DEMORA          ; hacemos un retardo mas para PORTB, asi se movera a 120º
674.                 movlw   0XF0            ; mascara para PORTA
675.                 andwf   PORTB,f         ; nibble alto como estaba y bajo a 0   
676.                 return                  ;
677.  
678. MOV_B130       
679.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
680.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
681.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
682.  
683.        
684.                 movlw   .13             ; para 90º
685.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
686.                
687.                        
688.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
689.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
690.                 andwf   PORTA,f         ; y nos deja el valor del nibble alto como estaba en PORTA
691.                 movlw   .4              ; cargamos 4 en w para tiempo adicional de PORTA de 0.444ms>40º
692.                 call    DEMORA          ; hacemos un retardo mas para PORTA, asi se movera a 130º
693.                 movlw   0XF0            ; mascara para PORTB
694.                 andwf   PORTB,f         ; nibble alto como estaba y bajo a 0   
695.                 return                  ;
696.  
697. MOV_B140       
698.                 movlw   0x0F            ; ponemos dutty a 1 del PORTA y PORTB estan conectados
699.                 iorwf   PORTA,f         ; entre RA0>RA3 y RB0>RB3, el nibble bajo lo ponemos a 1111
700.                 iorwf   PORTB,f         ; y al alto le aplicamos mascara para dejarlo como esta
701.  
702.        
703.                 movlw   .13             ; para 90º
704.                 call    DEMORA          ; durante 0.111 ms para movimientos de 10º aprox
705.                
706.                        
707.                 movlw   0XF0            ; mascara para que no nos afecte al nibble alto
708.                                         ; nos pone a 0 el nibble bajo donde tenemos los servos
709.                 andwf   PORTA,f         ; y nos deja el valor del nibble alto como estaba en PORTA
710.                 movlw   .5              ; cargamos 5 en w para tiempo adicional de PORTA de 0.555ms>50º
711.                 call    DEMORA          ; hacemos un retardo mas para PORTA, asi se movera a 140º
712.                 movlw   0XF0            ; mascara para PORTB
713.                 andwf   PORTB,f         ; nibble alto como estaba y bajo a 0   
714.                 return                  ;
715.  
716.  
717.  
718. ;************ Rutina de servicio de Interrupcion ************************
719.  
720. ;********Aqui haremos copia de respaldo porque este programa ira integrado*********
721. ;*****************en otro mayor y puede afectar a dicho programa********************
722.  
723. ;copia de registro W y STATUS.-
724. Inicio_ISR
725.         movwf   W_Temp          ; Copiamos W a un registro Temporal.
726.         swapf   STATUS, W       ; Invertimos los nibles del registro STATUS.
727.         movwf   STATUS_Temp     ; Guardamos STATUS en un registro temporal.
728. ;**** Interrupcion por TMR0 ****
729. ISR
730.         btfss   INTCON,T0IF     ; Consultamos si es por TMR0.-
731.         goto    Fin_ISR         ; No, entonces restauramos valores.-
732.                                
733.  
734. Actualizo_TMR0                  ; Actualizo TMR0 para obtener una temporizacion de 20 ms.
735.         movlw   .100            ; d'100'
736.         movwf   TMR0
737.         goto    Fin_ISR         ; Restauro valores.-
738.  
739. ; Restauramos los valores de W y STATUS.-
740. Fin_ISR
741.         swapf   STATUS_Temp,W   ; Invertimos lo nibles de STATUS_Temp.-
742.         movwf   STATUS
743.         swapf   W_Temp, f       ; Invertimos los nibles y lo guardamos en el mismo registro.-
744.         swapf   W_Temp,W        ; Invertimos los nibles nuevamente y lo guardamos en W.-
745.        
746.         return                  ; Salimos de interrupción.-
747. ;..........................................
748.  
749.  
750.  
751. DEMORA                          ; haremos demoras de 0.111ms equivalentes 10º del servo aprox
752.         movwf   PDel1           ; la demora multiplicara 0.111 por el numero que traiga w
753.  
754. TOP1    movlw     .23       ; 1 set numero de repeticion
755.         movwf     PDel0     ; 1 |
756. PLoop0  clrwdt              ; 1 clear watchdog
757.         decfsz    PDel0, 1  ; 1 + (1) es el tiempo 0  ?
758.         goto      PLoop0    ; 2 no, loop
759. PDelL1  goto PDelL2         ; 2 ciclos delay
760. PDelL2  clrwdt              ; 1 ciclo delay
761.        
762.             decfsz      PDel1,1
763.             goto        TOP1
764.         return                  ; 2+2 Fin.
765.  
766.         END