from machine import Pin, PWM
import utime

trig_pin = Pin(4, Pin.OUT)
echo_pin = Pin(5, Pin.IN)
buzzer_pin = Pin(15, Pin.OUT)
pwm = PWM(buzzer_pin)

def measure_distance():
    trig_pin.high()
    utime.sleep_us(10)
    trig_pin.low()

    while echo_pin.value() == 0:
        pulse_start = utime.ticks_us()
    while echo_pin.value() == 1:
        pulse_end = utime.ticks_us()
    pulse_duration = pulse_end - pulse_start

    distance = (pulse_duration * 34300) / 2 / 1000000
    return distance, pulse_duration / 1000  # Zwraca dystans i czas w ms

def activate_buzzer():
    pwm.freq(1000)
    pwm.duty_u16(32768)  

def deactivate_buzzer():
    pwm.duty_u16(0)

def get_direction_message(distance):
    
    if distance >= 100:
        return "go straight"
    elif 70 <= distance < 100:
        return "turn left"
    elif 40 <= distance < 70:
        return "turn right"
    elif 30 <= distance < 40:
        return "go slow"
    else:
        return "reached"

while True:
    distance, time_elapsed = measure_distance()
    print("distance:", distance, "cm")
    print("time:", time_elapsed, "ms")
    
    if distance < 30:  
        activate_buzzer()
        print("you in place")
    else:
        deactivate_buzzer()
        direction_message = get_direction_message(distance)
        print("direction:", direction_message)
    
    utime.sleep(2)