// ETEC Aristóteles Ferreira.
// Lucas Nascimento 3P1.
  
//  -----------------------------------------------------  //

#include <Servo.h>

// Criação dos objetos para os servos.

Servo servo1;
Servo servo2;

// Definição dos pinos dos sensores ultrassônicos e dos servos.

int trigPin1 = 5;  // Pino Trigger do sensor 1.
int echoPin1 = 4;  // Pino Echo do sensor 1.

int trigPin2 = 11;  // Pino Trigger do sensor 2.
int echoPin2 = 12;  // Pino Echo do sensor 2.

void setup() {

  // Inicializa os pinos dos servos.

  servo1.attach(9); // Pino para o servo 1.
  servo2.attach(6); // Pino para o servo 2.
  
  // Inicializa os pinos dos sensores ultrassônicos.

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  
}

void loop() {

  // Leitura do sensor 1.

  int duration1;
  int distance1;
  
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  
  duration1 = pulseIn(echoPin1, HIGH);
  distance1 = duration1 / 58; // Cálculo da distância em cm.

  // Controle do servo 1.

  if (distance1 <= 3) {
    servo1.write(180);
  } else if (distance1 > 10) {
    servo1.write(0);
  } else {
    int servoPosition = map(distance1, 3, 10, 180, 0);
    servo1.write(servoPosition);
  }
  
  // Leitura do sensor 2.

  int duration2;
  int distance2;
  
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = duration2 / 58; // Cálculo da distância em cm.

  // Controle do servo 2.

  if (distance2 <= 3) {
    servo2.write(0);
  } else if (distance2 > 10) {
    servo2.write(180);
  } else {
    int servoPosition = map(distance2, 3, 10, 0, 180);
    servo2.write(servoPosition);
  }

  delay(100); 
  
  // Aguarda um pequeno intervalo antes da próxima leitura.

}

//  -----------------------------------------------------  //