#include <Servo.h>

Servo servo1;
Servo servo2;

const int trig = 9;
const int echo = 10;
const int servo1Pin = 4;
const int servo2Pin = 2;

void setup() {
  Serial.begin(115200);
  
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  
  servo1.write(0);
  servo2.write(0);
}

void loop (){
  long duration, distance;

  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  duration = pulseIn(echo, HIGH);

  distance = duration * 0.034 / 2;

  int angulo1 = map(distance, 12, 15, 0, 90);

  int angulo2 = map(distance, 17, 23, 0, 90);

  servo1.write(angulo1);
  servo2.write(angulo2);

//VV  trava-limite dos servos VV\\

  if (distance >= 23) {

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



  Serial.print("distancia: ");
  Serial.print(distance);
  Serial.println(" cm");

  delay(500);
}