Saltar la navegación

Código esquiva-objetos

// Incluimos la librería de los motores
#include <ControlMotor.h>
// Configuramos los pines que vamos a usar
ControlMotor control(2,3,7,4,5,6); // MotorDer1,MotorDer2,MotorIzq1,MotorIzq2,PWM_Derecho,PWM_Izquierdo

// Agregamos la librería del ultrasonido.
// Se puede descargar de http://www.ardublog.com/library-for-arduino-ultrasonic-ranging-hc-sr04/
#include <Ultrasonic.h>
// Configuramos los pones y el delay máximo (Distancia)
Ultrasonic sensor(9,8,30000);   // (Trig PIN,Echo PIN, microsegundos max "distancia máxima") 30000us = 5 metros


// Declaramos las variables
long distancia=0;                // Almacenamos la distancia en esta variable
int sentido=1;                   // Sentido en el que va girar el robot.

// Esta función se usa para parar los motores, mas que nada para ahorrar espacio y no poner tantas veces el mismo código.
// Se llama a la función indicando el tiempo que quiere que estén los motores parados
void parada(uint16_t tiempo){
  control.Motor(0, 0);  // Para los motores
  delay(tiempo);        // Espera el tiempo que se le indique.
}

//----------------------------- Configuramos Ardunio ---------------------------------------
//------------------------------------------------------------------------------------------
void setup(void){
  sentido=1;                   // Iniciamos sentido en positivo
}
//--------------------------- Arrancamos el programa ---------------------------------------
//------------------------------------------------------------------------------------------
void loop(void)
{

  distancia=sensor.Ranging(CM);  // Guardamos la distancia en la variable
  
  //--------------------------------------------- Si hay mas de 1,5 metros -------------------------------------------
  if(distancia > 150){          
    sentido *= -1;          // Invertimos el sentido para usarlo cuando haya menos de 1 mentro (1 x -1 = -1) o (-1 x -1 = 1)
    control.Motor(255, 0);  // Avanzamos en línea recta a máxima potencia
  }
  //----------------------------------------- Si hay entre 1 metro y 1,5 metros ---------------------------------------
  else if(distancia > 100) {// Avanzamos pero no cambiamos el sentido ya en esta función
    control.Motor(255, 0);  // Avanzamos en línea recta a máxima potencia
  }
  //-------------------------------------------- Si hay entre 80 cm y 1 metro -----------------------------------------
  else if(distancia > 80) {
    control.Motor(220, 20*sentido);  // Avanzamos un poco mas lento y vamos girando 20% en el sentido que esté memorizado 
  }
  //--------------------------------------------- Si hay entre 50 cm y 80 cm ------------------------------------------
  else if(distancia > 50) {
    control.Motor(200, 60*sentido);  // Avanzamos un poco mas lento y vamos girando 60% en el sentido que esté memorizado 
  }
  //--------------------------------------------- Si hay entre 10 cm y 50 cm ------------------------------------------
  else if(distancia > 10) {
     control.Motor(150, 100*sentido);delay(300); // Giramos sobre el eje del robot durante 300 ms en el sentido que esté memorizado a una velocidad de 150
     parada(200);     // Paramos el robot unos 200ms para que le de tiempo al ultrasonido de reacciona
  }
  //----------------------------------------------- Si hay menos de 10 cm ---------------------------------------------
  else{
    control.Motor(-200, 0);delay(300);            // Damos marcha atras en línea recta a una velocidad de 200 durante 300 ms
    control.Motor(200, 100*sentido);delay(200);   // Giramos sobre el eje a una velocidad de 200 con el sentido memorizado durante 200 ms
    parada(200);     // Paramos el robot unos 200ms para que le de tiempo al ultrasonido de reacciona                               
  }
 
}