#include <Servo.h>

Servo myservo;  // Create a servo object
const int triggerPin = 2; // Trigger pin of the ultrasonic sensor
const int echoPin = 3;    // Echo pin of the ultrasonic sensor
const int gripperPin = 9; // Control pin for the gripper (servo)

long duration;
int distance;
bool objectDetected = false;

void setup() {
  myservo.attach(gripperPin); // Attach the servo to the gripper pin
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
}

void loop() {
  // Measure distance using the ultrasonic sensor
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;
  
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // If an object is detected within a certain range (adjust as needed)
  if (distance < 10) {
    objectDetected = true;
    Serial.println("Object detected!");

    // Move the servo to pick up the object
    myservo.write(90); // Adjust the angle as needed

    // Add code here to activate your gripper mechanism to pick up the object
    // This may require additional hardware and code specific to your gripper.

    delay(1000); // Wait for a moment to simulate gripping the object
    
    // After gripping the object, you can release it by reversing the servo position
    myservo.write(0); // Adjust the angle to release the object
    
    objectDetected = false;
  }
}