#include <Servo.h>

//Initializes the variables.
Servo servo;
int trigPin = 6;
int echoPin = 5;
long duration;
int distance;
int pos;

//Sets up servo motor and ultrasonic sensor. Assigns a position to the servo.
void setup() 
{
  pos = 90;
  servo.attach(3);
  servo.write(pos);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
}

void loop() 
{
//Resets and reads from ultrasonic sensor.
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
//Values from the sensor are used to calculate the distance.
duration = pulseIn(echoPin, HIGH);
distance= duration*0.034/2;
delay(10);
//Prints the distance from the sensor and position of the servo motor. (Mainly for testing)
// Serial.print("Distance: ");
// Serial.println(distance);
// Serial.print("POS: ");
// Serial.println(pos);

//Moves the servo motor to open and closed position (0 and 90) if distance is less than or equal to 25.
//Changed original code to include a slight delay and allowed for smoother movement.
if ( distance <= 25  ) {
  for (int i = pos; pos > 0; pos -= 1)
  {
    servo.write(pos);
    delay(20);
  }
  delay(5000);
 } 
else {
  for (int i = pos; pos <= 90; pos += 1)
  {
    servo.write(pos);
    delay(20);
  }
 }
}