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:
Construyendo a Chispas 