Cómo construir un sonar de barrido con Arduino y Processing

#include <Servo.h>

const int trigPin = 10;
const int echoPin = 11;
const int servoPin = 12;

Servo miServo;

void setup() {
  Serial.begin(9600); // Iniciamos comunicación Serie
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  miServo.attach(servoPin);
}

void loop() {
  // Barrido de ida (de 15º a 165º)
  // Evitamos 0º y 180º porque el servo a veces tiembla en los extremos
  for(int i = 15; i <= 165; i++){  
    miServo.write(i);
    delay(30); // Damos tiempo al servo para llegar y estabilizarse
    
    int distancia = calcularDistancia();
    
    // Enviamos al PC: "angulo,distancia\n"
    Serial.print(i);
    Serial.print(",");
    Serial.println(distancia);
  }
  
  // Barrido de vuelta (de 165º a 15º)
  for(int i = 165; i > 15; i--){  
    miServo.write(i);
    delay(30);
    int distancia = calcularDistancia();
    
    Serial.print(i);
    Serial.print(",");
    Serial.println(distancia);
  }
}

int calcularDistancia(){ 
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  unsigned long duracion = pulseIn(echoPin, HIGH, 30000UL);

  // Sin eco dentro del tiempo máximo: fuera del rango visual
  if (duracion == 0) return 41;

  int distancia = duracion * 0.0343 / 2;
  
  // Limitamos la distancia máxima a 40cm para que se vea bien en pantalla
  // Si mide más, lo recortamos
  if (distancia > 40) distancia = 41;
  
  return distancia;
}

import processing.serial.*; // Importar librería Serie
import java.awt.event.KeyEvent; 
import java.io.IOException;

Serial myPort; 

String angle="";
String distance="";
String data="";

int iAngle, iDistance;
int index1=0;

void setup() {
  size(1200, 700); // Tamaño de la ventana
  smooth();
  // Busca tu puerto Arduino (normalmente es el primero o segundo de la lista)
  // Cambia el [0] si no te conecta
  myPort = new Serial(this, Serial.list()[0], 9600); 
  myPort.bufferUntil('\n'); // Una muestra por línea
}

void draw() {
  fill(0,4); // Relleno negro semitransparente para crear el efecto de desvanecimiento (trail)
  rect(0, 0, width, height);
  
  fill(98,245,31); // Color Verde Radar

  // Trasladamos el origen (0,0) al centro-abajo de la pantalla
  translate(width/2, height); 
  
  // --- DIBUJAR LÍNEAS DEL RADAR (ESTÉTICA) ---
  strokeWeight(2);
  stroke(98,245,31); // Verde
  noFill();
  // Arcos de distancia (10cm, 20cm, 30cm, 40cm)
  // Multiplicamos por ratio para escalar a la pantalla
  float pixelsPorCm = 15; 
  arc(0,0, 20*pixelsPorCm, 20*pixelsPorCm, PI, TWO_PI);
  arc(0,0, 40*pixelsPorCm, 40*pixelsPorCm, PI, TWO_PI);
  arc(0,0, 60*pixelsPorCm, 60*pixelsPorCm, PI, TWO_PI);
  arc(0,0, 80*pixelsPorCm, 80*pixelsPorCm, PI, TWO_PI);
  
  // --- DIBUJAR EL OBJETO DETECTADO ---
  // Convertimos coordenadas Polares a Cartesianas
  float pixDistance = iDistance * pixelsPorCm;
  
  // OJO: Processing usa radianes. Y el servo va de 15 a 165.
  // También invertimos el sentido porque Processing dibuja Y hacia abajo por defecto
  // pero hemos hecho translate/rotate. Simplifiquémoslo:
  
  float rads = radians(iAngle);
  
  // Si la distancia es menor de 40 (rango máximo), pintamos objeto
  if(iDistance < 40){
    // Matemáticas polares:
    // x = r * cos(theta)
    // y = r * sin(theta) -> Negativo porque en Processing Y crece hacia abajo
    float x = pixDistance * cos(rads);
    float y = -pixDistance * sin(rads); 
    
    strokeWeight(9);
    stroke(255,10,10); // Rojo para el objeto
    point(x, y);
  }
  
  // Línea de barrido (Scanner)
  strokeWeight(3);
  stroke(30,250,60);
  line(0,0, 40*pixelsPorCm * cos(rads), -40*pixelsPorCm * sin(rads));
}

// Evento que se ejecuta cuando llega una línea por USB
void serialEvent (Serial myPort) { 
  try {
    String linea = myPort.readStringUntil('\n');
    if (linea == null) return;
    data = trim(linea);
    
    // Buscamos la coma
    index1 = data.indexOf(",");
    
    // Partimos el string
    angle = data.substring(0, index1);
    distance = data.substring(index1 + 1, data.length());
    
    // Convertimos a enteros
    iAngle = int(angle);
    iDistance = int(distance);
    
    // Corregir la orientación del servo para que coincida con la pantalla
    // (Depende de cómo hayas montado el motor)
    iAngle = 180 - iAngle; 
    
  } catch (Exception e) {
    // Si llega basura por el puerto, la ignoramos
  }
}