print("PARKING SENSOR")
print("By: Alif")
print("Date: 22/4/2024")

from machine import Pin, PWM, Timer
import ultrasonic_library
from utime import sleep

# Pin declaration
TRIG = Pin(25)
ECHO = Pin(33)
SERVO_PIN = 13  # Assuming servo is connected to pin 13
MOTION_DISTANCE = 100  # 100 cm

# Create object for sensor
sensor = ultrasonic_library.HCSR04(trigger_pin=TRIG, echo_pin=ECHO, echo_timeout_us=500*2*30)

# Initialize servo
servo = PWM(Pin(SERVO_PIN), freq=50)
servo.duty(0)  # Start with the servo in locked position (0 degrees)

# Flag to track motion detection
motion_detected = False

# Timer object
tim = Timer(-1)

# Function to check for motion
def detect_motion(distance_threshold):
    # Measure distance
    distance_cm = sensor.distance_cm()

    # Check if distance is within threshold range
    return distance_threshold - 5 <= distance_cm <= distance_threshold + 5

# Callback function to return servo to origin position
def return_to_origin(timer):
    servo.duty(0)  # Lock position (angle set to 0 degrees)

# Main program
while True:
    if detect_motion(MOTION_DISTANCE):
        # Motion detected
        if not motion_detected:
            servo.duty(90)  # Set angle to 90 degrees
            motion_detected = True
            # Start timer for returning to origin position
            tim.init(mode=Timer.ONE_SHOT, period=5000, callback=return_to_origin)
    else:
        # No motion detected
        motion_detected = False
        servo.duty(0)  # Return servo to origin position

    # Add a small delay before next measurement
    sleep(0.1)  # Adjust the delay as needed to control the frequency of distance measurements