import machine
import time

# Pin Setup
TRIG_PIN = 21  # Pin connected to Trig of HC-SR04
ECHO_PIN = 22  # Pin connected to Echo of HC-SR04
SERVO_PIN = 15  # Pin connected to the signal of Servo

# Initialize Pins
trig = machine.Pin(TRIG_PIN, machine.Pin.OUT)
echo = machine.Pin(ECHO_PIN, machine.Pin.IN)
servo = machine.PWM(machine.Pin(SERVO_PIN))

# Configure servo PWM frequency
servo.freq(50)

# Function to measure distance using HC-SR04
def measure_distance():
    # Ensure the trigger pin is low
    trig.value(0)
    time.sleep_us(2)
    
    # Send a 10us pulse to trigger
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)

    # Wait for echo to go high (start of the pulse)
    while echo.value() == 0:
        pulse_start = time.ticks_us()
    
    # Wait for echo to go low (end of the pulse)
    while echo.value() == 1:
        pulse_end = time.ticks_us()

    # Calculate pulse duration and convert to distance (in cm)
    pulse_duration = pulse_end - pulse_start
    distance = (pulse_duration * 0.0343) / 2  # Speed of sound is 343 m/s

    return distance

# Function to set servo angle (0 to 180 degrees)
def set_servo_angle(angle):
    duty = int(2500 + (angle / 180.0) * 5000)  # Calculate duty cycle for the angle
    servo.duty_u16(duty)

# Main loop
try:
    while True:
        distance = measure_distance()
        print("Distance:", distance, "cm")

        # Move the servo based on the distance
        if distance < 10:
            print("Object detected within 10 cm, moving servo to 90 degrees")
            set_servo_angle(90)  # Move servo to 90 degrees if object is closer than 10cm
            time.sleep(5)  # Keep the servo at 90 degrees for 5 seconds
            print("Returning servo to 0 degrees")
            set_servo_angle(0)  # Move servo back to 0 degrees
        else:
            set_servo_angle(0)  # Keep servo at 0 degrees if object is farther than 10cm

        time.sleep(1)  # Wait for 1 second before the next measurement

except KeyboardInterrupt:
    # Reset servo before exiting
    set_servo_angle(0)
    print("Program stopped")