#include <Servo.h>
#define PIN_TRIG 5
#define PIN_ECHO 4
Servo servo1;
Servo servo2;

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

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 CM =(duration / 58);
 int costas= map(CM,2,5,0,180);
 int peito= map(CM,7,13,0,180);
  Serial.print("Distância em CM: ");
  Serial.println(duration / 58);
   servo1.write(costas);
   servo2.write(peito); 
  
  delay(1000);
}