#include <Servo.h>
#include <math.h>
#define TRIG 13
#define ECHO 12
Servo servo;

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  Serial.begin(9600);
  servo.attach(7);

}

void loop() {
  

  for(int i= 0;i<180;i+=3){
    servo.write(i);
    delay(30);
    Serial.println(i);
    calculate();
  }

  for(int i= 180;i>0;i-=3){
    servo.write(i);
    delay(30);
    Serial.println(i);
    calculate();
  }
}

void calculate(){
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  delayMicroseconds(200);

  long duration = pulseIn(ECHO, HIGH);
  float distance = (duration * 0.034) / 2;

  Serial.print("Distancia en cm : ");
  Serial.println(distance);
}