Crear un sonar en Arduino con un servo y un sensor de ultrasónico HC-SR04

Ya vimos como medir distancias con un sensor ultrasonico, ahora crearemos un sonar con él. Para ello lo pondremos encima de un servo que ira girando el sensor de 5 en 5 grados. Cada movimiento hará tres medidas con el sensor de la cual tomara la mediana (ya vimos que esto se hace para reducir el ruido). Esos datos serán enviados al puerto serie como «angulo, distancia». Para que luego los muestre un programa realizado en processing.

#include <Servo.h> // incluir la biblioteca para controlar el servo

Servo myservo; // declarar un objeto de tipo Servo

int trigPin = 12;  // pin para el trigger del sensor
int echoPin = 11; // pin para el echo del sensor
int angle = 0; // ángulo actual del servo

void setup() {
  Serial.begin(9600); // inicializar el puerto serie
  myservo.attach(9); // conectar el servo al pin 9
  pinMode(trigPin, OUTPUT); // configurar el pin trigger como salida
  pinMode(echoPin, INPUT); // configurar el pin echo como entrada
}

void loop() {
  for (angle = 0; angle <= 180; angle += 5) {
    myservo.write(angle); //movemos el servo
    delay(100); //para que el servo no este moviendose
    int distance = getMedianDistance(); // distancia medida por el sensor
    Serial.print(angle);
    Serial.print(", ");
    Serial.println(distance);
    delay(50);
  }
  for (angle = 180; angle >= 0; angle -= 5) {
    myservo.write(angle); //movemos el servo
    delay(100); //para que el servo no este moviendose
    int distance = getMedianDistance(); // distancia medida por el sensor
    Serial.print(angle);
    Serial.print(", ");
    Serial.println(distance);
    delay(50);
  }
}

int cmp_desc(const void *c1, const void *c2){  
  return *((int *)c2) - *((int *)c1);
}

int getMedianDistance() {
  int distances[3];
  for (int i = 0; i < 3; i++) { //3 mediciones
    long duration, distance;
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    distance = (duration / 2) / 29.1;
    distances[i] = distance;
  }

  //ordenamos
  qsort(distances, 3, sizeof(int), cmp_desc);
  
  //tomamos la mediana (el central)
  return distances[1];
}

Para dibujar el resultado usaremos el siguiente programa en processing:

import processing.serial.*;

Serial port;
boolean drawLines = true; // indica si se deben dibujar las líneas
int x0, y0; // coordenadas del punto central
float r = 100; // radio
float angle, distance; // ángulo y distancia leídos desde el puerto serie
float zoom = 2.0; //multiplicamos la distancia para mejorar la visualizacion
int radio = 50; //se usa pra dibujar los circulos del fondo del radar

void setup() {
  size(400, 400);
  x0 = width / 2;
  y0 = height - 50;
  String portName = Serial.list()[0]; // elegir el primer puerto serie disponible
  port = new Serial(this, portName, 9600); // inicializar el puerto serie
  drawBackground();
}

void draw() {
  if (port.available() > 0) {
    String data = port.readStringUntil('\n'); // leer los datos desde el puerto serie
    if (data != null) {      
      String[] values = split(data, ','); // separar los datos en grados y distancia
      if(values.length == 2){ //tiene que haber dos datos
        angle = float(values[0]) / 180 * PI; // convertir los grados a radianes
        if (angle == 0.0) { //si angulo es 0 limpiamos el radar          
          drawBackground();
        }
        distance = float(values[1])*zoom;
        float x = x0 + cos(angle) * distance; // calcular las coordenadas x,y
        float y = y0 - sin(angle) * distance;
        println(distance, angle, x, y);
        stroke(0, 255, 0);
        fill(0, 255, 0);
        ellipse(x, y, 5, 5); // dibujar un punto en las coordenadas calculadas
      }
    }
  }
}

void drawBackground(){ //dibuja el fondo con forma de radar
  background(0);
  noFill();
  radio = 50;
  for (int i = 0; i < 10; i++) {
    stroke(100, 200, 100);
    strokeWeight(2);
    ellipse(x0, y0, 2 * radio, 2 * radio);
    radio += 50;
  }
}

Vamos a centrarnos en la parte que dibuja los datos

Primero vemos si el puerto está disponible, si hay datos en el puesto y si tenemos dos valores separados por una coma:

 if (port.available() > 0) {
    String data = port.readStringUntil('\n'); // leer los datos desde el puerto serie
    if (data != null) {      
      String[] values = split(data, ','); // separar los datos en grados y distancia
      if(values.length == 2){ //tiene que haber dos datos

Convertimos el angulo a radianes:

angle = float(values[0]) / 180 * PI; // convertir los grados a radianes

Ajustamos la distancia para que se vea bien en el canvas:

distance = float(values[1])*zoom;

Finalmente teniendo el angulo y la distancia proyectamos el punto usando, nuestra por todos querida (¿verdad?), trigonometría:

 float x = x0 + cos(angle) * distance; // calcular las coordenadas x,y
 float y = y0 - sin(angle) * distance;

La variables x0 y y0 son las coordenadas del centro de nuestro radar.

El resultado tiene este aspecto:

¡Ojo! Tener en cuenta que la orientación en el radar no tiene porque coincidir con el del sensor, puedes girar el sensor y la imagen de la pantalla no se girará. Seguramente ahora parezca una tontería pero cuando los datos salen invertidos horizontalmente puede resultar confuso.

Puedes ver todo esto en este vídeo de mi canal de Youtube:

Relacionado