#include <Servo.h>
#include <Timer.h>

#define trig_pin 11//set trigpin
#define echo_pin 10//set echopin

Servo claw_servo;// declare servo name type servo
int duration, distance, previous_distance;//declare variable for unltrasonic sensor

bool isInOperation = false;

Timer timer;

void setup()
{
  timer.update();
  Serial.begin(9600);
  pinMode(trig_pin, OUTPUT);
  pinMode(echo_pin, INPUT);

  timer.every(1000, updatePreviousDistance);

  claw_servo.attach(2);// attaching servo
  claw_servo.writeMicroseconds(1500);
}
void loop()
{
  timer.update();
  digitalWrite(trig_pin, HIGH);
  _delay_ms(500);
  digitalWrite(trig_pin, LOW);

  duration = pulseIn(echo_pin, HIGH);
  distance = (duration / 2) / 29.1;
  Serial.print("cm"); //print distance unit cm
  Serial.println(distance);//distance

  if (distance > -1 && distance <= 10 && previous_distance > distance && isInOperation == false)
  {
    claw_servo.write(5);
    isInOperation = true;

    timer.after(3000, closeClaw);
  }
}

void updatePreviousDistance()
{
  previous_distance = distance;
}

void closeClaw()
{
  claw_servo.write(175);
  timer.after(5000, setOperationFalse);
}

void setOperationFalse()
{
  isInOperation = false;
}