Si además quieres enviarnos un Artículo para el Blog y redes sociales, pulsa el siguiente botón:
Buenas noches, después de un buen tiempo desconectado vuelvo a la carga con otro robocillo. El primero tuve que dejarlo porque al hacer las piezas en plastico y Loctite era un dolor pegarlo otra vez antes de que se moviera. Ahora he hecho uno de metal y más a conciencia. Se parece mucho al BRAT pero prefiero empezar con algo que funciona que aventurarme en cosas nuevas.
Buenos os dejo unas fotillos para que las critiqueis y de paso unas dudas para seguir adelante con mi proyecto.
También os paso el código que como vereis es bastante cutre salchichero pero ahí entrais vosotros. En breve (cuando consiga caminar sin que se caiga) colgaré un video que ya tengo ganas de enseñar mi pqueña obra maestra.
LIST P=16F84A
INCLUDE <P16F84A.INC>
__CONFIG _CP_OFF & _WDT_OFF & _PWRTE_ON & _XT_OSC
cblock 0x0c
contA
ENDC
ORG 0 ; El programa comienza en la dirección 0.
Inicio
bsf STATUS,RP0 ; Pone a 1 el bit 5 del STATUS. Acceso al Banco 1.
clrf TRISB ; Las líneas del Puerto B configuradas como salidas.
movlw b'00011111' ; Las 5 líneas del Puerto A configuradas como entradas.
movwf TRISA
bcf STATUS,RP0
movlw d'30'
movwf contA
Saludo
bsf PORTB,0 ; Activa el PORTB1
call Retardo_50micros ; la suma de los call genera el tiempo
call Retardo_400micros ; en alto para un ángulo de 60º
bcf PORTB,0 ; Desactiva el PORTB1
call Retardo_10ms ; la suma de los call genera el tiempo
call Retardo_5ms ; la suma de los call genera el tiempo
call Retardo_2ms ; la suma de los call genera el tiempo
call Retardo_2ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Saludo ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del saludo
movwf contA
Saludo_2
bsf PORTB,0 ; Activa el PORTB1
call Retardo_1ms ; la suma de los call genera el tiempo
call Retardo_500micros ; la suma de los call genera el tiempo
call Retardo_400micros ; en alto para un ángulo de 60º
bcf PORTB,0 ; Desactiva el PORTB1
call Retardo_10ms ; la suma de los call genera el tiempo
call Retardo_5ms ; la suma de los call genera el tiempo
call Retardo_2ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Saludo_2 ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del saludo_2
movwf contA
Paso_1
bsf PORTB,1
call Retardo_500micros ; la suma de los call genera el tiempo de 30º
call Retardo_100micros
bcf PORTB,1
bsf PORTB,2
call Retardo_500micros ; la suma de los call genera el tiempo de 30º
call Retardo_100micros
bcf PORTB,2
bsf PORTB,3
call Retardo_1ms ; la suma de los call genera el tiempo de 105º
call Retardo_500micros ;
call Retardo_100micros ;
call Retardo_50micros ;
bcf PORTB,3
bsf PORTB,4
call Retardo_1ms ; la suma de los call genera el tiempo de 105º
call Retardo_200micros ;
call Retardo_100micros ;
call Retardo_50micros ;
bcf PORTB,4
bsf PORTB,5
call Retardo_1ms ; la suma de los call genera el tiempo de 75º
call Retardo_50micros ;
bcf PORTB,5
bsf PORTB,6
call Retardo_1ms ; la suma de los call genera el tiempo de 75º
call Retardo_50micros ;
bcf PORTB,6
bsf PORTB,7
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,7
call Retardo_10ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Paso_1 ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del Paso_1
movwf contA
Paso_2
bsf PORTB,1
call Retardo_500micros ; la suma de los call genera el tiempo de 30º
call Retardo_100micros
bcf PORTB,1
bsf PORTB,2
call Retardo_500micros ; la suma de los call genera el tiempo de 45º
call Retardo_200micros
call Retardo_50micros
bcf PORTB,2
bsf PORTB,3
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,3
bsf PORTB,4
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,4
bsf PORTB,5
call Retardo_500micros ; la suma de los call genera el tiempo de 45º
call Retardo_200micros ;
call Retardo_50micros ;
bcf PORTB,5
bsf PORTB,6
call Retardo_500micros ; la suma de los call genera el tiempo de 30º
call Retardo_100micros ;
bcf PORTB,6
bsf PORTB,7
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,7
call Retardo_10ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Paso_2 ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del Paso_1
movwf contA
Paso_3
bsf PORTB,1
call Retardo_500micros ; la suma de los call genera el tiempo de 30º
call Retardo_100micros
bcf PORTB,1
bsf PORTB,2
call Retardo_500micros ; la suma de los call genera el tiempo de 45º
call Retardo_200micros
call Retardo_50micros
bcf PORTB,2
bsf PORTB,3
call Retardo_1ms ; la suma de los call genera el tiempo de 105º
call Retardo_50micros ;
bcf PORTB,3
bsf PORTB,4
call Retardo_1ms ; la suma de los call genera el tiempo de 105º
call Retardo_50micros ;
bcf PORTB,4
bsf PORTB,5
call Retardo_500micros ; la suma de los call genera el tiempo de 45º
call Retardo_200micros ;
call Retardo_50micros ;
bcf PORTB,5
bsf PORTB,6
call Retardo_500micros ; la suma de los call genera el tiempo de 30º
call Retardo_100micros ;
bcf PORTB,6
bsf PORTB,7
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,7
call Retardo_10ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Paso_3 ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del Paso_1
movwf contA
Paso_4
bsf PORTB,1
call Retardo_1ms ; la suma de los call genera el tiempo de 150º
call Retardo_200micros
bcf PORTB,1
bsf PORTB,2
call Retardo_1ms ; la suma de los call genera el tiempo de 135º
call Retardo_200micros
bcf PORTB,2
bsf PORTB,3
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_50micros ;
bcf PORTB,3
bsf PORTB,4
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_50micros ;
bcf PORTB,4
bsf PORTB,5
call Retardo_1ms ; la suma de los call genera el tiempo de 135º
call Retardo_500micros
call Retardo_100micros
call Retardo_50micros
bcf PORTB,5
bsf PORTB,6
call Retardo_1ms ; la suma de los call genera el tiempo de 150º
call Retardo_500micros
call Retardo_200micros
call Retardo_100micros
bcf PORTB,6
bsf PORTB,7
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,7
call Retardo_10ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Paso_4 ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del Paso_1
movwf contA
Paso_5
bsf PORTB,1
call Retardo_1ms ; la suma de los call genera el tiempo de 135º
call Retardo_500micros
call Retardo_100micros
call Retardo_200micros
bcf PORTB,1
bsf PORTB,2
call Retardo_1ms ; la suma de los call genera el tiempo de 150º
call Retardo_500micros
call Retardo_50micros
call Retardo_100micros
bcf PORTB,2
bsf PORTB,3
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,3
bsf PORTB,4
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,4
bsf PORTB,5
call Retardo_1ms ; la suma de los call genera el tiempo de 135º
call Retardo_500micros
call Retardo_100micros
call Retardo_50micros
bcf PORTB,5
bsf PORTB,6
call Retardo_1ms ; la suma de los call genera el tiempo de 150º
call Retardo_500micros
call Retardo_200micros
call Retardo_100micros
bcf PORTB,6
bsf PORTB,7
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,7
call Retardo_10ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Paso_5 ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del Paso_1
movwf contA
Paso_6
bsf PORTB,1
call Retardo_1ms ; la suma de los call genera el tiempo de 135º
call Retardo_500micros
call Retardo_200micros
call Retardo_100micros
bcf PORTB,1
bsf PORTB,2
call Retardo_1ms ; la suma de los call genera el tiempo de 150º
call Retardo_500micros
call Retardo_100micros
call Retardo_50micros
bcf PORTB,2
bsf PORTB,3
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
call Retardo_100micros
call Retardo_50micros
bcf PORTB,3
bsf PORTB,4
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
call Retardo_100micros
call Retardo_50micros
bcf PORTB,4
bsf PORTB,5
call Retardo_1ms ; la suma de los call genera el tiempo de 150º
call Retardo_500micros
call Retardo_100micros
call Retardo_50micros
bcf PORTB,5
bsf PORTB,6
call Retardo_1ms ; la suma de los call genera el tiempo de 135º
call Retardo_500micros
call Retardo_200micros
call Retardo_100micros
bcf PORTB,6
bsf PORTB,7
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,7
call Retardo_10ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Paso_6 ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del Paso_1
movwf contA
Paso_7
bsf PORTB,1
call Retardo_500micros ; la suma de los call genera el tiempo de 30º
call Retardo_100micros
bcf PORTB,1
bsf PORTB,2
call Retardo_500micros ; la suma de los call genera el tiempo de 45º
call Retardo_200micros
call Retardo_50micros
bcf PORTB,2
bsf PORTB,3
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
call Retardo_100micros ;
call Retardo_50micros ;
bcf PORTB,3
bsf PORTB,4
call Retardo_1ms ; la suma de los call genera el tiempo de 105º
call Retardo_200micros ;
call Retardo_100micros ;
call Retardo_50micros ;
bcf PORTB,4
bsf PORTB,5
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,5
bsf PORTB,6
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,6
bsf PORTB,7
call Retardo_1ms ; la suma de los call genera el tiempo de 90º
call Retardo_200micros ;
bcf PORTB,7
call Retardo_10ms ; la suma de los call genera el tiempo
decfsz contA ; Cuenta a contA y sigue en la línea de abajo
goto Paso_7 ; Cuando contA sea = 0 omite la línea siguiente.
clrf contA
movlw d'80' ; Tiempo de duración del Paso_1
movwf contA
goto Paso_2
INCLUDE<LIBSERVOS.INC>
End
No os fijeis en los comentarios de los ángulos porque quizás no estén todos arreglados, porque lo voy cambiando sobre la marcha.
Y ahora es cuando necesito toda vuestra ayuda para seguir adelante. Tengo tres frentes a atacar y no se como empezar.
Frente 1 - Actualmente puedo controlar los ángulos, pero no la velocidad de los motores, como lo consigo??? alguien tiene algún ejemplo sencillo en ASM??
Frente 2 - Quiero montarle un sensor de infrarrojos el GP2DXX, que os parece?? es sencillo de usar?? me conformo que pueda detectar obstaculos a cierta distancia y si pudiera medir la distancia para un futuro mejor que mejor.
Frente 3 - También sé que con los conectores del programador de PICS lo puedo conectar al PIC16F84A para no tener que estar montando y desmontando el micro. Es cierto?? es solamente conectar los cables y listo?? sabeis el orden?? Os dejo fotillo de lo que os digo por sino me explico bien
Pues nada, espero vuestras respuestas y a ver si llego más lejos con RoYbot que con el anterior Panchito jejejeje.
Un saludo a todos y muchas gracias
La oscuridad se cierna sobre mi, tengo un problemón con la programación. He conseguido que dos servos se muevan suavemente, pero por alguna razón los dos siempre giran con el mismo ángulo. Si cambio el ángulo en cont_167 o cont_226, aunque solo lo haga en un servo, el movimiento se repite. Por lo que no puedo hacer que dos servos giren en diferentes ángulos.
Ayudaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa
Como siempre, os paso el código.
list p=16F84A ; definir procesador
include <p16F84A.inc> ; definiciones de variables específicas del procesador
__CONFIG _CP_OFF & _WDT_OFF & _PWRTE_ON & _XT_OSC
#define SERVO0 PORTB,0 ; puerto de salida de servo 0
#define SERVO1 PORTB,1 ; puerto de salida de servo 1
CBLOCK 0x20
Posic ; posición servo
acum_A
cont_A
cont_B
cont_C
cont_167
cont_226
cont_110
contA
MemServ_0
MemServ_1
FinMemServ_0
FinMemServ_1
ENDC
;********************************************************************************
org 0x000
goto configuracion
;*****************************************************************************
; Principal
;********************************************************************************
configuracion
clrf PORTA ; inicia ports
clrf PORTB ; inicia ports
bsf STATUS,RP0 ; Apunta a banco 1
movlw b'11111111' ; PORTA
movwf TRISA ; entradas
MOVLW b'00000000' ; PORTB
movwf TRISB ; salidas
movlw b'00000010' ; Configuración para TMR0
movwf OPTION_REG ; preescaler 2, 1:8 con CLK interno (que es
; de 1 MHz si el oscilador es de 4 MHz).
; El contador cuenta cada 8 useg
bcf STATUS,RP0 ; Apunta a banco 0
clrf TMR0 ; inicia registro de timer en 0
movlw d'133' ; inicia valor de posición de servo (centro)
movwf MemServ_0
movlw d'133' ; inicia valor de posición de servo (centro)
movwf MemServ_1
movlw d'226' ; Tiempo de duración del Movimiento_2
movwf cont_226
movlw d'167' ; Tiempo de duración del Movimiento_2
movwf cont_167
movlw d'110' ; Tiempo de duración del Movimiento_2
movwf cont_110
movlw d'2' ; Tiempo de duración del Movimiento_2
movwf cont_A
movlw d'1' ; Tiempo de duración del Movimiento_2
movwf cont_C
movlw d'2' ; Tiempo de duración del Movimiento_2
movwf FinMemServ_0
movlw d'2' ; Tiempo de duración del Movimiento_2
movwf FinMemServ_1
principal
; Los servos actúan con un pulso de control que va entre 0,5 ms y 2,5 ms.
; En el bloque que sigue se inicia en 1 el pulso de control y se espera un
; retardo fijo de aproximadamente 0,5 ms, que es el valor mínimo del pulso.
; Con este valor de longitud de pulso y una longitud cero en la parte variable,
; el servo está parado al máximo hacia la izquierda (sentido antihorario).
bsf SERVO0 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf MemServ_0,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO0 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
bsf SERVO1 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf MemServ_1,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO1 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
; ============================================================================
; Retardo de 20 ms, que es el tiempo estándar que debe separar los pulsos
; de control para los servos comunes de RC
movlw d'9' ; cantidad de veces: 9 x 2 ms = 18 ms + 2 ms
movwf acum_A
retardo
bcf INTCON,T0IF ; borra el flag de timer
movlw d'6' ; (256-6 = 250) 250 * 8 us = 2 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
decfsz acum_A ; decrementa cantidad de veces
goto retardo
Condicion
movf cont_A,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContB y W (SOLO MIRA EN W NO EN F)
btfsc STATUS,Z ;STATUS "Z" si son iguales va a la siguiente línea, si ContB y
;no son iguales se salta la siguiente línea.
goto Movimiento_2
goto Movimiento_1
Movimiento_1
incf MemServ_0 ;se incrementa MemServ_0
movf MemServ_0
movf MemServ_0,W
subwf cont_226,W ;revisa el valor entre MemServ_0 y cont_226
btfsc STATUS,Z ;si son iguales ves a decfsz, sino salta la línea
decfsz FinMemServ_0 ;decrementa en uno FinMemServ_0
incf MemServ_1
movf MemServ_1
movf MemServ_1,W
subwf cont_226,W ;revisa el valor entre ContB y W (SOLO MIRA EN W NO EN F)
btfsc STATUS,Z
decfsz FinMemServ_1 ;decrementa en uno cuando MemServ_1 haya llegado al final del recorrido
; ------------ Condicion fin del movimiento uno --------------------
; se comprueba que ambos servos han llegado al final de recorrido para pasar al segundo movimiento.
movf FinMemServ_0,W
subwf cont_C,W
btfss STATUS,Z ;revisa ambos valores, si son iguales salta goto.
goto principal
movf FinMemServ_1,W
subwf cont_C,W
btfss STATUS,Z
goto principal
goto InMovimiento_2 ;
; volvemos a cargar las tres memorias.
InMovimiento_2
decfsz cont_A ; Cuenta a contA y sigue en la línea de abajo
movlw d'2' ; Tiempo de duración del Movimiento_2
movwf FinMemServ_0
movlw d'2' ; Tiempo de duración del Movimiento_2
movwf FinMemServ_1
Movimiento_2
decf MemServ_1
movf MemServ_1
movf MemServ_1,W
subwf cont_167,W ;revisa el valor entre ContB y W (SOLO MIRA EN W NO EN F)
btfsc STATUS,Z
decfsz FinMemServ_1
decf MemServ_0
movf MemServ_0
movf MemServ_0,W
subwf cont_167,W ;revisa el valor entre ContB y W (SOLO MIRA EN W NO EN F)
btfsc STATUS,Z
decfsz FinMemServ_0
; ------------ Condicion fin del movimiento dos --------------------
movf FinMemServ_0,W
subwf cont_C,W
btfss STATUS,Z
goto principal
movf FinMemServ_1,W
subwf cont_C,W
btfss STATUS,Z
goto principal
goto Fin
Fin
movlw d'2'
movwf cont_A
movlw d'2'
movwf FinMemServ_0
movlw d'2'
movwf FinMemServ_1
goto principal
END
Lo siento no me he mirado el código (tengo el ensamblador muy oxidado). Sólo me pasaba para darte ánimo con el proyecto.
S2
Ranganok Schahzaman
Gracias Ranganok, en fin, yo iré colgando mis procesos, ya que he abusado de mucha gente que lo ha hecho, es hora de que yo haga mi pequeña aportación a la red.
En cuanto consiga algo más, lo colgaré, a ver si alguien se anima.
Saludos a todos.
Después de un tiempo sigo con lo mío, y casi lo tengo. Ahora mi problema es que quiero ir sumando los FinAngServ (que su valor es uno) a FinMovUno. Con lo cual si cada FinAngServ es 1 al final FinMovUno debería ser 7. Yo lo he hecho así pero no funiona y voy algo perdido.
Como siempre os paso el código, un saludo a todos y muchas gracias.
Movimiento_1_1
addwf FinAngServ0
movwf FinMovUno
addwf FinAngServ1
movwf FinMovUno
addwf FinAngServ2
movwf FinMovUno
addwf FinAngServ3
movwf FinMovUno
addwf FinAngServ4
movwf FinMovUno
addwf FinAngServ5
movwf FinMovUno
addwf FinAngServ6
movwf FinMovUno
movf FinMovUno,W
subwf cont_C,W
btfss STATUS,Z
goto principal
Por fin, ya ha dado sus primeros pasos, bueno más o menos, aquí os dejo el video para criticas (por cierto, se ha subido al revés, otro día lo pongo del derecho).
B6H76jbE0Dg
Como podéis ver en el video, RoYbot hace dos cosas, primero hace unos movimientos suaves y luego en cada movimiento para unos cuantos segundos (demasiados teniendo en cuenta que no hay esperas en el programa). A ver si me podéis echar una mano para descubrir porque estos paros y hacer los movimientos mucho más fluidos.
Si os acordais hace un tiempo hice otro y andaba más fluido y la verdad es que por más que mire el código no encuentro porque tarda tanto en decirdirse en dar otro paso, quizás el micro vaya muy lento??? no tengo ni idea. El anterior robot iba algo más rápido.
nmLD8M1SleE
Como siempre dejo el código a disposición para el que quiera ayudarme o quiera usarlo.
Muchas gracias a todos de antemano.
list p=16F84A ; definir procesador
include <p16F84A.inc> ; definiciones de variables específicas del procesador
__CONFIG _CP_OFF & _WDT_OFF & _PWRTE_ON & _XT_OSC
#define SERVO0 PORTB,0 ; puerto de salida de servo 0
#define SERVO1 PORTB,1 ; puerto de salida de servo 1
#define SERVO2 PORTB,2 ; puerto de salida de servo 2
#define SERVO3 PORTB,3 ; puerto de salida de servo 3
#define SERVO4 PORTB,4 ; puerto de salida de servo 4
#define SERVO5 PORTB,5 ; puerto de salida de servo 5
#define SERVO6 PORTB,6 ; puerto de salida de servo 6
CBLOCK 0x20
Posic ; posición servo
AngServ0 ; memoria del ángulo del servo 0
AngServ1 ; memoria del ángulo del servo 1
AngServ2
AngServ3
AngServ4
AngServ5
AngServ6
FinAngServ0
FinAngServ1
FinAngServ2
FinAngServ3
FinAngServ4
FinAngServ5
FinAngServ6
FinMovimiento
TotalFinMov
grad_50
grad_65
grad_70
grad_75
grad_90
grad_95
grad_105
grad_110
grad_115
grad_120
grad_140
acum_A
cont_0
cont_1
cont_2
cont_3
cont_4
cont_5
cont_A
cont_B
cont_C
contA
ENDC
;********************************************************************************
org 0x000
goto configuracion
;*****************************************************************************
; Principal
;********************************************************************************
configuracion
clrf PORTA ; inicia ports
clrf PORTB ; inicia ports
bsf STATUS,RP0 ; Apunta a banco 1
movlw b'11111111' ; PORTA
movwf TRISA ; entradas
MOVLW b'00000000' ; PORTB
movwf TRISB ; salidas
movlw b'00000010' ; Configuración para TMR0
movwf OPTION_REG ; preescaler 2, 1:8 con CLK interno (que es
; de 1 MHz si el oscilador es de 4 MHz).
; El contador cuenta cada 8 useg
bcf STATUS,RP0 ; Apunta a banco 0
clrf TMR0 ; inicia registro de timer en 0
; ======================= CARGA DE LOS TIEMPOS PARA LOS DIFERENTES ANGULOS ==================
movlw d'156' ; 50 grados son 800 ms.
movwf grad_50
movlw d'137' ; 65 grados son 950 ms.
movwf grad_65
movlw d'131' ; 70 grados son 1000 ms.
movwf grad_70
movlw d'125' ; 75 grados son 1050 ms.
movwf grad_75
movlw d'106' ; 90 grados son 1200 ms.
movwf grad_90
movlw d'100' ; 90 grados son 1200 ms.
movwf grad_95
movlw d'87' ; 105 grados son 1350 ms.
movwf grad_105
movlw d'81' ; 110 grados son 1400 ms.
movwf grad_110
movlw d'75' ; 110 grados son 1400 ms.
movwf grad_115
movlw d'68' ; 120 grados son 1500 ms.
movwf grad_120
; =====================================================================================
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ0
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ1
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ2
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ3
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ4
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ5
movlw d'106' ; inicia valor de posición de servo (centro)
movwf AngServ6
; =============================== CARGA DE LAS MEMORIAS INICIALES ====================================
movlw d'2'
movwf cont_1
movlw d'2'
movwf cont_2
movlw d'2'
movwf cont_3
movlw d'2'
movwf cont_4
movlw d'2'
movwf cont_5
movlw d'2'
movwf cont_A
movlw d'1'
movwf cont_C
movlw d'2'
movwf FinAngServ0
movlw d'2'
movwf FinAngServ1
movlw d'2'
movwf FinAngServ2
movlw d'2'
movwf FinAngServ3
movlw d'2'
movwf FinAngServ4
movlw d'2'
movwf FinAngServ5
movlw d'2'
movwf FinAngServ6
movlw d'1'
movwf FinMovimiento
movlw d'8'
movwf TotalFinMov
principal
; Los servos actúan con un pulso de control que va entre 0,5 ms y 2,5 ms.
; En el bloque que sigue se inicia en 1 el pulso de control y se espera un
; retardo fijo de aproximadamente 0,5 ms, que es el valor mínimo del pulso.
; Con este valor de longitud de pulso y una longitud cero en la parte variable,
; el servo está parado al máximo hacia la izquierda (sentido antihorario).
bsf SERVO0 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf AngServ0,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO0 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
bsf SERVO1 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf AngServ1,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO1 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
bsf SERVO2 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf AngServ2,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO2 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
bsf SERVO3 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf AngServ3,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO3 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
bsf SERVO4 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf AngServ4,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO4 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
bsf SERVO5 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf AngServ5,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO5 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
bsf SERVO6 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
movlw d'192' ; (256-192 = 64) 64 * 8 us = 0,512 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf INTCON,T0IF ; borra el flag de timer
movf AngServ6,w ; 256-nn x 8 uS = 1 ms, 1,5 ms, 2,5 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
bcf SERVO6 ; pone la señal de servo en 0
bcf INTCON,T0IF ; borra el flag de timer
; ============================================================================
; Retardo de 20 ms, que es el tiempo estándar que debe separar los pulsos
; de control para los servos comunes de RC
movlw d'9' ; cantidad de veces: 9 x 2 ms = 18 ms + 2 ms
movwf acum_A
retardo
bcf INTCON,T0IF ; borra el flag de timer
movlw d'6' ; (256-6 = 250) 250 * 8 us = 2 ms
movwf TMR0 ; valor al registro de timer
btfss INTCON,T0IF ; espera por timer = ff -> 00
goto $-1
decfsz acum_A ; decrementa cantidad de veces
goto retardo
; En este paso iremos seleccionando la posción a seguir, cuando acaba un movimiento los cont_1, cont_2, ...
; estos son diferentes a cont_C y por lo tanto va al siguiente movimiento.
Condicion
movf cont_1,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z ;STATUS "Z" si son iguales va a la siguiente línea, si ContC y
goto Movimiento_1 ;y cont_1 no son iguales se salta la siguiente línea.
movf cont_2,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_2
movf cont_3,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_3
movf cont_4,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_4
movf cont_5,W ;pasa el valor de F a W
subwf cont_C,W ;revisa el valor entre ContC y W (SOLO MIRA EN W NO EN F)
btfss STATUS,Z
goto Movimiento_5
goto CargaMemoria
Movimiento_1
movf AngServ0,W
subwf grad_90,W ;revisa el valor entre AngServo_0 y grad_90
btfss STATUS,Z ;si no son iguales ves a incf, si lo son salta la línea
incf AngServ0 ;incrementa AngServ0 hasta que llegue a grad_90, o sea hasta que
;AngServ0 llegue a 90 grados.
movf AngServ1,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ1
movf AngServ2,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ2
movf AngServ3,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ3
movf AngServ4,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ4
movf AngServ5,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ5
movf AngServ6,W
subwf grad_90,W
btfss STATUS,Z
incf AngServ6
movf AngServ0,W
subwf grad_90,W ;revisa el valor entre AngSer0 y grad90, si ha llegado y son iguales
btfsc STATUS,Z ;decrementa FinAngServ0 para que no se siga movimendo el servo.
decf FinAngServ0
movf AngServ1,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ1
movf AngServ2,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ2
movf AngServ3,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ3
movf AngServ4,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ4
movf AngServ5,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ5
movf AngServ6,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ6
; ------------ Condicion fin del movimiento uno --------------------
; se comprueba que todos los servos han llegado al final de recorrido para pasar al segundo movimiento.
; si alguno no ha llegado volverá al princpio hasta que lleguen todos. Una vez llegados
Movimiento_1_1
movf FinAngServ0,W
addwf FinMovimiento,F
movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F
movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal
; ---------------------- COMIENZO EL MOVIMIENTO DOS --------------------------------------------
; volvemos a cargar todas las memorias, decrementamos cont_1, de esta forma cuando se vuelva a principal
; ya no pasaremos al movimiento uno y empezará el movimiento 2.
CargaMovimiento_2
movlw d'1'
movwf cont_1
movlw d'2'
movwf FinAngServ0
movlw d'2'
movwf FinAngServ1
movlw d'2'
movwf FinAngServ2
movlw d'2'
movwf FinAngServ3
movlw d'2'
movwf FinAngServ4
movlw d'2'
movwf FinAngServ5
movlw d'2'
movwf FinAngServ6
Movimiento_2
movf AngServ0,W
subwf grad_90,W
btfss STATUS,Z
decf AngServ0
movf AngServ1,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ1
movf AngServ2,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ2
movf AngServ3,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ3
movf AngServ4,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ4
movf AngServ5,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ5
movf AngServ6,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ6
movf AngServ0,W
subwf grad_90,W
btfsc STATUS,Z
decf FinAngServ0
movf AngServ1,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ1
movf AngServ2,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ2
movf AngServ3,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ3
movf AngServ4,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ4
movf AngServ5,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ5
movf AngServ6,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ6
Movimiento_2_1
movf FinAngServ0,W
addwf FinMovimiento,F
movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F
movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal
; ------------ Condicion fin del movimiento dos --------------------
; _______________________________________________________________________________________
; _______________________________________________________________________________________
CargaMovimiento_3
movlw d'1'
movwf cont_2
movlw d'2'
movwf FinAngServ0
movlw d'2'
movwf FinAngServ1
movlw d'2'
movwf FinAngServ2
movlw d'2'
movwf FinAngServ3
movlw d'2'
movwf FinAngServ4
movlw d'2'
movwf FinAngServ5
movlw d'2'
movwf FinAngServ6
Movimiento_3
movf AngServ0,W
subwf grad_90,W
btfss STATUS,Z
decf AngServ0
movf AngServ1,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ1
movf AngServ2,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ2
movf AngServ3,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ3
movf AngServ4,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ4
movf AngServ5,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ5
movf AngServ6,W
subwf grad_65,W
btfss STATUS,Z
incf AngServ6
movf AngServ0,W
subwf grad_90 ,W
btfsc STATUS,Z
decf FinAngServ0
movf AngServ1,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ1
movf AngServ2,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ2
movf AngServ3,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ3
movf AngServ4,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ4
movf AngServ5,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ5
movf AngServ6,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ6
Movimiento_3_1
movf FinAngServ0,W
addwf FinMovimiento,F
movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F
movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal
; ----------------------- COMIENZO DEL MOVIMIENTO CUATRO -------------------------------
CargaMovimiento_4
movlw d'1'
movwf cont_3
movlw d'2'
movwf FinAngServ0
movlw d'2'
movwf FinAngServ1
movlw d'2'
movwf FinAngServ2
movlw d'2'
movwf FinAngServ3
movlw d'2'
movwf FinAngServ4
movlw d'2'
movwf FinAngServ5
movlw d'2'
movwf FinAngServ6
Movimiento_4
movf AngServ0,W
subwf grad_90,W
btfss STATUS,Z
decf AngServ0
movf AngServ1,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ1
movf AngServ2,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ2
movf AngServ3,W
subwf grad_65,W
btfss STATUS,Z
decf AngServ3
movf AngServ4,W
subwf grad_65,W
btfss STATUS,Z
decf AngServ4
movf AngServ5,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ5
movf AngServ6,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ6
movf AngServ0,W
subwf grad_90 ,W
btfsc STATUS,Z
decf FinAngServ0
movf AngServ1,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ1
movf AngServ2,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ2
movf AngServ3,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ3
movf AngServ4,W
subwf grad_65,W
btfsc STATUS,Z
decf FinAngServ4
movf AngServ5,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ5
movf AngServ6,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ6
Movimiento_4_1
movf FinAngServ0,W
addwf FinMovimiento,F
movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F
movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal
; ----------------------- COMIENZO DEL MOVIMIENTO CINCO -------------------------------
CargaMovimiento_5
movlw d'1'
movwf cont_4
movlw d'2'
movwf FinAngServ0
movlw d'2'
movwf FinAngServ1
movlw d'2'
movwf FinAngServ2
movlw d'2'
movwf FinAngServ3
movlw d'2'
movwf FinAngServ4
movlw d'2'
movwf FinAngServ5
movlw d'2'
movwf FinAngServ6
Movimiento_5
movf AngServ0,W
subwf grad_90,W
btfss STATUS,Z
decf AngServ0
movf AngServ1,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ1
movf AngServ2,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ2
movf AngServ3,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ3
movf AngServ4,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ4
movf AngServ5,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ5
movf AngServ6,W
subwf grad_115,W
btfss STATUS,Z
decf AngServ6
movf AngServ0,W
subwf grad_90 ,W
btfsc STATUS,Z
decf FinAngServ0
movf AngServ1,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ1
movf AngServ2,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ2
movf AngServ3,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ3
movf AngServ4,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ4
movf AngServ5,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ5
movf AngServ6,W
subwf grad_115,W
btfsc STATUS,Z
decf FinAngServ6
Movimiento_5_1
movf FinAngServ0,W
addwf FinMovimiento,F
movf FinAngServ1,W
addwf FinMovimiento,F
movf FinAngServ2,W
addwf FinMovimiento,F
movf FinAngServ3,W
addwf FinMovimiento,F
movf FinAngServ4,W
addwf FinMovimiento,F
movf FinAngServ5,W
addwf FinMovimiento,F
movf FinAngServ6,W
addwf FinMovimiento,F
movf FinMovimiento,W
subwf TotalFinMov,W
btfss STATUS,Z
goto principal
CargaMemoria
movlw d'2'
movwf cont_2
movlw d'2'
movwf cont_3
movlw d'2'
movwf cont_4
movlw d'2'
movwf cont_5
movlw d'2'
movwf FinAngServ0
movlw d'2'
movwf FinAngServ1
movlw d'2'
movwf FinAngServ2
movlw d'2'
movwf FinAngServ3
movlw d'2'
movwf FinAngServ4
movlw d'2'
movwf FinAngServ5
movlw d'2'
movwf FinAngServ6
goto principal
END
