UNIVERSIDAD DE MÁLAGA E.T.S.I. DE TELECOMUNICACIÓN LABORATORIO DE SISTEMAS DIGITALES

DEPARTAMENTO DE TECNOLOGÍA ELECTRÓNICA CURSO 2004/2005

Gomariz Abril, Ildefonso

4

López Pestaña, José Mª Sánchez Aranda, Agustín

Diseño Final

27/05/05

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

Índice 1.

Presentación............................................................................... pg. 2

2.

Esquema hardware y conexionado............................................ pg. 3

3.

Flujograma................................................................................. pg. 4

4.

Plan de pruebas ......................................................................... pg. 6

5.

Código fuente.............................................................................pg. 7

Apéndice – Flujograma y código fuente del aprobado...................... pg. 13

Página 1

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

1. Presentación El fin de este laboratorio ha sido el desarrollo de un microbot Tritt con la misión de recolectar vasos en un recinto cerrado con paredes. Los vasos han de ser depositados junto a la pared. En la memoria incluimos tanto el desarrollo del notable como el del aprobado (en apéndice) con sus correspondientes flujogramas y códigos fuente, ya que en el notable se eliminan o modifican partes importantes del código del aprobado. Podemos observar que el código del aprobado es más complejo que el del notable, porque al no poseer un algoritmo de recorrido ordenado se pueden dar múltiples situaciones a resolver que implican muchos trozos adicionales de código. El microbot reconoce las paredes de los vasos porque éstas activan tanto los bumpers como los sensores infrarrojos, mientras que los vasos sólo activan los infrarrojos. La estrategia seguida para buscar un vaso es recorrer el campo aleatoriamente, en el caso del aprobado, o siguiendo un recorrido serpenteante, en el notable. Al detectar un obstáculo en un sensor infrarrojo –normalmente se activarán éstos antes que los bumpers, excepto en algunas excepciones- el microbot girará hacia ese lado y avanzará. Si se pulsa un bumper estaremos frente a una pared, si por el contrario se activa el sensor infrarrojo central tendremos un vaso entre los brazos. Para transportar el vaso hacia la pared, en el aprobado simplemente seguimos avanzando hasta encontrarnos con una (en caso de cruzarnos con un vaso, lo esquivaríamos), donde lo dejamos y nos giramos para seguir buscando. En el caso del notable, todos los vasos son llevados a la pared inicial, con lo que no hay posibilidad de cruzarnos con otros vasos durante el transporte. La mayor diferencia con respecto al diseño preliminar es la eliminación de la variable que cuenta los vasos, debido a que es muy complicado saber si realmente hemos recogido todos los vasos. En el notable el microbot es capaz de detectar que ha llegado al final del recorrido y que, por lo tanto, no quedan vasos en el tablero porque ha recorrido todo el campo. En el aprobado es muy difícil asegurar que no acerca dos vasos a la vez, a pesar de que intenta esquivarlos si se da esta situación. Como parte de la presentación, el microbot incluye dos leds azules parpadeantes en la parte inferior del mismo. Éstos se han hecho parpadear usando la interrupción del comparador 2, de forma que cada vez que el temporizador pasa por el valor almacenado en él, el led cambia de estado.

Página 2

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

2. Esquema hardware y conexionado A continuación presentamos el conexionado de la interfaz exterior del microbot: sensores infrarrojos, bumpers y motores.

Para el desarrollo hardware del microbot hemos optimizado los recursos utilizados al menor coste económico y buscando la mayor simplicidad en el uso, consiguiendo el resultado deseado. Para los brazos del microbot hemos utilizado piezas originales de Lego con un trozo de madera para afianzar los sensores, además de pegamento y cinta aislante. Hemos colocado tres sensores infrarrojos, junto con sus respectivos bumpers, distribuidos de la siguiente forma: uno en el extremo de cada brazo y el tercero en el centro del microbot. Los sensores de los brazos están destinados a detectar las paredes y los vasos, y los del centro nos indican que transportamos un vaso. El cometido del bumper central es detectar que se ha llegado a una pared en el mismo lugar en el que ya se ha depositado un vaso anteriormente, ya que en esta situación los bumpers y sensores de los brazos quedarían muy alejados de la pared y no la detectarían. Los leds azules están situados en la parte inferior del microbot. Se encuentran conectados al bit 0 del puerto B. La configuración hardware del robot fue ligeramente modificada al pasar del aprobado al notable, con lo que está optimizada para este programa, y pudiera ser que aparecieran algunos problemas en la ejecución del aprobado.

Página 3

Laboratorio de Sistemas Digitales 3.

E.T.S.I. de Telecomunicación-UMA

Flujograma

Página 4

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

Página 5

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

4. Plan de Pruebas Movimiento del microbot: Hemos probado el avance y los giros del microbot sobre varios tipos de materiales y con diferentes condiciones luminosas. Presenta como limitaciones: • El microbot resbala al girar sobre las juntas de las baldosas, lo cuál es especialmente perjudicial para nuestra concepción de recorrido pues se basa en giros determinados por constantes. Sería recomendable realizar la prueba final sobre una superficie lisa como un tablero de madera. • El conector Jack de alimentación falla tanto con pilas como con transformador, iniciando de nuevo el algoritmo. Detección de vasos y pared: El microbot detecta correctamente vasos y paredes, vasos juntos, vasos pegados a la pared, etc... El único fallo encontrado es que en lugares demasiado iluminados o en los que la luz llegue directamente a los sensores infrarrojos éstos tienen un mal funcionamiento. Transporte de los vasos: En el algoritmo del notable el transporte de los vasos es sencillo puesto que no se encuentra vasos en su recorrido hacia la pared, sin embargo el aprobado ha necesitado de pruebas para esquivar los vasos cuando ya lleva uno. El plan de pruebas ha consistido en colocar vasos en diferentes posiciones delante del microbot cuando éste ya llevaba un vaso. En cualquiera de los casos, existe una pequeña posibilidad de que al acercar frontalmente un vaso a una pared en la que ya hay un vaso el microbot se quede atrancado con los dos vasos, aunque normalmente el bumper central detecta que estamos empujando los vasos contra una pared.

Página 6

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

5. Código fuente ****************************************************************************** * * * Laboratorio de Sistemas Digitales * * E.T.S.I. Telecomunicacion - Universidad de Malaga * * * * MICROBOT RECOLECTOR Peribot© * * ============================ * * Ildefonso Gomariz Abril * * Jose Maria Lopez Pestaña * * Agustin Sanchez Aranda * * * * Notl.asm -> Algoritmo de recoleccion de vasos optimizado. Recorre el * * campo secuencialmente siguiendo un camino y transporta * * todos los vasos a la pared inicial. * * Incluye la interrupcion del parpadeo de los leds azules * * * ****************************************************************************** ; Distribucion de elementos externos: ; S1 B1 S4 B2 ; ¡ ¡ Motor 1 -> Izquierda ; | | Motor 2 -> Derecha ; \ / Sensor 1 -> Izquierda ; \ / Sensor 2 -> Central ; |S2 B3| Sensor 3 -> NC ; |__¡__| Sensor 4 -> Derecha ; M1 o| |o M2 Bumper 1 -> Izquierda ; | | Bumper 2 -> Derecha ; |_____| Bumper 3 -> Central ; o

(PA3, PA5) (PA4, PA6) (PA0) (PA1) (PA2) (PE0) (PE1) (PE2)

*** CODIGO ENSAMBLADOR *** ****************************************************************************** * * * Directivas al ensamblador * * * ****************************************************************************** ; Declaracion de constantes ; Direccion de movimiento del robot ADELANTE EQU $18 DERECHA EQU $58 DERECHA2 EQU $08 ; Giro abierto IZQUIERDA EQU $38 IZQUIERDA2 EQU $10 ; Giro abierto ATRAS EQU $78 STOP EQU $00 ; Posicion relativa de los sensores y bumpers en los puertos SENSORIZQ EQU $01 SENSORDER EQU $04 SENSORCEN EQU $02 BUMPERIZQ EQU $01 BUMPERDER EQU $02 BUMPERCEN EQU $04 ; Tiempos de espera en maniobras ESP_ATRAS EQU $18 ; Tiempo de espera durante la marcha atras ESP_GIRO EQU $30 ; Tiempo de espera durante el giro (90 grados largos) ESP_AVAN EQU $49 ; Tiempo de espera durante el avance tras encontrar

Página 7

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

; una pared EQU $2D ; Tiempo de espera durante el giro (90 grados escasos) EQU $04 ; Tiempo de espera durante la fraccion de angulo en la ; alineacion con el vaso ; Puertos utilizados PORTA EQU $0 PORTB EQU $4 PORTE EQU $A TOC2 EQU $18 TMSK1 EQU $22 TFLG1 EQU $23 TMSK2 EQU $24 TFLG2 EQU $25 ESP_CORTO FRAC_ANG

; Declaracion de variables ORG $0000 t_espera bump_act prox_dir retorno angulo

RMB RMB RMB RMB RMB

1 1 1 1 1

; INICIO DEL PROGRAMA ORG $B600 LDX #$1000

; Para el dir. relativo de los registros

LDAB STAB

#DERECHA prox_dir

; Inicializamos prox_dir

BSET

TMSK2,X $03 ; Señal E del contador dividida por 16

LDAB #$7E ; Cargamos la instruccion JMP int_temp STAB $D0 ; en la posicion $00D0 de la memoria LDD #int_temp ; (Direccion de salto del vector de STD $D1 ; interrupcion del temporizador) BSET TMSK1,X $40 ; Desactivar mascara del comparador 2 LDD #$7FFF ; Cada vez que el temporizador pasa STD TOC2,X ; por 7FFF, interrupcion del comp. 2 LDAB #$7E ; Cargamos la instruccion JMP int_cmp2 STAB $DC ; en la posicion $00DC de la memoria LDD #int_cmp2 ; (Direccion de salto del vector de STD $DD ; interrupcion del comparador 2)

CLI

; Permitir interrupciones

****************************************************************************** * * * INICIO: Situacion inicial: el microbot recorre el espacio * * * ****************************************************************************** inicio LDAB #0 ; Inicializamos a 0 bump_act, STAB STAB STAB

bump_act retorno angulo

; retorno y angulo

Página 8

Laboratorio de Sistemas Digitales LDAA #ADELANTE STAA PORTA,X

E.T.S.I. de Telecomunicación-UMA ; Avanzar

; Comprobacion continua de todos los sensores comp BRCLR PORTA,X SENSORIZQ sen_iz ; Sensor izquierdo activado? BRCLR PORTA,X SENSORDER sen_dr ; Sensor derecho activado? BRSET PORTE,X BUMPERIZQ gotofin ; Bumpers laterales activados? BRSET PORTE,X BUMPERDER gotofin ; Si -> Es a la pared final BRCLR PORTA,X SENSORCEN gotovaso ; Sensor central activado? BRA comp gotofin JMP gotovaso JMP

fin vaso

; Debido a la distancia del salto

; Alineacion del microbot con el obstaculo ; Sensor izquierdo activado sen_iz LDAA #IZQUIERDA ; Girar a la izquierda STAA PORTA,X LDAB STAB JSR

#$FRAC_ANG t_espera espera

; El giro lo contabilizamos en ; fracciones de angulo, bucle de ; espera para recorrer esa fraccion

INC

angulo

; Contabilizamos el nº de fracciones

sigue_iz BRCLR PORTA,X SENSORIZQ sen_iz ; Sensor izquierdo activado? ; Si -> Sigue girando LDAA #ADELANTE ; Avanzar STAA PORTA,X BRCLR PORTE,X BUMPERIZQ sig1iz ; Bumper izquierdo activado? LDAB #BUMPERIZQ ; Si -> Es una pared STAB bump_act JMP alinp sig1iz BRCLR PORTE,X BUMPERDER sig2iz ; Bumper derecho activado? LDAB #BUMPERDER ; Si -> Es una pared STAB bump_act JMP alinp sig2iz BRSET PORTA,X SENSORCEN sigue_iz ; Sensor central activado? ; Si -> Es un vaso ang_iz LDAA #DERECHA ; Girar a la derecha STAA PORTA,X LDAB CMPB BEQ DEC

angulo #$00 vaso angulo

; Descontamos el angulo girado ; para alinearnos con el vaso

LDAB STAB JSR

#FRAC_ANG t_espera espera

; Bucle de espera de la fraccion ; de angulo

BRA

ang_iz

; Sensor derecho activado sen_dr LDAA #DERECHA STAA PORTA,X LDAB STAB

#FRAC_ANG t_espera

; Girar a la derecha

; El giro lo contabilizamos en ; fracciones de angulo, bucle de

Página 9

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

JSR

espera

; espera para recorrer esa fraccion

INC

angulo

; Contabilizamos el nº de fracciones

sigue_dr BRCLR PORTA,X SENSORDER sen_dr

; Sensor derecho activado? ; Si -> Sigue girando ; Avanzar

LDAA #ADELANTE STAA PORTA,X BRCLR PORTE,X BUMPERIZQ sig1dr ; Bumper izquierdo activado? LDAB #BUMPERIZQ ; Si -> Es una pared STAB bump_act JMP alinp sig1dr BRCLR PORTE,X BUMPERDER sig2dr ; Bumper derecho activado? LDAB #BUMPERDER ; Si -> Es una pared STAB bump_act JMP alinp sig2dr BRSET PORTA,X SENSORCEN sigue_dr ; Sensor central activado? ; Si -> Es un vaso ang_dr LDAA #IZQUIERDA ; Girar a la izquierda STAA PORTA,X LDAB CMPB BEQ DEC

angulo #$00 vaso angulo

LDAB #FRAC_ANG STAB t_espera JSR espera BRA

; Descontamos el angulo girado ; para alinearnos con el vaso

; Bucle de espera de la fraccion ; de angulo

ang_dr

****************************************************************************** * * * VASO: Nos encontramos con un vaso. Lo llevamos a la pared * * * ****************************************************************************** vaso LDAA prox_dir EORA #$60 ; Girar al lado contrario STAA PORTA,X ; de la proxima direccion LDAB STAB JSR

#ESP_GIRO t_espera espera

; Bucle de espera

LDAA STAA

#ADELANTE PORTA,X

; Avanzar

BSET

TMSK2,X $80

; Desactivar mascara de overflow ; del temporizador transporte ; Transporte del vaso BRCLR PORTA,X SENSORIZQ paredv ; Algun sensor o bumper activado? BRCLR PORTA,X SENSORDER paredv ; Si -> Lo dejamos y retrocedemos BRSET PORTE,X BUMPERIZQ paredv BRSET PORTE,X BUMPERDER paredv BRSET PORTE,X BUMPERCEN paredv BRA transporte

Página 10

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

paredv STAA

; Marcha atras

LDAA #ATRAS PORTA,X

LDAB retorno CMPB #$05 BLS bucle SUBB #$02 STAB retorno bucle

LDAB CMPB BNE

retorno #$00 bucle

BCLR

TMSK2,X $80

LDAA prox_dir STAA PORTA,X LDAB #ESP_CORTO STAB t_espera JSR espera JMP

; ; ; ;

Observamos cuanto hemos avanzado y le restamos 2 si es mayor de de 5, para compensar posibles errores en distancias largas

; Esperamos a que vuelva a la posicion ; donde encontro el vaso

; Activar mascara de overflow ; del temporizador ; Recobrar la direccion de avance

; Bucle de espera

inicio

****************************************************************************** * * * ALINP: Llegamos a una pared. Nos alineamos con ella antes de hacer el giro * * * ****************************************************************************** alinp LDAB bump_act CMPB #BUMPERDER ; Ha sido el bumper derecho? BEQ bump_dr ; Bumper izquierdo activado ; bump_iz LDAA #IZQUIERDA2 ; STAA PORTA,X alin_iz BRCLR PORTE,X BUMPERDER alin_iz ; BRA pared ;

No -> Ha sido el izquierdo Girar a la izquierda (giro abierto) Bumper derecho activado? Si -> Fin de alineacion

; Bumper derecho activado bump_dr LDAA #DERECHA2 ; Girar a la derecha (giro abierto) STAA PORTA,X alin_dr BRCLR PORTE,X BUMPERIZQ alin_dr ; Bumper izquierdo activado? ; Si -> Fin de alineacion ****************************************************************************** * * * PARED: Tras alinearnos con la pared, nos giramos * * * ****************************************************************************** pared LDAA #ATRAS ; Marcha atras STAA PORTA,X LDAB STAB JSR

#ESP_ATRAS t_espera espera

; Bucle de espera

LDAA STAA

prox_dir PORTA,X

; Giro a donde corresponda

Página 11

Laboratorio de Sistemas Digitales

LDAB STAB JSR

#ESP_GIRO t_espera espera

E.T.S.I. de Telecomunicación-UMA

; Bucle de espera

BRSET PORTE,X BUMPERIZQ fin ; Bumpers laterales activados? BRSET PORTE,X BUMPERDER fin ; Si -> Hemos llegado a la pared opuesta LDAA STAA

#ADELANTE PORTA,X

; Avanzar

LDAB STAB JSR

#ESP_AVAN t_espera espera

; Bucle de espera

BRSET PORTE,X BUMPERIZQ fin ; Bumpers laterales activados? BRSET PORTE,X BUMPERDER fin ; Si -> Hemos llegado a la pared opuesta LDAA STAA

prox_dir PORTA,X

LDAB STAB JSR

#ESP_CORTO t_espera espera

; Giro a donde corresponda

; Bucle de espera

BRSET PORTE,X BUMPERIZQ fin ; Bumpers laterales activados? BRSET PORTE,X BUMPERDER fin ; Si -> Hemos llegado a la pared opuesta LDAB EORB STAB JMP

prox_dir #$60 prox_dir

; Actualizacion de la direccion de giro

inicio

; SUBRUTINAS ****************************************************************************** * * * ESPERA: Bucle para introducir un tiempo de espera en el programa * * * ****************************************************************************** espera LDY #$0FFF decy DEY ; Decrementar Y CPY #0 BNE decy ; Y=0? No -> Seguir contando DEC t_espera ; Si -> Decrementar t_espera LDAB t_espera CMPB #0 BNE espera ; t_espera=0? No -> Seguir contando RTS ; RUTINAS DE ATENCION A INTERRUPCION ****************************************************************************** * * * INT_TEMP: Overflow del temporizador * * * ****************************************************************************** int_temp BSET TFLG2,X $80 ; Quitar flag de interrupcion de overflow

Página 12

Laboratorio de Sistemas Digitales

CMPA #ATRAS BEQ atras

atras

INC RTI DEC RTI

E.T.S.I. de Telecomunicación-UMA

; El robot avanza o retrocede?

retorno

; Si avanza, incrementar retorno

retorno

; Si retrocede, decrementar retorno

****************************************************************************** * * * INT_CMP2: Activacion del comparador 2 * * * ****************************************************************************** int_cmp2 BSET TFLG1,X $40 ; Quitar flag de interrupcion del comparador 2 LDAB EORB STAB

PORTB,X ; Comprobamos el estado del led #$01 ; y lo cambiamos PORTB,X

RTI

; FIN DEL PROGRAMA fin LDAA #STOP STAA PORTA,X BRA fin

; El microbot se para

END

Página 13

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

Apéndice – Flujograma y código fuente del aprobado

Página 14

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

****************************************************************************** * * * Laboratorio de Sistemas Digitales * * E.T.S.I. Telecomunicación - Universidad de Málaga * * * * MICROBOT RECOLECTOR Peribot© * * ============================ * * Ildefonso Gomariz Abril * * José María López Pestaña * * Agustín Sánchez Aranda * * * * Apr.asm -> Algoritmo de recolección de vasos no optimizado. Recorre el * * campo en todas direcciones indefinidamente buscando vasos y * * transportándolos hasta las paredes. * * * ****************************************************************************** ; Distribucion de elementos externos: ; S1 B1 S4 B2 ; ¡ ¡ Motor 1 -> Izquierda (PA3, PA5) ; | | Motor 2 -> Derecha (PA4, PA6) ; \ / Sensor 1 -> Izquierda (PA0) ; \ / Sensor 2 -> Central (PA1) ; |S2 B3| Sensor 3 -> NC ; |__¡__| Sensor 4 -> Derecha (PA2) ; M1 o| |o M2 Bumper 1 -> Izquierda (PE0) ; | | Bumper 2 -> Derecha (PE1) ; |_____| Bumper 3 -> Central (PE2) ; o

*** CODIGO ENSAMBLADOR *** ****************************************************************************** * * * Directivas al ensamblador * * * ****************************************************************************** ; Declaracion de constantes ; Direccion de movimiento del robot ADELANTE EQU $18 DERECHA EQU $58 DERECHA2 EQU $08 ; Giro abierto IZQUIERDA EQU $38

Página 15

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

IZQUIERDA2 EQU $10 ; Giro abierto ATRAS EQU $78 ; Posicion relativa de los sensores y bumpers en los puertos SENSORIZQ EQU $01 SENSORDER EQU $04 SENSORCEN EQU $02 BUMPERIZQ EQU $01 ; Estos valores se usan tambien con bump_act, para BUMPERDER EQU $02 ; indicar que bumper ha sido pulsado BUMPERCEN EQU $04 ; Tiempos de espera en maniobras ESP_ATRAS EQU $38 ; Tiempo de espera durante la marcha atras en PARED ESP_GIRO EQU $2F ; Tiempo de espera durante el giro en PARED (90 grados) ESP_ATRASV EQU $10 ; Tiempo de espera durante la marcha atras ; esquivando un vaso pegado a la pared ESP_GIROV EQU $1E ; Tiempo de espera durante el giro esquivando ; un vaso pegado a la pared ESP_BUMP EQU $3FFF ; Tiempo de espera durante la comprobacion de bumpers ESP_ALIN EQU $18 ; Tiempo de marcha atras durante la alineacion con pared MAX90 EQU $2F ; Tiempo maximo de giro en alinp (90 grados) ; Puertos y registros utilizados PORTA EQU $0 PORTE EQU $A TCNTl EQU $F ; Declaracion de variables ORG $0000 t_espera bump_act giro90

RMB 1 RMB 1 RMB 1

; INICIO DEL PROGRAMA ORG $B600 LDX #$1000 ; Para el dir. relativo de los registros ****************************************************************************** * * * INICIO: Situacion inicial: el microbot recorre el espacio * * * ****************************************************************************** inicio LDAA #ADELANTE ; Avanzar STAA PORTA,X ; Comprobacion continua de todos los sensores comp BRCLR PORTA,X SENSORIZQ sen_iz ; Sensor izquierdo activado? BRCLR PORTA,X SENSORDER sen_dr ; Sensor derecho activado? BRSET PORTE,X BUMPERIZQ pared ; Bumpers laterales activados? BRSET PORTE,X BUMPERDER pared ; Si -> Es una pared BRCLR PORTA,X SENSORCEN vaso ; Sensor central activado? BRA comp

; Alineacion del microbot con el obstaculo ; Sensor izquierdo activado

Página 16

Laboratorio de Sistemas Digitales sen_iz

LDAA STAA

#IZQUIERDA PORTA,X

E.T.S.I. de Telecomunicación-UMA ; Girar a la izquierda

sigue_iz BRCLR PORTA,X SENSORIZQ sen_iz ; Sensor izquierdo activado? ; Si -> Sigue girando LDAA #ADELANTE ; Avanzar STAA PORTA,X BRSET PORTE,X BUMPERIZQ pared ; Bumpers laterales activados? BRSET PORTE,X BUMPERDER pared ; Si -> Es una pared BRCLR PORTA,X SENSORCEN vaso ; Sensor central activado? ; Si -> Es un vaso BRA sigue_iz ; Sensor derecho activado sen_dr LDAA #DERECHA ; Girar a la derecha STAA PORTA,X sigue_dr BRCLR PORTA,X SENSORDER sen_dr ; Sensor derecho activado? ; Si -> Sigue girando LDAA #ADELANTE ; Avanzar STAA PORTA,X BRSET PORTE,X BUMPERIZQ pared ; Bumpers laterales activados? BRSET PORTE,X BUMPERDER pared ; Si -> Es una pared BRCLR PORTA,X SENSORCEN vaso ; Sensor central activado? ; Si -> Es un vaso BRA sigue_dr ****************************************************************************** * * * PARED: Nos encontramos con una pared. Damos marcha atras y nos giramos * * * ****************************************************************************** pared LDAA #ATRAS ; Marcha atras STAA PORTA,X LDAB STAB JSR

#ESP_ATRAS ; Bucle de espera t_espera espera

LDAA STAA

#DERECHA PORTA,X

LDAB #ESP_GIRO LDAA TCNTl ANDA #$0F ABA STAA t_espera JSR espera JMP

; Giro a la derecha

; ; ; ;

; Bucle de espera Tomar el byte bajo del temporizador Nos quedamos con los 4 bits menos significativos Se suman, obteniendo un tiempo de espera aleatorio Obtenemos un giro un poco mayor de 90 grados, ; distinto cada vez

inicio

Página 17

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

****************************************************************************** * * * VASO: Nos encontramos con un vaso * * * ****************************************************************************** vaso LDAA #ADELANTE ; Avanzar STAA PORTA,X ; Comprobamos JSR bumpers LDAB CMPB BNE

si el vaso esta pegado a la pared bump_inst ; Bucle de comprobacion de bump_act #0 pared

transporte LDAA STAA

#ADELANTE PORTA,X

BRCLR BRCLR BRCLR LDAB STAB JMP trans1 BRCLR LDAB STAB JMP trans2 BRSET BRA

PORTA,X SENSORIZQ PORTA,X SENSORDER PORTE,X BUMPERIZQ #BUMPERIZQ bump_act alinp PORTE,X BUMPERDER #BUMPERDER bump_act alinp PORTE,X BUMPERCEN transporte

; Sensor izquierdo activado ir_izq JSR bump_inst bumpers LDAB bump_act CMPB #0 BNE alinp

; Bumpers laterales activados? ; Si -> Esta pegado a la pared ; Transporte del vaso ; Avanzar

ir_izq ir_der trans1

; Sensor izquierdo activado? ; Sensor derecho activado? ; Bumper izquierdo activado? ; Si -> Es una pared. Nos ; nos alineamos con ella

trans2

; Bumper derecho activado? ; Si -> Es una pared. Nos ; alineamos con ella

bump_cen

; Bumper central activado?

; Bucle de comprobacion de

; Bumpers laterales activados? ; Si -> Es una pared. Nos ; alineamos con ella ; No -> Esquivar el vaso ; Girar a la derecha

LDAA #DERECHA STAA PORTA,X esquiva_iz BRCLR PORTA,X SENSORIZQ esquiva_iz ; Sensor izquierdo activado? ; Si -> Sigue girando BRA transporte ; Sensor derecho activado ir_der JSR bump_inst LDAB bump_act CMPB #0 BNE alinp

LDAA STAA esquiva_dr

#IZQUIERDA PORTA,X

; Bucle de comprobacion de bumpers ; Bumpers laterales activados? ; Si -> Es una pared. Nos ; alineamos con ella ; No -> Esquivar el vaso ; Girar a la izquierda

Página 18

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

BRCLR PORTA,X SENSORDER esquiva_dr ; Sensor derecho activado? ; Si -> Sigue girando BRA transporte ; Bumper central activado bump_cen LDAA #ATRAS STAA PORTA,X

; Marcha atras

LDAB STAB JSR

#ESP_ATRASV t_espera espera

; Bucle de espera

LDAB STAB

#DERECHA PORTA,X

; Giro a la derecha

LDAB STAB JSR

#ESP_GIROV t_espera espera

; Bucle de espera

JMP

transporte

****************************************************************************** * * * ALINP: Alineacion con la pared para dejar el vaso cerca * * * ****************************************************************************** alinp LDAB #MAX90 ; Inicializacion de giro90 STAB giro90 LDAA STAA

#ATRAS PORTA,X

LDAB #ESP_ALIN STAB t_espera JSR espera LDAB CMPB BEQ

bump_act #BUMPERDER bump_dr

; Marcha atras

; Bucle de espera

; Ha sido el bumper derecho?

; Bumper izquierdo activado ; No -> Ha sido el izquierdo bump_iz LDAA #IZQUIERDA2 ; Girar a la izquierda (giro abierto) STAA PORTA,X alin_iz BRCLR PORTA,X SENSORDER fin_alin ; Sensor derecho activado? ; Si -> Fin de alineacion LDAB #$01 ; Introducimos un pequeño retraso STAB t_espera ; en cada vuelta del bucle para JSR espera ; que dure suficiente tiempo DEC LDAB CMPB BNE

giro90 giro90 #0 alin_iz

JMP

PARED

; Hemos girado 90 grados? ; No -> Sigue girando

Página 19

Laboratorio de Sistemas Digitales

E.T.S.I. de Telecomunicación-UMA

; Bumper derecho activado bump_dr LDAA #DERECHA2 ; Girar a la derecha (giro abierto) STAA PORTA,X alin_dr BRCLR PORTA,X SENSORIZQ fin_alin ; Sensor izquierdo activado? ; Si -> Fin de alineacion LDAB #$01 ; Introducimos un pequeño retraso STAB t_espera ; en cada vuelta del bucle para JSR espera ; que dure suficiente tiempo DEC LDAB CMPB BNE

giro90 giro90 #0 alin_dr

JMP

pared

; Hemos girado 90 grados? ; No -> Sigue girando

; SUBRUTINAS ****************************************************************************** * * * ESPERA: Bucle para introducir un tiempo de espera en el programa * * * ****************************************************************************** espera LDY #$0FFF decy DEY ; Decrementar Y CPY #0 BNE decy ; Y=0? No -> Seguir contando DEC t_espera ; Si -> Decrementar t_espera LDAB t_espera CMPB #0 BNE espera ; t_espera=0? No -> Seguir contando RTS ****************************************************************************** * * * BUMP_INST: Comprobacion de los bumpers durante un instante para * * distinguir entre vaso y pared * * * ****************************************************************************** bump_inst LDY #ESP_BUMP dec BRSET PORTE,X BUMPERIZQ actizq ; Bumpers activados BRSET PORTE,X BUMPERDER actder DEY CPY #0 BNE dec ; Y=0? LDAB #$00 ; Bumpers no activados STAB bump_act RTS actizq ; Bumpers activados LDAB #BUMPERIZQ

Página 20

Laboratorio de Sistemas Digitales STAB RTS

E.T.S.I. de Telecomunicación-UMA

bump_act

actder LDAB #BUMPERDER STAB bump_act RTS

END

Página 21