#include <Servo.h>
#include <Wire.h>

Servo myServo;  // Crear un objeto Servo para controlar el servo
int trig = 8;
int echo = 7;
long dist;
char msg[30];

long leersensor(int triggerPin, int echoPin)
{
  long duration;
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  
  return (duration* 0.01715);
  
}

void setup() {
  Serial.begin(9600);
  myServo.attach(9);  // Adjuntar el servo al pin 9 del Arduino
  pinMode(trig, OUTPUT);  // Configurar el pin Trigger como salida
  pinMode(echo, INPUT);  
}

void loop() {
  dist = leersensor(trig, echo);

  sprintf(msg,"Distancia: %ld cm\n",dist);
  Serial.println(msg);

  myServo.write(0);

  if(dist>5 && dist < 10){
    Serial.println("Giro");
    myServo.write(90);
    delay(1000);  // Esperar 1 segundo
    if(dist>5 && dist < 10){
      Serial.println("Giro");
      myServo.write(180);
    }
  }
}