ARDUINO: DETECCIÓN DE OBSTÁCULOS

Un detector de obstáculos ultrasónico es un dispositivo que detecta la presencia de un objeto mediante la reflexión de una onda que transmite . Este detecta la reflexión de la onda y con el código programado hacemos que gire en la mejor opción.

Los detectores de obstáculo suelen proporcionarse con una placa de medición, que permite obtener la lectura como un valor digital cuando se supera un cierto umbral, que se regula a través de un potenciómetro ubicado en la placa.

El código es:

#include <Servo.h>  //Libreria servo
Servo myservo;      // Crea el servo
/*Define los pines que se usan*/
int Echo = A4;
int Trig = A5;
#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11
/*Define dos variables de velocidad utilizada*/
#define carSpeed 200
#define carRotation 250
int rightDistance = 0, leftDistance = 0, middleDistance = 0;
void forward(){
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);

Importamos la librería servo y usamos myservo para declarar nuestro servo, definimos los pines que se van a usar y las variables constantes de velocidad que vamos a utilizar, empezamos con las funciones que va a tener el robot con forward hacia adelante.

digitalWrite(IN4, HIGH);
  Serial.println("Forward");
}
void back() {
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Back");
}
void left() {
  analogWrite(ENA, carRotation);
  analogWrite(ENB, carRotation);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("Left");
}

Seguimos con marcha atrás e izquierda.

void right() {
  analogWrite(ENA, carRotation);
  analogWrite(ENB, carRotation);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Right");
}
void stop() {
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  Serial.println("Stop!");
} 

Funciones a la derecha y de detención del arduino.

//Definde la subfunción del sensor ultrasónico
int Distance_test() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);
  float Fdistance = pulseIn(Echo, HIGH);
  Fdistance= Fdistance / 58;
  return (int)Fdistance;
}
void setup() {
  myservo.attach(3,700,2400);  // Pin 3 servo define los ángulos
  Serial.begin(9600);
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  stop();
} 

Integramos las subfunciones del sensor ultrasónico para testear la distancia y hacer una medición lo mejor posible de la distancia entre él y los obstáculos. Definimos el pin 3 para el servo y los ángulos de rotación, además de los pines con input como entrada y output como salida.

void loop() {
    myservo.write(90);  //Inicia con servo en ángulo 90º
    delay(500);
    middleDistance = Distance_test();
    if(middleDistance <= 40) {
      stop();
      delay(500);
      myservo.write(10);
      delay(500);
      rightDistance = Distance_test();
      delay(500);
      myservo.write(90);
      delay(500);
      myservo.write(180);
      delay(500);
      leftDistance = Distance_test();
   

Iniciamos el servo con un ángulo de 90 grados y vamos calculando y midiendo la distancia, añadiendo el tiempo como delay en milisegundos.

delay(500);
      myservo.write(90);
      delay(500);
      if(rightDistance > leftDistance) {
        right();
        delay(360);
      }
      else if(rightDistance < leftDistance) {
        left();
        delay(360);
      }
      else if((rightDistance <= 40) || (leftDistance <= 40)) {
        back();
        delay(360);
      }
      else {
        forward();
      }
    }
    else {
        forward();
    }
}

Si la distancia derecha es mayor que la izquierda entonces giramos hacia la derecha x distancia y así sucesivamente, sino sigue hacia adelante.

Video de demostración: Evitar obstáculos

https://youtu.be/tt5ZMn9i6v8

Este proyecto ha sido realizado basándose en el código por defecto de Smart Robot Car Kit 3.0 Plus.

Proyecto Arduino

Realizado por: Kevin Maldonado Pico y Laura Tovar Pérez