* RoBoT MeGaMoNKeY(tm) * Copyright (c) 2001 by Mix'n'Mojo(tm) * * Aprendizaje y recorrido de un laberinto * mediante el microcontrolador 68HC11. * * Versión: 1.31d (full version) * (aprendizaje a derechas) * * Autor Original: Luis F. Gimilio Barboza * * Modificaciones: Carlos García Argos * Luis F. Gimilio Barboza * * What's new: * * - 1.31d (full): ajuste del tiempo de espera para el control * antirrebotes de IRQ. Detección del fin o salida del * laberinto. ¡Listo para el examen! :-) * * - 1.30 (full): primera versión completa, con cierta * simplificación de código, y revisión completa de * los comentarios. * * - 1.28 (beta): ¡primera versión que funciona! Muchísimos * cambios respecto a las versiones anteriores: giros, * cruces totalmente nuevos, corrección de todos los fallos... * * - 1.20 (beta): modificaciones de Carlos: subrutinas de giro * y ajuste nuevas. * * - 1.19 (alfa): modificación del algoritmo original, incluyendo * la presencia de un flag de error al encontrar un camino * sin salida. * * ETS de Ingenieros de Telecomunicación * Universidad de Málaga * --------------------------------------------------------------- * | Parte 1: Directivas al ensamblador | * --------------------------------------------------------------- * Etiquetas del sistema PORTA EQU $1000 ; Puerto A del microcontrolador (absoluto) OPTION EQU $39 ; Registro de config. opciones (relativo) PILA EQU $C3 ; Inicio del área de pila (absoluto) * Etiquetas de opciones INACTIVO1 EQU $00 ; Fase de espera previa al aprendizaje APRENDER EQU $40 ; Fase de aprendizaje del laberinto INACTIVO2 EQU $80 ; Fase de espera previa al recorrido REPRODUCIR EQU $C0 ; Fase de recorrido del laberinto SALTO EQU $40 ; Salto entre fases * Etiquetas de sensores VAL01 EQU $03 ; NOTA: Para ver el significado de estos VAL02 EQU $02 ; valores, ver página 6 de la memoria. VAL03 EQU $01 VAL04 EQU $00 VAL05 EQU $87 VAL06 EQU $84 VAL07 EQU $80 VAL08 EQU $04 VAL09 EQU $83 VAL10 EQU $07 VAL11 EQU $05 VAL12 EQU $82 VAL13 EQU $06 VAL14 EQU $81 VAL15 EQU $85 VAL16 EQU $86 * Etiquetas de posición/movimiento GIRO EQU $40 ; Valor de dirección giro a derechas GIRO180 EQU $80 ; Valor de dirección giro 180º ADELANTE EQU $00 ; Dirección adelante IZQUIERDA EQU $C0 ; Dirección izquierda ATRAS EQU $80 ; Dirección atrás DERECHA EQU $40 ; Dirección derecha * Etiquetas de tiempos T_AJ1 EQU $03 ; Tiempo de giro (Ajuste) T_AJ2 EQU $04 ; Tiempo de comprobación (Ajuste) T_IRQ EQU $0A ; Tiempo antirrebotes IRQ T_ESP EQU $1D00 ; Tiempo de espera (Espera) T_GIRA1 EQU $04 ; Tiempo de giro (bucle mayor) T_GIRA2 EQU $1FFF ; Tiempo de giro (bucle menor) T_AV_CRUCE EQU $0A ; Tiempo para dejar atrás un cruce * Etiquetas de motores/puerto A M_STOP EQU $00 ; Detenido, con el LED apagado M_LED EQU $40 ; Detenido, con el LED encendido M_AVAN EQU $18 ; Ambos motores hacia delante M_IZQ1 EQU $38 ; Izquierda: atrás Derecha: adelante M_IZQ2 EQU $10 ; Izquierda: off Derecha: adelante M_DER1 EQU $58 ; Izquierda: adelante Derecha: atrás M_DER2 EQU $08 ; Izquierda: adelante Derecha: off * --------------------------------------------------------------- * | Parte 2: Configuración inicial | * --------------------------------------------------------------- * Reserva de espacio en memoria RAM ORG $0 direcc RMB 1 ; Dirección a la que apunta el microbot caminos RMB 1 ; Caminos posibles en cada cruce indice RMB 2 ; Indice para el vector de reproducción pila_temp RMB 2 ; Valor temporal de la pila inter RMB 1 ; Estado del programa error RMB 1 ; Para el flag de camino incorrecto * Inicio del programa en eeprom ORG $B600 ; Inicio de la eeprom * Inicialización de la interrupción de IRQ LDAA #$7E ; Carga el código de JUMP en el acumulador A STAA $EE ; Guarda dicho código en la posición $EE LDD #Int_irq ; Carga la dirección de la rutina de atención en D STD $EF ; Almacena dicho valor en la posición $EF * Configuración del sistema LDX #PORTA ; Inicializa registro X al puerto A CLI ; Permite interrupciones * --------------------------------------------------------------- * | Parte 3: Desarrollo del programa | * --------------------------------------------------------------- Inicio LDAA #INACTIVO1 ; Carga el valor inicial STAA inter ; en la posición de memoria inter. LDS #PILA ; Inicializa el stack pointer * ---> Estados 0 y 2: bucle de espera Inactivo LDAA inter ; Carga el modo actual en el registro Y CMPA #APRENDER ; ¿Modo aprendizaje? BEQ Activo BLO Detener ; Modo espera 1 CMPA #REPRODUCIR ; ¿Modo reproducir? BEQ Activo LDAA #M_LED ; Modo espera 2: enciende LED BRA Fin_inactivo Detener LDAA #M_STOP ; Detiene al microbot (modos de espera) Fin_inactivo STAA $0,X BRA Inactivo * ---> Estados 1 y 3: modo aprendizaje y reproducción Activo LDAA #ADELANTE ; Carga en el acumulador la dirección adelante STAA direcc ; y se almacena en la posición de memoria direcc LDY #PILA ; Carga el valor de índice del vector de STY indice ; reproducción, que coincide con la pila. LDAA #M_STOP ; Carga un cero en la posición de memoria STAA error ; error (flag de camino equivocado). Sondeo LDAA inter ; Comprueba si no finaliza el modo CMPA #INACTIVO2 ; de aprendizaje/reproducción. BEQ Inactivo BRCLR $0,X VAL05 TodoBlanco ; Detecta todo blanco BRCLR $0,X VAL15 Corrige_der ; Detecta desviación izquierda BRCLR $0,X VAL16 Corrige_izq ; Detecta desviación derecha BRCLR $0,X VAL06 Avanza ; Detecta línea adelante BRA Cruce ; Y si no, es un cruce TodoBlanco JSR Ajuste ; Avanza un poco (evita errores) BRCLR $0,X VAL05 A_Fuera ; Detecta fuera del circuito BRCLR $0,X VAL09 Curva_der ; Detecta curva a la derecha BRCLR $0,X VAL10 Curva_izq ; Detecta curva a la izquierda BRA Cruce_2 ; En otro caso, es un cruce A_Fuera JMP Fuera Corrige_izq LDAA #M_IZQ2 ; Activa sólo motor derecho BRA Fin_corrige ; (corrección hacia la izquierda) Corrige_der LDAA #M_DER2 ; Activa sólo motor izquierdo BRA Fin_corrige ; (corrección hacia la derecha) Avanza LDAA #M_AVAN ; Activa ambos motores hacia delante Fin_corrige STAA $0,X ; Realiza el cambio en los motores BRA Sondeo Curva_izq LDAA #M_IZQ1 ; Sentido de los motores LDAB #IZQUIERDA ; Sentido del giro BRA Curva Curva_der LDAA #M_DER1 ; Sentido de los motores LDAB #DERECHA ; Sentido del giro Curva JSR Gira ; Realiza el giro de forma completa LDAA #GIRO CBA ; Comprueba si el giro realizado es a la derecha, BEQ Sigue_curva ; y en ese caso se suma 40 a la dirección. NEGA ; En caso contrario, se resta dicho valor. Sigue_curva LDAB direcc ABA ; Suma/resta el valor GIRO a la dirección STAA direcc ; y la almacena. BRA Sondeo Cruce JSR Ajuste ; Avanza, para comprobación Cruce_2 LDAA error ; Comprueba si el flag de error CMPA #$01 ; está activo, pues el tratamiento BEQ Cruce_error ; en ese caso es distinto. LDAA inter ; Comprueba si está en modo de CMPA #REPRODUCIR ; reproducción. BEQ Cruce_rep BRCLR $0,X VAL01 Cruce_T ; Detecta cruce en T BRSET $0,X VAL06 Cruce_X ; Detecta cruce en X BRSET $0,X VAL07 Cruce_izq ; Detecta cruce adelante e izquierda BRSET $0,X VAL08 Cruce_der ; Detecta cruce adelante y derecha JMP Fin_cruce Cruce_error PULB ; Saca el último valor de la pila ANDB #$0F ; Elimina los 4 bits superiores DECB ; Decrementa los 4 bits inferiores BEQ Sigue_error ; Comprueba si quedan caminos LDAA #$00 ; Si todavía quedan, baja el flag STAA error ; de camino equivocado. Sigue_error BRSET $0,X VAL08 Gira_cruce ; Comprueba camino a la derecha STAB caminos ; Almacena los caminos restantes LDAB #$00 BRA Sigue_cruce Cruce_X LDAB #$03 ; Cruce en X: 3 caminos posibles BRA Gira_cruce Cruce_T LDAB #$02 ; Cruce en T: 2 caminos posibles BRA Gira_cruce Cruce_izq LDAB #$02 ; Cruce izq.: 2 caminos posibles STAB caminos LDAB #$00 ; Cruce de frente y a la izquierda, BRA Sigue_cruce ; no hace falta ningún giro. Cruce_der LDAB #02 ; Cruce der.: 2 caminos posibles Gira_cruce STAB caminos ; Guarda el número de caminos LDAA #M_DER1 ; Carga el valor de giro derecha JSR Gira ; Realiza el giro completo LDAB #GIRO ; Carga el valor $40 Sigue_cruce LDAA direcc ; Actualiza la nueva dirección ABA ; sumando el salto pertinente. STAA direcc ; La almacena en memoria BSR Ajuste ; Avanza hasta dejar atrás el cruce LDAA error ; Comprueba si el flag de error CMPA #$01 ; está activo, y en ese caso no BEQ Fin_cruce ; se introduce nada en pila. LDAA direcc ; Direcc. --> 4 bits superiores LDAB caminos ; Caminos --> 4 bits inferiores ABA ; Suma ambos valores PSHA ; Y los almacena en la pila BRA Fin_cruce Cruce_rep STS pila_temp ; Almacena temporalmente el SP LDS indice ; Carga el índice en registro S DES ; Lo decrementa STS indice ; Y actualiza la variable índice PULA ; Saca el valor del índice LDS pila_temp ; Restaura la pila ANDA #$F0 ; Elimina los 4 bits inferiores LDAB direcc ; Carga la dirección en B NEGB ; Y la niega, para obtener el valor ABA ; de la resta en A. CMPA #DERECHA ; Comprueba si es giro a la derecha BEQ Rep_der ; Y en ese caso se realiza el giro CMPA #IZQUIERDA ; Comprueba si es giro a la izquierda BEQ Rep_izq ; Y en ese caso se realiza el giro LDAA #M_AVAN ; En otro caso, avanza hasta dejar STAA $0,X ; atrás el cruce. LDAA #T_AV_CRUCE BSR Espera Fin_cruce JMP Sondeo Rep_der JMP Curva_der Rep_izq JMP Curva_izq * Fuera de la línea Fuera BSR Ajuste ; Sale completamente de la línea BSR Ajuste ; negra, para poder dar un buen giro. LDAA inter ; Comprueba si estamos en CMPA #REPRODUCIR ; modo reproducción. BEQ Fuera_rep ; En ese caso, fin del laberinto LDAA #M_IZQ1 ; Realiza el giro completo JSR Gira ; hacia la izquierda. LDAA direcc ; Carga la dirección en A LDAB #GIRO180 ; Carga el valor del giro completo en B ABA ; Suma ambos valores STAA direcc ; Almacena la nueva dirección en direcc LDAA #$01 ; Activa el flag de camino equivocado STAA error ; y lo almacena en la variable error. BRA Fin_fuera Fuera_rep LDAA #INACTIVO2 ; Carga el estado 2 en el registro A STAA inter ; Almacena el cambio en la memoria Fin_fuera JMP Sondeo * --------------------------------------------------------------- * | Parte 4: Subrutinas | * --------------------------------------------------------------- * Subrutina de ajuste, avanza y gira al llegar a un posible cruce Ajuste LDAA #M_AVAN ; En principio, avance BRCLR $0,X VAL06 Sigue_ajuste ; Si estamos en una linea o fuera BRCLR $0,X VAL08 Ajuste_izq ; Si el de la derecha esta blanco BRCLR $0,X VAL07 Ajuste_der ; O si lo está el de la izquierda BRA Sigue_ajuste Ajuste_izq LDAA #M_IZQ2 ; Gira hacia la izquierda BRA Sigue_ajuste Ajuste_der LDAA #M_DER2 ; Gira hacia la derecha Sigue_ajuste STAA $0,X ; Hace la operación que proceda, LDAA #T_AJ1 ; según el estado de los sensores BSR Espera LDAA #M_AVAN ; Ahora avanza un cierto tiempo STAA $0,X LDAA #T_AJ2 BSR Espera RTS * Subrutina de espera Espera LDY #T_ESP ; Carga en Y el valor de la cuenta Bucle_esp DEY ; Realiza la cuenta menor BNE Bucle_esp DECA ; Realiza la cuenta mayor BNE Espera RTS * Subrutinas de giro Gira PSHB ; Almacena el registro B STAA $0,X ; Carga el puerto con el giro Encuentra1 BRCLR $0,X VAL01 Encuentra2 ; Abandona la línea negra actual BRA Encuentra1 Encuentra2 BRSET $0,X VAL01 Sigue_gira ; Busca la siguiente línea negra BRA Encuentra2 Sigue_gira LDAA #M_STOP ; Detiene de forma temporal STAA $0,X ; el robot. LDAB #T_GIRA1 Bucle_gira1 LDY #T_GIRA2 ; Comienza a ajustar el robot a Bucle_gira2 ; la línea negra durante un cierto DEY ; tiempo, para dejar atrás la curva BEQ Fin_gira2 ; o el cruce. BRCLR $0,X VAL02 Gira_izq ; Detecta desviación derecha BRCLR $0,X VAL03 Gira_der ; Detecta desviación izquierda Gira_av LDAA #M_AVAN ; En otro caso, avanza BRA Sigue2 Gira_der LDAA #M_DER2 ; Carga el valor de giro a derecha BRA Sigue2 Gira_izq LDAA #M_IZQ2 ; Carga el valor de giro a izquierda Sigue2 STAA $0,X ; Lo almacena y vuelve al bucle BRA Bucle_gira2 Fin_gira2 DECB ; Controla el bucle mayor BEQ Fin_gira1 BRA Bucle_gira1 Fin_gira1 PULB ; Restaura el valor del ac. B RTS * --------------------------------------------------------------- * | Parte 5: Rutinas de atención a interrupción | * --------------------------------------------------------------- * Rutina de atención de la interrupción de IRQ Int_irq LDAA inter ; Carga el modo actual en el registro A CMPA #REPRODUCIR ; Comprueba si estaba en modo reproducción BEQ Vuelve_2 ; En ese caso, vuelve a dicho modo LDAB #SALTO ; Si no, se suma 40 al modo, pasando al ABA ; modo correspondiente. BRA Fin_irq Vuelve_2 LDAA #INACTIVO2 ; Carga el estado 2 en el registro A Fin_irq STAA inter ; Almacena el cambio en la memoria LDAA #T_IRQ ; Espera un cierto tiempo BSR Espera ; para evitar rebotes. RTI ; Retorno de interrupción END