| /***************************************************************** * Mortal3.c:Programa velocista para el pic18f452 * *****************************************************************/ /**************** INCLUDES *********************** *************************************************/ #include <p18f452.h> // Include para las definiciones del PIC18f452 #include <delays.h> // Include para las funciones de RETARDO #include <adc.h> // Include para el manejo del ADC #include <stdlib.h> // Include para la funcion ITOA #include <usart.h> // Include para el manejo del PUERTO SERIE #include <pwm.h> // Include para el manejo de la PWM #include <timers.h> // Include para configurar los TIMERS #include <portb.h> // Include para configurar los pull-up del PUERTO B /**************** CONSTANTES ********************* *************************************************/ #define OFF 1 #define ON 0 #define HIGH 1 // Constante para el nivel alto de un pin #define LOW 0 // Constante para el nivel bajo de un pin #define LED0 PORTDbits.RD0 // Led 0 se controla con el pin RD0 #define LED1 PORTDbits.RD1 // Led 1 se controla con el pin RD1 #define LED2 PORTDbits.RD2 // Led 2 control RD2 #define LED3 PORTDbits.RD3 #define LED4 PORTDbits.RD4 // LEDS 3,4,5 controles RD3,RD4,RD5 #define LED5 PORTDbits.RD5 #define SENSOR_I ADC_CH0 // Sensor izquierdo = Canal 0 del ADC #define SENSOR_D ADC_CH1 // Sensor derecho = Canal 1 del ADC #define SENSOR_C ADC_CH2 // Sensor izquierdo interior = Canal 3 del ADC #define SENSOR_E ADC_CH3 // Sensor derecho interior =Canal 4 del ADC #define SENSOR_F ADC_CH4 //Sensor central izq =Canal 5 del ADC #define SENSOR_G ADC_CH5 //Sensor central der= Canal 6 del ADC #define ENAPWM1 PORTCbits.RC5 // Pin enable motor de traccion #define ENAPWM2 PORTCbits.RC0 // Pin enable motor de giro #define GIRA_DER 2 // Estado: Girando a derecha #define GIRA_IZQ 1 // Estado: Girando a izquierda #define NO_GIRA 0 // Estado: En recta #define CONVIRTIENDO ADCON0bits.GO_DONE // Bit de inicio y fin de conversion del ADC /************* VARIABLES GLOBALES **************** *************************************************/ int PWM_GIRO_IZ; int PWM_GIRO_DER; int UMBRAL ; // Umbral para lecturas de sensores int VEL_GIRA_I ; //425 Velocidad coche girando a izquierda int VEL_GIRA_D ; //425 Velocidad coche girando a derecha int VEL_RECTO ; //725 Velocidad coche en recta int VEL_MAX ; //velocidad maxima para los sensores centrales int VEL_TRAS_CURVA ; //350 Velocidad freno tras una curva int ANG_GIRA_I ; //800,1023 Rapidez giro a izquierda (enable LOW) int ANG_GIRA_D ; //0 Rapidez giro a derecha (enable HIGH) int NO_ANG ; // No gira int ant_estado,emergencia; // almacena estado anterior al actual //int ANG_GIRA_I,ANG_GIRA_D; /**************** FUNCIONES ********************** *************************************************/ /* Configuracion de la PWM */ void Init_Pwm(void) { // Configuracion de los pines TRISCbits.TRISC0 = 0; //RC0 como salida => Enable PWM2 TRISCbits.TRISC3 = 0; //RC3 como salida => Enable PWM1 // Configuracion del timer2, para las PWM OpenTimer2(T2_POST_1_1); // Configuracion de las PWM a la frecuencia m xima OpenPWM1(0xFF); OpenPWM2(0xFF); // Configuracion de inicio de los motores SetDCPWM1(0x00); // parado ENAPWM1 =LOW; SetDCPWM2(0x00); // recto ENAPWM2 = LOW; } /* Configuracion del PUERTO SERIE */ void Conf_USART(void) { OpenUSART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE &USART_EIGHT_BIT & USART_BRGH_HIGH, 129); //Usart configurada para 9600 baudios } /* Configuracion del ADC */ void Conf_ADC(void) { OpenADC(ADC_FOSC_RC & ADC_RIGHT_JUST & ADC_8ANA_0REF, ADC_INT_OFF); } /* Configuracion LEDS */ void Conf_Leds_Sensores(void) { // Configuramos sus pines como salidas TRISDbits.TRISD0 = 0; TRISDbits.TRISD1 = 0; TRISDbits.TRISD2 = 0; TRISDbits.TRISD3 = 0; TRISDbits.TRISD4 = 0; TRISDbits.TRISD5 = 0; // Apagamos los leds LED0 = HIGH; LED1 = HIGH; LED2 = HIGH; LED3 = HIGH; LED4 = HIGH; LED5 = HIGH; } /* Configuracion SWICH */ void Conf_PORTB(void) { OpenPORTB(PORTB_CHANGE_INT_OFF & PORTB_PULLUPS_ON); } /* Lectura del canal del ADC */ int LeeADC(unsigned char canal) { int medida; // int = 16 bits para una medida de 10 bits // Realizo la conversion del canal pedido y retorno su valor SetChanADC(canal); Delay10TCYx(5); ConvertADC(); while(CONVIRTIENDO); medida=ReadADC(); return(medida); } /* Establecimiento de los giros del coche */ void Girar(unsigned char estado) { // Dependiendo del estado actual pasado ... switch(estado) { // Giramos a la derecha case GIRA_DER: // Giro a derecha SetDCPWM2(ANG_GIRA_D); ENAPWM2 = HIGH; // Cargo velocidad de giro a derecha SetDCPWM1(VEL_GIRA_D); ENAPWM1 = LOW; break; // Giramos a la izquierda case GIRA_IZQ: // Giro a izquierda SetDCPWM2(ANG_GIRA_I); ENAPWM2 = LOW; // Cargo velocidad de giro a izquierda SetDCPWM1(VEL_GIRA_I); ENAPWM1 = LOW; break; // Seguimos recto case NO_GIRA: // Centro la dieccion SetDCPWM2(NO_ANG); ENAPWM2 = LOW; // Cargo la velocidad en recta if(ant_estado != NO_GIRA) // Si vengo de un estado de curva ... { SetDCPWM1(VEL_TRAS_CURVA); // ... acelero menos } else { SetDCPWM1(VEL_RECTO); // ... o pongo velocidad de recta } ENAPWM1 = LOW; break; } } /**************** MAIN **************************** **************************************************/ void main() { //valores por defecto int PWM=0; int medida_ADC=0; // Valor de la medida del ADC int der=0,izd=0,cen=0;// Estado de SENSOR_D y SENSOR_I respectivamente int der2=0,izd2=0,cen2=0;// Estado de SENSOR_D y SENSOR_I respectivamente // Inicializamos todos los dispositivos y funciones Conf_Leds_Sensores(); Conf_USART(); Conf_ADC(); Init_Pwm(); Conf_PORTB(); // Cargamos valores iniciales a las variables y motores ant_estado = NO_GIRA; // El estado anterior es NO_GIRA SetDCPWM1(VEL_RECTO); // Velocidad de recto ENAPWM1 = LOW; SetDCPWM2(NO_ANG); // Direccion recto ENAPWM2 = LOW; //Configuracion por defecto UMBRAL = 800; // Umbral para lecturas de sensores VEL_GIRA_I = 355; //425 Velocidad coche girando a izquierda VEL_GIRA_D = 355; //425 Velocidad coche girando a derecha VEL_RECTO = 675; //725 Velocidad coche en recta VEL_MAX = 1023; //velocidad maxima para los sensores centrales VEL_TRAS_CURVA = 350; //350 Velocidad freno tras una curva ANG_GIRA_I = 1023; //800,1023 Rapidez giro a izquierda (enable LOW) ANG_GIRA_D = 0 ; //0 Rapidez giro a derecha (enable HIGH) NO_ANG = 0 ; // No gira //configuracion swich 1=on if(PORTBbits.RB1 == ON) { UMBRAL = 800; // Umbral para lecturas de sensores VEL_GIRA_I = 400; //425 Velocidad coche girando a izquierda VEL_GIRA_D = 400; //425 Velocidad coche girando a derecha VEL_RECTO = 775; //725 Velocidad coche en recta VEL_MAX = 1023; //velocidad maxima para los sensores centrales VEL_TRAS_CURVA = 400; //350 Velocidad freno tras una curva ANG_GIRA_I = 1023; //1023 Rapidez giro a izquierda (enable LOW) ANG_GIRA_D = 0 ; //0 Rapidez giro a derecha (enable HIGH) NO_ANG = 0 ; // No gira } //configuracion swich 2=on if(PORTBbits.RB2 == ON) { UMBRAL = 800; // Umbral para lecturas de sensores VEL_GIRA_I = 500; //425 Velocidad coche girando a izquierda VEL_GIRA_D = 500; //425 Velocidad coche girando a derecha VEL_RECTO = 875; //725 Velocidad coche en recta VEL_MAX = 1023; //velocidad maxima para los sensores centrales VEL_TRAS_CURVA = 500; // Velocidad freno tras una curva ANG_GIRA_I = 1023; //1023 Rapidez giro a izquierda (enable LOW) ANG_GIRA_D = 0 ; //0 Rapidez giro a derecha (enable HIGH) NO_ANG = 0 ; // No gira } //configuracion swich 3=on if(PORTBbits.RB3 == ON) { UMBRAL = 800; // Umbral para lecturas de sensores VEL_GIRA_I = 555; //425 Velocidad coche girando a izquierda VEL_GIRA_D = 555; //425 Velocidad coche girando a derecha VEL_RECTO = 850; //725 Velocidad coche en recta VEL_MAX = 1023; //velocidad maxima para los sensores centrales VEL_TRAS_CURVA = 550; //350 Velocidad freno tras una curva ANG_GIRA_I = 1023; //1023 Rapidez giro a izquierda (enable LOW) ANG_GIRA_D = 0 ; //0 Rapidez giro a derecha (enable HIGH) NO_ANG = 0 ; // No gira } //configuracion swich 4=on if(PORTBbits.RB4 == ON) { UMBRAL = 800; // Umbral para lecturas de sensores VEL_GIRA_I = 655; //425 Velocidad coche girando a izquierda VEL_GIRA_D = 655; //425 Velocidad coche girando a derecha VEL_RECTO = 975; //725 Velocidad coche en recta VEL_MAX = 1023; //velocidad maxima para los sensores centrales VEL_TRAS_CURVA = 650; //350 Velocidad freno tras una curva ANG_GIRA_I = 1023; //800,1023 Rapidez giro a izquierda (enable LOW) ANG_GIRA_D = 0 ; //0 Rapidez giro a derecha (enable HIGH) NO_ANG = 0 ; // No gira } /* Maquina de estados para el control del vehiculo */ while(1) { /* Tomamos el estado actual de los sensores */ if ((LeeADC(SENSOR_G) > UMBRAL ) || (LeeADC(SENSOR_F) > UMBRAL )) { SetDCPWM1(VEL_MAX); // Velocidad MAXIMA LED4=LOW; LED5=LOW; } else { SetDCPWM1(VEL_RECTO); // Velocidad normal LED4=HIGH; LED5=HIGH; } if( LeeADC(SENSOR_C) > UMBRAL ) { der2=1; // sensor dentro linea LED3= LOW; if( LeeADC(SENSOR_D) < UMBRAL ) { der2=0; // 1 sensor dentro linea } } if( LeeADC(SENSOR_E) > UMBRAL ) { izd2=1; // 1 sensor dentro de linea LED2=LOW; if( LeeADC(SENSOR_I)< UMBRAL ) { izd2=0; // 1 sensor dentro de linea } } if (der2==1) izd=0; if (izd2==1) der=0; if (der2==0) izd=1; if (izd2==0) der=1; /* Tomamos decision a partir del nuevo estado */ // Si se ha salido por la izquierda ... if(der && !izd) { Girar(GIRA_DER); // Giro con premisa: a la derecha LED0 = LOW; // Visualizamos el estado en los leds LED1 = HIGH; // LED2 =LOW; // LED3 = HIGH; ant_estado = GIRA_DER; // Actualizamos valor de ant_estado } // Si se ha salido por la derecha ... else if(!der && izd) { Girar(GIRA_IZQ); // Giro con premisa: a la izquierda LED0 = HIGH; // Visualizamos el estado en los leds LED1 = LOW; // LED2 = HIGH; // LED3 = LOW; ant_estado = GIRA_IZQ; // Actualizamos valor de ant_estado } // Si no se ha salido de la linea ... else if(der && izd) { Girar(NO_GIRA); // Giro con premisa: ir recto LED0 = LOW; // Visualizamos el estado en los leds LED1 = LOW; // LED2 = LOW; // LED3 = LOW; ant_estado = NO_GIRA; // Actualizamos valor de ant_estado } } } |
El programa que presentamos a continuación fue desarrollado por SorgoNet.com en la última edición de la Campus-party 2002 que se celebro en Valencia, con motivo del primer seminario de microrrobotica impartido por D.Julio Pastor de la universidad de Alcalá (Alcabot). El algoritmo obtuvo el 2º puesto en la competición de robots velocistas que se realizo al final del seminario. Para el hardware del robot se utilizo la estructura de un coche teledirigido, por lo tanto no le daremos relevancia a este punto. La tarea del robot velocista consiste en recorrer un circuito marcado por 2 lineas negras en un tiempo menor que el de sus oponentes, por supuesto ajustándose lo mas posible a estas lineas, de lo contrario quedaría descalificado Para la gestión del bot se utilizo un microcontrolador 18F452 y 6 sensores de infrarrojos tipo CNY70. El programa presentamos controla 6 sensores en la parte delantera del vehículo dándole al robot los datos suficiente para recorrer el circuito. 2 sensores se encargan de detectar la salida del circuito indicando al robot que gire en un sentido u otro , otros 2 se encargan de detectar que ha regresado al circuito indicando al robot que continué recto y los 2 últimos activan la máxima velocidad del robot pues en cada giro se obliga al bot a reducir su velocidad, El programa también consta de 5 configuraciones distintas, activables a través de uno swiches,estas configuraciones son usadas para los distintos oponentes,circuitos o la carga en batería de la que disponemos. Alguna imagenes del robot las podeis encontrar aquí.
Para enviar el programa al microcontolador utilizamos el puerto serie . Si no tienes un compilador adecuado puedes utilizar este fichero que ya esta preparado para ser enviado Mortal3.hex |
