lunes, 9 de octubre de 2017

Arduino: Robot Esquiva Obstaculos | Sensor de Distancia Ultrasonido HC-SR04


En este vídeo se muestran dos ejemplos de un robot esquiva obstáculos con Arduino implementando el sensor de distancia HC-SR04


/*
----------------------------------
Conectemos Ideas
----------------------------------

Robot chasis 4WD /Arduino/Sensor HCSR04/Servo

*/

#include <Servo.h> // Libreria de Arduino para usar el servo-motor
Servo Servo1;      //Crear un nuevo Objeto tipo "Servo" llamado Servo1

#include <NewPing.h>     // Libreria de Arduino para usar el Sensor HCSR04/ link http://playground.arduino.cc/Code/NewPing#Download
#define TRIGGER_PIN  7   //Trigger Del Sensor Ultrasonido Conectado Al Pin 7 del arduino Uno
#define ECHO_PIN     10  //Echo Del Sensor Ultrasonido Conectado Al Pin 10 del arduino Uno
#define MAX_DISTANCE 100 //Maxima Disctancia que establecemos al sensor (100 Cm en este caso)

NewPing HCSR04(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); 

unsigned int uS = HCSR04.ping_cm(); //Conversion de medida sensor a Cm.

/*
/////////////////////
Entradas modulo L298N
/////////////////////
*/

//////////////////////////////////////////Adelante////////////////////////////////////////////////////
int IN1 = 3; //Entrada In1 de Modulo L298n conectada al pin 3 del arduino (motores izquierdos Adelante)    
int IN4 = 9; //Entrada In4 de Modulo L298n conectada al pin 9 del arduino (motores derechos Adelante)
///////////////////////////////////////////Atras/////////////////////////////////////////////////////
int IN2 = 5; //Entrada In2 de Modulo L298n conectada al pin 5 del arduino (motores izquierdos Atras)     
int IN3 = 6; //Entrada In3 de Modulo L298n conectada al pin 6 del arduino (motores izquierdos Atras)

///////////////////////////////Distancias////////////////////////////
const int Minima = 15;
const int Deseada=22;
const int Maxima=30;



//////////////////////////////Posision Servo/////////////////////////
const int Servo1_Centro=105;
const int Servo1_Derecha=0;
const int Servo1_Izquierda=175;

int Distancia_Servo_Derecha=0;
int Distancia_Servo_Centro=0;
int Distancia_Servo_Izquierda=0;

//////////////////////////////////////////////////////////////////////

void setup()
{
 Servo1.attach(11);

  pinMode(IN1, OUTPUT); // Pin 3 arduino = Salida
  pinMode(IN2, OUTPUT); // Pin 5 arduino = Salida
  pinMode(IN3, OUTPUT); // Pin 6 arduino = Salida
  pinMode(IN4, OUTPUT); // Pin 9 arduino = Salida
}

void loop()
{

    DatosHCSR04();
    if ((Distancia_Servo_Centro == 0) || ( Distancia_Servo_Centro > Maxima))
    {
     Adelante();
     delay(50);
     
    }
   if ( (Distancia_Servo_Centro <= Maxima) && ( Distancia_Servo_Centro >= Deseada))
    {
     Adelante();
     delay(50);
   } 
    
    else
   {
      if ( (Distancia_Servo_Centro <= Deseada) && ( Distancia_Servo_Centro >= Minima))
      {        
        Analizar();
        Decidir();
      } 
      else 
      {
        if ( (Distancia_Servo_Centro < Minima) && (Distancia_Servo_Centro !=0))
       {
         Stop();
          delay(100);
          Atras();
          delay(500);
          Izquierda();
          delay(1400);
          Analizar();
          Decidir_Giro_Atras();
      }
    }
   }
}

/*
//////////////////////////////////////////////////////
DatosHCSR04 con el servo en posición de (Servo1_Centro)
//////////////////////////////////////////////////////
*/

void DatosHCSR04(void)
{   
   Servo1.write(Servo1_Centro);
   delay(100);
   Distancia_Servo_Centro=HCSR04.ping_cm();
   
}

/*
///////////////////////////////////////////////////////////////////
Gira el servo para analizar la distancia ¡derecha,centro,izquierda! 
///////////////////////////////////////////////////////////////////
*/

void Analizar(void)
{
   Stop();
   delay(500);
    
   Servo1.write(Servo1_Derecha);
   delay(500);
   Distancia_Servo_Derecha=HCSR04.ping_cm();

   Servo1.write(Servo1_Centro);
   delay(500);
   
   Servo1.write(Servo1_Izquierda);
   delay(500);
   Distancia_Servo_Izquierda=HCSR04.ping_cm();

   Servo1.write(Servo1_Centro);
   delay(500);
   Distancia_Servo_Centro=HCSR04.ping_cm();

}

/*
//////////////////////////
Tomar la decisión correcta 
//////////////////////////
*/

void Decidir(void)
{
  if(Distancia_Servo_Derecha>Distancia_Servo_Izquierda)
  {
    Derecha();
    delay(2000);
  }

 else
  {
   if(Distancia_Servo_Izquierda>Distancia_Servo_Derecha)
    {
    Izquierda();
    delay(2000);
    }
  }
}

void Decidir_Giro_Atras(void)
{
  if(Distancia_Servo_Derecha>Distancia_Servo_Izquierda)
  {
    Atras_Derecha();
    delay(2500);
  }

 else
  {
   if(Distancia_Servo_Izquierda>Distancia_Servo_Derecha)
    {
    Atras_Izquierda();
    delay(2500);
    }
  }
}

/*
////////////////////////////////////////////////////
Creamos los movimientos respectivos de nuestro carro
////////////////////////////////////////////////////
*/

void Stop(void)
{
     analogWrite(IN1, 0);     
     analogWrite(IN2, 0); 
     analogWrite(IN3, 0);    
     analogWrite(IN4, 0);
}

void Adelante(void)
{
      analogWrite(IN2, 0);     
      analogWrite(IN3, 0); 
      analogWrite(IN1, 255); // Puedes colocar directamente el valor PWM
      analogWrite(IN4, 255); // Puedes colocar directamente el valor PWM
}

void Atras(void)
{
       analogWrite(IN1, 0);    
       analogWrite(IN4, 0);
       analogWrite(IN2, 255);  
       analogWrite(IN3, 255); 
}

void Derecha(void)
{
       analogWrite(IN1, 0);     
       analogWrite(IN3, 0);
       analogWrite(IN2,255);
       analogWrite(IN4,255); 
}

void Izquierda(void)
{
      analogWrite(IN2, 0);     
      analogWrite(IN4, 0); 
      analogWrite(IN1,255);  
      analogWrite(IN3,255); 
}


void Atras_Izquierda(void)
{
       analogWrite(IN1, 0);    
       analogWrite(IN3, 0);
       analogWrite(IN2, 255);  
       analogWrite(IN4, 0); 
}


void Atras_Derecha(void)
{
       analogWrite(IN1, 0);    
       analogWrite(IN2, 0);
       analogWrite(IN4, 0);  
       analogWrite(IN3, 255); 
}