Se trata de un sencillo prototipo de robot que se desplaza sorteando obstáculos.
Para ello
comienza analizando el espacio libre que posee delante. Si este espacio
es suficiente decide avanzar. Si por el contrario detecta obstáculo, el
servo del sensor gira a derecha e izquierda para analizar las opciones y
decidir cual es el camino más adecuado.
Una vez
toma la decisión gira hacia su opción, se asegura que la elegida es la
correcta y comienza a avanzar hasta el siguiente obstáculo.
Para medir
las distancias y saber si dispone de espacio libre se emplea el sensor
de distancias por ultrasonidos SRF05 pero funcionando en modo 1, es
decir, con las señales de inicio de la medida y retorno de eco por pines
indepenientes, tal como lo haría el SRF04. Cuando quiere detectar la
distancia al objeto, el pic manda un pulso no inferior a 10uSeg. al
sensor y este responde con un tren de impulsos ultrasónicos que
"rebotarán" en el obstáculo, y por otro lado pasa la señal de retorno de
eco a estado alto hasta recibir el tren ultrasonico reflejado. Cuanto
menor sea la distancia menor en tiempo será este pulso.
Para
interpretar esta señal de retorno el pic emplea en este proyecto el
registro CCP que mide el valor del pulso recibido a partir del valor del
contador interno activo durante el tiempo de duración de la señal. El
dato final del registro gestionado mediante esta fórmula; d = CCP*1.0
/58 , dará la distancia en cm.
La
orientación del servo se establece con el valor que se carga en la
variable pwm. Con cada llamada a la subrutina de generación de la señal
por mediación de la interrupción por rebose del timer 0, se incrementa
la variable pulso_pwm que comparandose con el valor en pwm, decide en
qué momento debe cortar el pulso.
Este es el programa del PIC para el compilador de CCS.
//////////////////////////////////////////////////////////////////////////////// // // // ROBOT // // // // Robot avanza sin chocar con obstáculos // // // // (c) RobotyPic 2011 // // // //////////////////////////////////////////////////////////////////////////////// #include <16F876A.h> #FUSES NOWDT #fuses XT //Oscilador por cristal entre 4Mhz y 10Mhz #fuses NOPROTECT #use delay(clock=4000000) //Frecuencia del cristal oscilador 4MHz #priority rtcc #byte PIR1=0x0C #byte trisa=0x85 #byte porta=0x05 #byte trisb=0x86 #byte portb=0x06 #byte trisc=0x87 #byte portc=0x07 #define BIT_TEST_ON output_high(PIN_C0) #define BIT_TEST_OFF output_low(PIN_C0) #define AVANCE output_high(PIN_C2),output_high(PIN_C4),output_low(PIN_C3),output_low(PIN_C5) #define RETROCESO output_low(PIN_C2),output_low(PIN_C4),output_high(PIN_C3),output_high(PIN_C5) #define PARO output_low(PIN_C2),output_low(PIN_C4),output_low(PIN_C3),output_low(PIN_C5) #define GIRO_DERECHA output_high(PIN_C2),output_low(PIN_C4),output_low(PIN_C3),output_high(PIN_C5) #define GIRO_IZQUIERDA output_low(PIN_C2),output_high(PIN_C4),output_high(PIN_C3),output_low(PIN_C5) #bit Bit_PWM = PORTB.7 //Bit 7 puerto B Salida modulación 1 /********************** Prototipos de las funciones ***************************/ void main (void); //función principal void Generacion_pwm (void); //genera señales moduladas para control de servos void pulso_test (void); //da pulso y calcula distancia al obstáculo void ccp2_int (void); //mide pulso de eco ultrasonidos void navegacion (void); //avance sorteando obstáculos /********************** Variables para generación PWM *************************/ int8 PWM=0; //Guardará los valores de la señal PWM int8 Pulso_pwm=0; //Ancho del pulso de control del servo /******************** Variables para lectura obstáculo ************************/ int1 nuevopulso=0; //Entra otro pulso de lectura int16 TFB=0,TFS=0,TF=0; //Tiempo flancos float AP=0.0; //Valor del pulso del obstáculo en microsegundos int16 distancia=0; //Valor distancia al obstáculo int16 distancia_derecha=0; int16 distancia_izquierda=0; int1 cambio=0; //Control flanco subida o bajada del impulso leido /******************************************************************************/ /******************* FUNCIÓN GENERACIÓN MODULACIONES PWM **********************/ #int_Timer0 void Generacion_pwm() { Pulso_pwm++; //Incremento cada rebose del timer0 if (Pulso_pwm==0) { Bit_PWM=1; } if (Pulso_pwm==PWM){ Bit_PWM=0; } delay_us(5); set_timer0(255); } /******************************************************************************/ /*********************** FUNCIÓN PULSO DE TEST ********************************/ void pulso_test() { enable_interrupts(INT_CCP2); //Habilitación interrup. medida de pulso eco BIT_TEST_ON; //Pulso test durante 10us por Bit 0 puerto C delay_us(10); BIT_TEST_OFF; while(nuevopulso==0) //Espera a finalizar el pulso eco {} if(nuevopulso==1) { //Finalizado el pulso eco se calcula su valor. TF=(TFB-TFS); //Valor entre pulso de subida y bajada. AP=TF*1.0; //Valor pulso leido en us(obstáculo) de 100u a 25ms AP = AP/58; //Distancia del obstáculo en cm distancia =(int16)AP; //paso de flotante a entero largo nuevopulso=0; //Listo para recibir nuevo pulso } disable_interrupts(INT_CCP2); } /******************************************************************************/ /************************ FUNCIÓN LECTURA OBSTÁCULO ***************************/ #int_ccp2 //LLamada por interrupción flanco en RC2 void ccp2_int() { if(cambio==0) //Si es flanco de subida... { TFS=CCP_2; //Carga en valor flanco subida valor registro ccpr1 setup_ccp2(CCP_CAPTURE_FE); //Configuración captura en flanco de bajada cambio=1; //Próximo flanco debe ser de bajada } else { //Si es flanco de bajada... TFB=CCP_2; //Carga valor flanco bajada valor registro ccpr1 setup_ccp2(CCP_CAPTURE_RE); //Configuración captura en flanco de subida cambio=0; //Próximo flanco debe ser de subida if(nuevopulso==0) //Fin de pulso... nuevopulso=1; //Pulso finalizado. } } /******************************************************************************/ /********************* FUNCIÓN AVANCE - DETECCIÓN OBSTÁCULO *******************/ void navegacion() { pulso_test(); if (distancia>30) { //Si distancia >30cm avanza AVANCE; } else { PARO; pwm=8; delay_ms(700); pulso_test(); distancia_derecha=distancia; pwm=25; delay_ms(700); pulso_test(); distancia_izquierda=distancia; pwm=16; if (distancia_derecha>distancia_izquierda) { GIRO_DERECHA; delay_ms(500); } else { GIRO_IZQUIERDA; delay_ms(500); } } } /******************************************************************************/ /*************************** FUNCIÓN PRINCIPAL ********************************/ void main() { trisb=0x00; //Puerto B todo salidas trisc=0b00000100; //Puerto C definición de entradas y salidas pwm=16; //Impulso de 1,5 msg de pwm posición 0º PARO; setup_timer_0(RTCC_INTERNAL|RTCC_DIV_32); //Config.interrupción generación PWM setup_timer_1(T1_INTERNAL); //Config.timer1 lectura obstáculo setup_ccp2(CCP_CAPTURE_RE); //Configuración captura en flanco subida enable_interrupts(INT_TIMER0); //Habilitación interrupción generación pwm disable_interrupts(INT_TIMER1); disable_interrupts(INT_CCP2); //Deshabilitación interrup.modo comparación enable_interrupts (GLOBAL); delay_ms(500); //Estabilización en el arranque del sistema while (1) { delay_ms(500); //Tiempo entre impulsos test navegacion(); //Salto a la función navegacion } }
Hay que
tener en cuenta que para realizar la rotación a 90º del robot, no se
emplean encoder de giro sino que se plantea un tiempo de funcionamiento
de las ruedas en contradirección. Por ello, dependiendo de la señal
clock del PIC, diámetro de las ruedas, cantidad de código del programa,
etc.; puede ser necesario cambiar estos tiempos para obtener el giro
deseado del robot.
A continuación detallo
cuales son esos tiempos a modificar para conseguir un giro mayor o
menor del robot y un tiempo mayor o menor de la cabeza del sensor
girada:
pwm=8;
delay_ms(700); //Cuanto mayor es este valor mayor tiempo permanecerá la cabeza del robot girada a la derecha
pulso_test();
distancia_derecha=distancia;
pwm=25;
delay_ms(700); //Cuanto mayor es este valor mayor tiempo permanecerá la cabeza del robot girada a la izquierda
pulso_test();
distancia_izquierda=distancia;
pwm=16;
if (distancia_derecha>distancia_izquierda) {
GIRO_DERECHA;
delay_ms(500); //Cuanto mayor es este valor mayor será el ángulo de giro del robot a la derecha
}
else {
GIRO_IZQUIERDA;
delay_ms(500); //Cuanto mayor es este valor mayor será el ángulo de giro del robot a la izquierda
}
}
}
En el
siguiente enlace se pueden descargar los archivos del código fuente para
CCS, el archivo compilado para cargar en el PIC y el esquema para
Proteus.
