#include <ESP32Servo.h>
// constants won't change
const int TRIG_PIN = 4; // Arduino pin connected to Ultrasonic Sensor's TRIG pin
const int ECHO_PIN = 5; // Arduino pin connected to Ultrasonic Sensor's ECHO pin
const int SERVO_PIN = 13; // Arduino pin connected to Servo Motor's pin
const int DISTANCE_THRESHOLD = 50; // centimeters
const int SERVO_MIN_ANGLE = 0; // Minimum angle for servo
const int SERVO_MAX_ANGLE = 180; // Maximum angle for servo
const int SERVO_SPEED = 5; // Speed of servo movement
Servo servo; // create servo object to control a servo
// variables will change:
float duration_us, distance_cm;
unsigned long servoMovedTime = 0;
int currentServoAngle = 0;
void setup() {
Serial.begin(115200); // initialize serial port
pinMode(TRIG_PIN, OUTPUT); // set arduino pin to output mode
pinMode(ECHO_PIN, INPUT); // set arduino pin to input mode
servo.attach(SERVO_PIN); // attaches the servo on pin 13 to the servo object
servo.write(currentServoAngle); // initialize servo position
}
void loop() {
// generate 10-microsecond pulse to TRIG pin
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
// measure duration of pulse from ECHO pin
duration_us = pulseIn(ECHO_PIN, HIGH);
// calculate the distance
distance_cm = 0.017 * duration_us;
if (distance_cm < DISTANCE_THRESHOLD) {
moveServo(180); // Rotate servo motor to 90 degrees
servoMovedTime = millis();
} else {
if (millis() - servoMovedTime >= 10000) {
moveServo(0); // Rotate servo motor to 0 degrees
}
}
// print the value to Serial Monitor
Serial.print("distance: ");
Serial.print(distance_cm);
Serial.println(" cm");
delay(500);
}
void moveServo(int targetAngle) {
if (currentServoAngle != targetAngle) {
if (currentServoAngle < targetAngle) {
for (int angle = currentServoAngle; angle <= targetAngle; angle += SERVO_SPEED) {
servo.write(angle);
delay(25); // Adjust the delay as per your servo motor speed requirement
currentServoAngle = angle;
}
} else {
for (int angle = currentServoAngle; angle >= targetAngle; angle -= SERVO_SPEED) {
servo.write(angle);
// Adjust the delay as per your servo motor speed requirement
currentServoAngle = angle;
}
}
}
}