Robot que evita obstáculos.

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.

[+/-] Ver / Ocultar programa completo en C

////////////////////////////////////////////////////////////////////////////////
//                                                                            //
//                                  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=0b00000010;             //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:

void navegacion()  {
   pulso_test();       
   if (distancia>30) {
      AVANCE;
      }
   else {
      PARO;
     
      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. 

Descargar robot que evita obstáculos