; ************************************************************************************ ;** Programa para el robot Cilindrico Ucc ** ;** Autor: Guillermo Núñez ** ;** 19/09/2007 ** ;************************************************************************************** LIST P=16F877A RADIX HEX #INCLUDE "C:\Archivos de programa\Microchip\MPASM Suite\P16F877A.INC" __config 0x3FBA cblock 0x20 cont_art_00; Contador de la articulación 0 (byte bajo) {Motor Base, Encoder 1} cont_art_01; cont_art_10; Contador de la articulación 1 (byte bajo) {Cilindro 1, Potenciómetro lineal 1} cont_art_11; cont_art_20; Contador de la articulación 2 (byte bajo) {Cilindro 2, Potenciómetro lineal 2} cont_art_21; cont_art_30; Contador de la articulación 3 (byte bajo) {Muñeca, Encoder 2} cont_art_31; w_temp status_temp pclath_temp dato enc_base band_enc_base; Es la bandera de inicialización del encoder del motor de la base enc_muneca band_enc_muneca ciclos auxint aux2 aux3 endc TOIF EQU 2 #define enc_base_A PORTD,7 ; Entradas de las señales A y B del encoder de la base #define enc_base_B PORTD,6 #define enc_muneca_A PORTD,5 ; Entradas de las señales A y B del encoder de la muñeca #define enc_muneca_B PORTD,4 ;//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ; MACROS ;//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// BANK0 MACRO bcf STATUS,RP0 bcf STATUS,RP1 ENDM BANK1 MACRO BSF STATUS,RP0 BCF STATUS,RP1 ENDM BANK2 MACRO BSF STATUS,RP1 BCF STATUS,RP0 ENDM BANK3 MACRO BSF STATUS,RP1 BSF STATUS,RP0 ENDM ORG 0x00 nop goto Inicio ORG 0x04 Interrupcion movwf w_temp; movf STATUS, W; movwf status_temp; movf PCLATH, W; movwf pclath_temp; BANK0 btfss PIR1, RCIF; Verifica si ocurrió una interrupción por recepción de datos via USART goto Fin_Interrupcion ; Se recibió un dato movf RCREG, W; movwf dato ; bcf RCSTA, CREN; Lo hago cero para borrar un posible bit de Overrun ; movf RCREG, W; ; movf RCREG, W; Leo dos bytes para vaciar el bufer de recepción Art_00 movlw 0x00 subwf dato, W; W:= RCREG - 0; Si le envian "0" entonces se transmite el valor del contador 00 btfss STATUS, Z; goto Art_01 movf cont_art_00, W call Tx; goto Fin_Interrupcion Art_01 movlw 0x01 subwf dato, W; btfss STATUS, Z; goto Art_10 movf cont_art_01, W call Tx; goto Fin_Interrupcion Art_10 movlw 0x02 subwf dato, W; btfss STATUS, Z; goto Art_11 movf cont_art_10, W call Tx; goto Fin_Interrupcion Art_11 movlw 0x03 subwf dato, W; btfss STATUS, Z; goto Art_20 movf cont_art_11, W call Tx; goto Fin_Interrupcion Art_20 movlw 0x04 subwf dato, W; btfss STATUS, Z; goto Art_21 movf cont_art_20, W call Tx; goto Fin_Interrupcion Art_21 movlw 0x05 subwf dato, W; btfss STATUS, Z; goto Art_30 movf cont_art_21, W call Tx; goto Fin_Interrupcion Art_30 movlw 0x06 subwf dato, W; btfss STATUS, Z; goto Art_31 movf cont_art_30, W call Tx; goto Fin_Interrupcion Art_31 movlw 0x07 subwf dato, W; btfss STATUS, Z; goto Reset_Arts movf cont_art_31, W call Tx; goto Fin_Interrupcion Reset_Arts; Reseta los contadores de todas las articulaciones movlw 0x08 subwf dato, W; btfss STATUS, Z; goto Reset_Art_0 clrf cont_art_00; clrf cont_art_01; clrf cont_art_10; clrf cont_art_11; clrf cont_art_20; clrf cont_art_21; clrf cont_art_30; clrf cont_art_31; goto Fin_Interrupcion Reset_Art_0 movlw 0x09 subwf dato, W; btfss STATUS, Z; goto Reset_Art_1 clrf cont_art_00; clrf cont_art_01; goto Fin_Interrupcion Reset_Art_1 movlw 0x0A subwf dato, W; btfss STATUS, Z; goto Reset_Art_2 clrf cont_art_10; clrf cont_art_11; goto Fin_Interrupcion Reset_Art_2 movlw 0x0B subwf dato, W; btfss STATUS, Z; goto Reset_Art_3 clrf cont_art_20; clrf cont_art_21; goto Fin_Interrupcion Reset_Art_3 movlw 0x0C subwf dato, W; btfss STATUS, Z; goto Fin_Interrupcion clrf cont_art_30; clrf cont_art_31; Fin_Interrupcion ; bsf RCSTA, CREN; Habilito nuevamente la recepción de datos bcf PIR1,RCIF ; Clear the interrupt flag movf pclath_temp, W; movwf PCLATH movf status_temp, W; movwf STATUS; swapf w_temp, F; swapf w_temp, w retfie Inicio ;=============================================================================== ; Configuración de los puertos ;------------------------------------------------------------------------------- bsf STATUS,RP0; Activa el banco 1 movlw 0xFF movwf TRISA; Configura todo el puerto A como entradas, aunque solo se use AN0 y AN1 como entradas analógicas movlw 0xF0; Configura RD7:RD4 como entradas y RD3:RD0 como salidas movwf TRISD; movlw b'10000000' movwf TRISC; Se configura RX y TX clrf TRISB; No se utiliza el puerto B ;=============================================================================== ; Configuración de la comunicación serial USART ;------------------------------------------------------------------------------- movlw b'00100100' movwf TXSTA; 8 bits, Habilita transmisión, Modo Asíncrono, Alta velocidad BANK0; Selecciona el Banco 0 bsf PORTC, 6; Aseguro que este bit sea 1 (STOP) y corregir posibles fallos en la comunicación que ocurren al ; encender el PIC bcf RCSTA, CREN; Lo hago cero para borrar un posible bit de Overrun movf RCREG, W; movf RCREG, W; Leo dos bytes para vaciar el bufer de recepción movlw b'10010000' movwf RCSTA; Configura los pines RC7/RX y RC6/TX como pines del puerto serial, 8 bits y CREN=1 BANK1; Selecciona el Banco 1 nuevamente para configurar movlw .129; 129=> 9600 // 64=> 19200 movwf SPBRG; Velocidad de transmision a 19200 Baudios ;=============================================================================== ; Configuración del módulo A/D ;------------------------------------------------------------------------------- BANK0; Selecciona el Banco 0 movlw b'10000001' movf ADCON0; Como se tiene un reloj de 20Mhz se selecciona FOSC/32 para asegurar un mínimo tiempo de Tad=1.6us ; Además se enciende el módulo conversor A/D ; Después de haber obtenido una conversión con el módulo A/D es necesario esperar un mínimo de 2Tad para empezar ; una nueva conversión de datos, esto es 2Tad = 2*1.6us = 3.2us BANK1; Activa el banco 1 movlw 0x80; Selecciona todos los pines del PORTA como entradas analógicas, justifica a la derecha el resultado movwf ADCON1; de la conversión del módulo A/D y toma a Vss y Vdd como señales de referencia ;=============================================================================== ; Configuración del OPTION_REG ;------------------------------------------------------------------------------- movlw b'00000011' movwf OPTION_REG; El TIMER0 se configura para obtener 1 pulso cada 16 ciclos del reloj, cada ciclo de reloj equivale a 0,2us ; Por lo tanto cada 0.2us*16 = 3.2us el TIMER0 se incrementa en 1 ;=============================================================================== ; Configuración de las interrupciones ;------------------------------------------------------------------------------- movlw b'11000000' movwf INTCON; Habilita las interrupciones globales y las periféricas movlw b'00100000' movwf PIE1; Habilita interrupción solo por recepción de datos via USART ;=============================================================================== ; Selecciona el banco 0 ;------------------------------------------------------------------------------- BANK0; Activa el banco 0 ;=============================================================================== ; Inicialización de variables ;------------------------------------------------------------------------------- bsf band_enc_base, 0; Esta bandera sirve para inicializar 'encoder_base_pos_ant' correctamente bsf band_enc_muneca, 0; Esta bandera sirve para inicializar 'encoder_muneca_pos_ant' correctamente clrf cont_art_00; clrf cont_art_01; clrf cont_art_30; clrf cont_art_31; Ciclo_inf; Éste es el cuerpo principal del programa ; Conversión de AN0 movlw b'10000001' movwf ADCON0; Selecciono AN0 bcf PIR1, ADIF; call Retardo; Retardo = 5.4us call Retardo; Retardo = 5.4us call Retardo; Retardo = 5.4us call Retardo; Retardo = 5.4us ; Retardo Total = 21.6us bsf ADCON0, 2; Iniciar la conversión Adquiere_AN0 btfsc ADCON0, 2; Si ADCON0(2)=0 => Terminó la conversión A/D goto Adquiere_AN0 movf ADRESH, W movwf cont_art_11; Contador de la Articulación 1, byte alto BANK1; Activa el banco 1 movf ADRESL, W BANK0; Activa el banco 0 movwf cont_art_10; Contador de la Articulación 1, byte bajo bcf PIR1, ADIF; call Retardo; Espera unos '5.4 us' antes de empezar una nueva conversión A/D ; Conversión de AN1 movlw b'10001001' movwf ADCON0; Selecciono AN1 bcf PIR1, ADIF; call Retardo; Retardo = 5.4us call Retardo; Retardo = 5.4us call Retardo; Retardo = 5.4us call Retardo; Retardo = 5.4us ; Retardo Total = 21.6us bsf ADCON0, 2; Iniciar la conversión Adquiere_AN1 btfsc ADCON0, 2; Si ADCON0(2)=0 => Terminó la conversión A/D goto Adquiere_AN1 movf ADRESH, W movwf cont_art_21; Contador de la Articulación 2, byte alto BANK1; Activa el banco 1 movf ADRESL, W BANK0; Activa el banco 0 movwf cont_art_20; Contador de la Articulación 2, byte bajo bcf PIR1, ADIF; ; Contador del Encoder de la base Regresa_enc_base btfss band_enc_base, 0; goto Rota_enc_base bcf band_enc_base, 0; Esto se hace para inicializar 'enc_base' Lee_senal_B_enc_base_inic clrf enc_base; btfsc enc_base_B; bsf enc_base,0 call Antirebote; btfss enc_base,0; goto Vale_cero_enc_base_B_inic; Vale_uno_enc_base_B_inic btfss enc_base_B; goto Lee_senal_B_enc_base_inic; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve a leer hasta que inicialice correctamente ; En caso contrario, Implica que la señal B es estable y vale 1 goto Lee_senal_A_enc_base_inic; Pasamos a leer la otra señal del encoder que es la A, ya que la B es estable Vale_cero_enc_base_B_inic btfsc enc_base_B; goto Lee_senal_B_enc_base_inic; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve a leer hasta que inicialice correctamente ; En caso contrario, Implica que la señal B es estable y vale 0 ; Por lo tanto pasamos a analizar la señal A Lee_senal_A_enc_base_inic btfsc enc_base_A; bsf enc_base,1 call Antirebote; btfss enc_base,1; goto Vale_cero_enc_base_A_inic; Vale_uno_enc_base_A_inic btfss enc_base_A; goto Lee_senal_B_enc_base_inic; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve a leer hasta que inicialice correctamente ; En caso contrario, Implica que la señal A es estable y vale 1 goto Rota_enc_base; Seguimos con el programa, ya que la A es estable Vale_cero_enc_base_A_inic; btfsc enc_base_A; goto Lee_senal_B_enc_base_inic; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve a leer hasta que inicialice correctamente ; En caso contrario, Implica que la señal B es estable y vale 0 ; Por lo tanto podemos continuar con la ejecución del programa ; Listo ya está inicializado correctamente 'enc_base' Rota_enc_base; clrf auxint; btfsc enc_base_B; bsf auxint,0 call Antirebote; btfss auxint,0; goto Vale_cero_enc_base_B; Vale_uno_enc_base_B btfss enc_base_B; goto Regresa_enc_muneca; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto analiza el otro encoder y no hagas nada aqui todavia ; En caso contrario, Implica que la señal B es estable y vale 1 goto Lee_senal_A_enc_base; Pasamos a leer la otra señal del encoder que es la A, ya que la B es estable Vale_cero_enc_base_B btfsc enc_base_B; goto Regresa_enc_muneca; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto analiza el otro encoder y no hagas nada aqui todavia ; En caso contrario, Implica que la señal B es estable y vale 0 ; Por lo tanto pasamos a analizar la señal A Lee_senal_A_enc_base btfsc enc_base_A; bsf auxint,1 call Antirebote; btfss auxint,1; goto Vale_cero_enc_base_A; Vale_uno_enc_base_A btfss enc_base_A; goto Regresa_enc_muneca; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto analiza el otro encoder y no hagas nada aqui todavia ; En caso contrario, Implica que la señal A es estable y vale 1 goto Senal_AB_enc_base_estable; Seguimos con el programa, ya que la A es estable Vale_cero_enc_base_A; btfsc enc_base_A; goto Regresa_enc_muneca; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto analiza el otro encoder y no hagas nada aqui todavia ; En caso contrario, Implica que la señal B es estable y vale 0 ; Por lo tanto podemos continuar con la ejecución del programa Senal_AB_enc_base_estable ; movf auxint,W ; enc_base <- (Estado actual) xor (Estado anterior) xorwf enc_base,F ; (solo los bits 0 y 1 se ven afectados) ; rrf auxint,F ; Se actualiza el estado anterior y además se puede obtener información del desplazamiento rrf auxint,F rlf enc_base,F ; rlf auxint,F ; rlf enc_base,F ; ; Analiza el bit 2 btfss enc_base,2; Si enc_base(2)==1 => Analiza enc_base_2 goto Enc_base_3 Analiza_enc_base_2; movf enc_base, W; call Procesa_bits_01; btfss aux2, 0; Si aux2==1 => los bits 0 y 1 de W son diferentes, goto Decrementa_Art_0 goto Incrementa_Art_0; Estoy colocando esto asi según la logica que vi en la secuencia del encoder implementado ; Analiza el bit 3 Enc_base_3 btfss enc_base,3; Si enc_base(3)==1 => Analiza enc_base_3 goto Regresa_enc_muneca; Porque no hubo cambio alguno, entonces no hace nada Analiza_enc_base_3; movf enc_base, W; call Procesa_bits_01; btfss aux2, 0; Si aux2==1 => los bits 0 y 1 de W son diferentes, goto Incrementa_Art_0 Decrementa_Art_0 decf cont_art_00, f; movlw .255 subwf cont_art_00, W; cont_art_00 - 255 btfss STATUS, Z; Z=1 => cont_art_00 = 255 goto Regresa_enc_muneca; Se pasa analizar el próximo encoder movf cont_art_01, W; Toma el byte alto btfsc STATUS, Z; Z=0 => cont_art_01 <> 0 goto Corrige_base; Si esto se cumple es porque: cont_art_01 = 0 decf cont_art_01, f; goto Regresa_enc_muneca Corrige_base clrf cont_art_00; goto Regresa_enc_muneca Incrementa_Art_0 incfsz cont_art_00, f; goto Regresa_enc_muneca incf cont_art_01, f; Regresa_enc_muneca btfss band_enc_muneca, 0; goto Rota_enc_muneca bcf band_enc_muneca, 0; Esto se hace para inicializar 'enc_muneca' Lee_senal_B_enc_muneca_inic clrf enc_muneca; btfsc enc_muneca_B; bsf enc_muneca,0 call Antirebote; btfss enc_muneca,0; goto Vale_cero_enc_muneca_B_inic; Vale_uno_enc_muneca_B_inic btfss enc_muneca_B; goto Lee_senal_B_enc_muneca_inic; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve a leer hasta que inicialice correctamente ; En caso contrario, Implica que la señal B es estable y vale 1 goto Lee_senal_A_enc_muneca_inic; Pasamos a leer la otra señal del encoder que es la A, ya que la B es estable Vale_cero_enc_muneca_B_inic btfsc enc_muneca_B; goto Lee_senal_B_enc_muneca_inic; Esta señal del encoder está inestable porque en 50us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve a leer hasta que inicialice correctamente ; En caso contrario, Implica que la señal B es estable y vale 0 ; Por lo tanto pasamos a analizar la señal A Lee_senal_A_enc_muneca_inic btfsc enc_muneca_A; bsf enc_muneca,1 call Antirebote; btfss enc_muneca,1; goto Vale_cero_enc_muneca_A_inic; Vale_uno_enc_muneca_A_inic btfss enc_muneca_A; goto Lee_senal_B_enc_muneca_inic; Esta señal del encoder está inestable porque en 25us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve a leer hasta que inicialice correctamente ; En caso contrario, Implica que la señal A es estable y vale 1 goto Rota_enc_muneca; Seguimos con el programa, ya que la A es estable Vale_cero_enc_muneca_A_inic; btfsc enc_muneca_A; goto Lee_senal_B_enc_muneca_inic; Esta señal del encoder está inestable porque en 25us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve a leer hasta que inicialice correctamente ; En caso contrario, Implica que la señal B es estable y vale 0 ; Por lo tanto podemos continuar con la ejecución del programa ; Listo ya está inicializado correctamente 'enc_muneca' Rota_enc_muneca; clrf auxint; btfsc enc_muneca_B; bsf auxint,0 call Antirebote; btfss auxint,0; goto Vale_cero_enc_muneca_B; Vale_uno_enc_muneca_B btfss enc_muneca_B; goto Ciclo_inf; Esta señal del encoder está inestable porque en 25us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve al inicio del programa principal ; En caso contrario, Implica que la señal B es estable y vale 1 goto Lee_senal_A_enc_muneca; Pasamos a leer la otra señal del encoder que es la A, ya que la B es estable Vale_cero_enc_muneca_B btfsc enc_muneca_B; goto Ciclo_inf; Esta señal del encoder está inestable porque en 25us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve al inicio del programa principal ; En caso contrario, Implica que la señal B es estable y vale 0 ; Por lo tanto pasamos a analizar la señal A Lee_senal_A_enc_muneca btfsc enc_muneca_A; bsf auxint,1 call Antirebote; btfss auxint,1; goto Vale_cero_enc_muneca_A; Vale_uno_enc_muneca_A btfss enc_muneca_A; goto Ciclo_inf; Esta señal del encoder está inestable porque en 25us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve al inicio del programa principal ; En caso contrario, Implica que la señal A es estable y vale 1 goto Senal_AB_enc_muneca_estable; Seguimos con el programa, ya que la A es estable Vale_cero_enc_muneca_A; btfsc enc_muneca_A; goto Ciclo_inf; Esta señal del encoder está inestable porque en 25us no se tiene una señal fija, sino que cambia ; Por lo tanto vuelve al inicio del programa principal ; En caso contrario, Implica que la señal B es estable y vale 0 ; Por lo tanto podemos continuar con la ejecución del programa Senal_AB_enc_muneca_estable ; movf auxint,W ; enc_muneca <- (Estado actual) xor (Estado anterior) xorwf enc_muneca,F ; (solo los bits 0 y 1 se ven afectados) ; rrf auxint,F ; Se actualiza el estado anterior y además se puede obtener información del desplazamiento rrf auxint,F rlf enc_muneca,F ; rlf auxint,F ; rlf enc_muneca,F ; Analiza el bit 2 btfss enc_muneca,2; Si enc_muneca(2)==1 => Analiza enc_muneca_2 goto Enc_muneca_3 Analiza_enc_muneca_2; movf enc_muneca, W; call Procesa_bits_01; btfss aux2, 0; Si aux2==1 => los bits 0 y 1 de W son diferentes, goto Decrementa_Art_3 goto Incrementa_Art_3; Estoy colocando esto asi segun la logica que vi en la secuencia del encoder implementado ; Analiza el bit 3 Enc_muneca_3 btfss enc_muneca,3; Si enc_muneca(3)==1 => Analiza enc_muneca_3 goto Ciclo_inf; Porque no hubo cambio alguno, entonces no hace nada Analiza_enc_muneca_3; movf enc_muneca, W; call Procesa_bits_01; btfss aux2, 0; Si aux2==1 => los bits 0 y 1 de W son diferentes, goto Incrementa_Art_3 Decrementa_Art_3 decf cont_art_30, f; movlw .255 subwf cont_art_30, W; cont_art_30 - 255 btfss STATUS, Z; Z=1 => cont_art_30 = 255 goto Ciclo_inf; movf cont_art_31, W; Toma el byte alto btfsc STATUS, Z; Z=0 => cont_art_31 <> 0 goto Corrige_muneca; Si esto se cumple es porque: cont_art_31 = 0 decf cont_art_31, f; goto Ciclo_inf Corrige_muneca clrf cont_art_30; goto Ciclo_inf Incrementa_Art_3 incf cont_art_30, f; btfsc STATUS, Z; Z=0 => 'cont_art_30' NO pasó de 255 a 0 incf cont_art_31, f; goto Ciclo_inf Retardo bcf INTCON, TOIF; Resetea el bit de overflow del TIMER0 movlw 0xFF movwf TMR0 Retardo_3.2us btfss INTCON, TOIF; goto Retardo_3.2us; return Antirebote; El retraso = (ciclos x 50.4us) // ACTUALMENTE TENGO UN RETRASO DE 25us ; movlw .1; quedamos en 56 ; movwf ciclos ;Repite_ciclo1 bcf INTCON, TOIF; Resetea el bit de overflow del TIMER0 movlw .121; Aqui estaba originalmente .241 para lograr 50.4us movwf TMR0 Retardo_Xus btfss INTCON, TOIF; goto Retardo_Xus; ; decfsz ciclos, f ; goto Repite_ciclo1 return Tx movwf TXREG BANK1 btfss TXSTA, TRMT goto $-1; Espera a que termine de transmitir el dato BANK0 return Procesa_bits_01; Esta subrutina procesa los bits 0 y 1 de W y si son iguales devuelve en aux2(0)=0, si son diferentes => aux2(0)=1 andlw b'00000011' movwf aux3 clrf aux2; btfsc aux3,1; bsf aux2,0; aux2(0) <-- aux3(1) movf aux3, W; xorwf aux2, F; return end