#define PIN_TRIG 4
#define PIN_ECHO 5

int angS1, angS2 = 0;

#include <Servo.h>                 //inicia a biblioteca servo.h
Servo servo1;                      //cria os objetos para controle dos servos
Servo servo2;

void setup() {
  Serial.begin(115200);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

servo1.attach(9);                  //define os pinos como entrada para os servos respectivos
servo2.attach(6);

servo1.write(0);
servo2.write(0);
}

void loop() {
  // Inicia uma nova medição:
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // Leia o resultado:
  int duration = pulseIn(PIN_ECHO, HIGH);
  int durationCM = duration / 58;
  Serial.print("Distância em CM: ");
  Serial.println(durationCM);
  
angS1 = map(durationCM, 20, 50, 0, 180);
angS2 = map(durationCM, 70, 130, 0, 180);

servo1.write(angS1);
if(durationCM > 50){
  servo1.write(180);
}
servo2.write(angS2);
if(durationCM > 130){
  servo2.write(180);
  delay(5);
}

}