#include <Servo.h>
#define PIN_TRIG 5
#define PIN_ECHO 4
#define PIN_TRIG2 3
#define PIN_ECHO2 2
Servo servo1;
Servo servo2;

void setup() {
  Serial.begin(9600);
  servo1.attach(11);
  servo2.attach(10);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
   pinMode(PIN_TRIG2, OUTPUT);
  pinMode(PIN_ECHO2, INPUT);
}

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

  // Leia o resultado:
  int duration = pulseIn(PIN_ECHO, HIGH);
  int duration2 = pulseIn(PIN_ECHO2, HIGH);

  int CM =(duration / 58);
  int CM2 =(duration2 / 58);

 int costas= map(CM,0,3,0,180);
 int ombro= map(CM2,0,3,0,180);

  Serial.print("Distância em CM: ");
  Serial.println(duration / 58);
  if(costas>9)
  {
   servo1.write(HIGH);
  }
  if(costas<=3)
{
  servo1.write(LOW);
}

  delay(1000);
}