#include <Servo.h>

const int trigPin = 5;   // Trig pin of the ultrasonic sensor

const int echoPin = 6;   // Echo pin of the ultrasonic sensor

const int servoPin = 9;  // Pin to which the servo is connected

int currentPosition; // The current position of the servo

int delayTime = 5;

Servo myservo;           // Create a servo object

void setup() {

  myservo.attach(servoPin);  // Attaches the servo on the specified pin

  pinMode(trigPin, OUTPUT);

  pinMode(echoPin, INPUT);

  Serial.begin(115200);

  myservo.write(180);

  currentPosition = 180;

  delay(500);

}

void loop() {

  long duration, distance;

  digitalWrite(trigPin, LOW);

  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);

  distance = duration * 0.034 / 2;

  Serial.print("Distance: ");

  Serial.print(distance);

  Serial.println(" cm");

  if (distance < 15) {   // Object detected within 15cm

    openTrashcan();

    Serial.write("open");

    delay(5000);        //stays open for 5 seconds

    Serial.write("closed");

    closeTrashcan();

  }

}

void openTrashcan() {

  //moveservo(90, 1);

  moveservo(90, 2);

}

void closeTrashcan() {

  //moveservo(180, 1);

  moveservo(180, 2);

}

void moveservo(int targetPosition, int velocity) {

  if (currentPosition < targetPosition) {

    currentPosition += velocity;

    if (currentPosition > targetPosition) {

      currentPosition = targetPosition;

    }

  } else if (currentPosition > targetPosition) {

    currentPosition -= velocity;

    if (currentPosition < targetPosition) {

      currentPosition = targetPosition;

    }

  }

  // Update servo position

  myservo.write(currentPosition);

  // Add delay

  delay(delayTime);

}