const int servoPin = 6;    // Pin al que está conectado el servo
const int servoMin = 0;    // Ángulo mínimo (0 grados)
const int servoMax = 180;  // Ángulo máximo (180 grados)
int anguloActual = 0;      // Ángulo inicial del servo 0 grados
int contBarridos = 0;
int echoPin = 4;
int trigPin = 5;
int relay = 8;
long duracion;
int distancia;
int cuadrante;
long matriz[10][4];
int aux = 0;
int time=0;

void setup() {
  pinMode(echoPin, INPUT_PULLUP);
  pinMode(trigPin, OUTPUT);
  pinMode(relay, OUTPUT);
  pinMode(servoPin, OUTPUT);    // Configura el pin del servo como salida
  digitalWrite(servoPin, LOW);  // Inicializa el servo en la posición 0 grados
  Serial.begin(9600);           // Inicia la comunicación serial
}

void loop() {
  while (contBarridos < 10) {
    while (anguloActual < 180) {
      moverServo();
      distanciaSensor();
      if (distancia > 0 && distancia < 15) {
        delayMicroseconds(50);
        cuadrante = calcCuadrante(anguloActual);
        matriz[contBarridos][0] = distancia;
        matriz[contBarridos][1] = anguloActual;
        matriz[contBarridos][2] = cuadrante;
        matriz[contBarridos][3]= millis()/1000;
      }
      anguloActual++;
    }
    anguloActual = 180;
    contBarridos++;
    while (anguloActual > 0) {
      moverServo();
      distanciaSensor();
      if (distancia > 0 && distancia < 15) {
        delayMicroseconds(50);
        calcCuadrante(anguloActual);
        matriz[contBarridos][0] = distancia;
        matriz[contBarridos][1] = anguloActual;
        matriz[contBarridos][2] = cuadrante;
        matriz[contBarridos][3]= millis()/1000;
      }
      anguloActual--;
    }
    anguloActual = 0;
    contBarridos++;
  }
  while (aux < 1) {
    Serial.println("Dist  Ang Cua   T(s)");
    for (int i = 0; i < 10; i++) {
      Serial.print(matriz[i][0]);
      Serial.print("    ");
      Serial.print(matriz[i][1]);
      Serial.print("    ");
      Serial.print(matriz[i][2]);
      Serial.print("    ");
      Serial.println(matriz[i][3]);
      aux++;
    }
  }
}

void moverServo() {
  digitalWrite(servoPin, HIGH);
  delayMicroseconds(500 + (anguloActual * 11));
  digitalWrite(servoPin, LOW);
  delay(15);
}

int distanciaSensor() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duracion = pulseIn(echoPin, HIGH);
  distancia = (duracion * 0.034) / 2;
  return distancia;
}

int calcCuadrante(int angulo) {
  if (angulo >= 0 && angulo <= 90) {
    cuadrante = 1;
    return cuadrante;
  } else if (angulo > 90 && angulo <= 180) {
    cuadrante = 2;
    return cuadrante;
  }
}