/*
   HC-SR04 example sketch

   https://create.arduino.cc/projecthub/Isaac100/getting-started-with-the-hc-sr04-ultrasonic-sensor-036380

   by Isaac100
*/
#include <Servo.h>
const int motorPin = 9 ;
Servo myservo;  // create servo object to control a servo

const int trigPin = 2;
const int echoPin = 4;

float duration, distance;

int pos = 0;    // variable to store the servo position


void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
  myservo.attach(6);  // attaches the servo on pin 9 to the servo object



}


void loop() {

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = (duration * .0343) / 2;
  Serial.print("Distance: ");
  Serial.println(distance);
  delay(100);
  int arrivo ;

  if (distance < 20 ) {
    arrivo = 180;
  } else {
    arrivo = 0;
  }


  for (pos = 0; pos <= arrivo; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  for (pos = arrivo; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
}