from machine import Pin, PWM, time_pulse_us
import time

# Variables
TRIG = 5
ECHO = 19
SERVO = 18
FREQ = 50
SOUND = 0.0343
INTERVAL = 500 # 500 ms
OPEN = 90
CLOSED = 0
LIMIT = 20

# Ultrasonic Sensor
trig = Pin(TRIG, Pin.OUT)      
echo = Pin(ECHO, Pin.IN)

# Servo
servo = PWM(Pin(SERVO), freq=FREQ)

door_status = "CLOSED"
DEBUG = False

def log(state, msg):
    if DEBUG:
        print("[{}] [{} ms] [{}]".format(state, time.ticks_ms(), msg))

log("START", "Starting system")
def get_distance():
    trig.off()
    time.sleep_us(2)

    trig.on()
    time.sleep_us(10)

    trig.off()

    duration = time_pulse_us(echo, 1)

    if duration < 0:
        print("Error")

    distance = (duration * SOUND) / 2

    log("HC-SR04", f"d = {distance}")

    return distance

def set_angle(angle):
    duty = int(((2 * angle / 180) + 0.5) / 20 * 1023)
    servo.duty(duty)


last = 0
while True:
    now = time.ticks_ms()

    if time.ticks_diff(now, last) > INTERVAL:
        distance = get_distance()

        if distance < LIMIT:
            if door_status == "CLOSED":
                set_angle(OPEN)
            door_status = "OPEN"
            log("Servo", f"Door Status: {door_status}")

        else:
            if door_status == "OPEN":
                set_angle(CLOSED)

            door_status = "CLOSED"
            log("Servo", f"Door Status: {door_status}")

        log("Time check", f"ΔT = {now - last}")

        last = now # RESET