#include #define TRIG_PIN D2 #define ECHO_PIN D3 #define SERVO_PIN D4 Servo gripperServo; // Adjust these after testing your mechanism const int OPEN_ANGLE = 20; const int CLOSED_ANGLE = 100; // Distance thresholds in centimetres const float CLOSE_DISTANCE = 15.0; const float RELEASE_DISTANCE = 18.0; bool gripperClosed = false; int currentAngle = OPEN_ANGLE; float getDistance() { digitalWrite(TRIG_PIN, LOW); delayMicroseconds(3); digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10); digitalWrite(TRIG_PIN, LOW); unsigned long duration = pulseIn(ECHO_PIN, HIGH, 30000); if (duration == 0) { return -1; // No valid echo } return duration * 0.0343 / 2.0; } void moveGripper(int targetAngle) { if (targetAngle > currentAngle) { for (int angle = currentAngle; angle <= targetAngle; angle++) { gripperServo.write(angle); delay(20); } } else { for (int angle = currentAngle; angle >= targetAngle; angle--) { gripperServo.write(angle); delay(20); } } currentAngle = targetAngle; } void setup() { Serial.begin(115200); pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT); gripperServo.attach(SERVO_PIN); gripperServo.write(OPEN_ANGLE); currentAngle = OPEN_ANGLE; Serial.println("Robotic Gripper Started"); } void loop() { float distance = getDistance(); if (distance > 0) { Serial.print("Distance: "); Serial.print(distance); Serial.println(" cm"); // Object detected within 15 cm if (distance <= CLOSE_DISTANCE && !gripperClosed) { Serial.println("Object detected - Closing gripper"); moveGripper(CLOSED_ANGLE); gripperClosed = true; } // Object moved beyond 18 cm else if (distance >= RELEASE_DISTANCE && gripperClosed) { Serial.println("Object moved away - Opening gripper"); moveGripper(OPEN_ANGLE); gripperClosed = false; } } // Invalid readings do not change the current grip state. delay(100); }