from machine import Pin, time_pulse_us, PWM
import time

# Define pins for Trig and Echo
trig_pin = Pin(12, Pin.OUT)
echo_pin = Pin(11, Pin.IN)

# Define LED output
led_green = Pin(21, Pin.OUT)
led_yellow = Pin(20, Pin.OUT)
led_red = Pin(19, Pin.OUT)

servo = PWM(Pin(15))
servo.freq(50)

# Fungsi untuk menetapkan sudut servo
def set_servo_angle(angle):
    min_duty = 25   # Nilai PWM minimum (0°)
    max_duty = 125  # Nilai PWM maksimum (180°)
    duty = min_duty + (angle / 180) * (max_duty - min_duty)
    servo.duty(int(duty))  # Tetapkan duty cycle


def measure_distance(): # function definition
    # Send a 10us pulse to trigger the sensor
    trig_pin.off() # Sets the trigger pin low. 
    time.sleep_us(2) # Very short pause to stabilize the low signal
    trig_pin.on()# Sets the trigger pin high.
    time.sleep_us(10) # Keeps the trigger pin high for 10 microseconds 
    trig_pin.off() #Sets the trigger pin low again.

    # Measure the pulse duration on the Echo pin
    pulse_duration = time_pulse_us(echo_pin, 1, 30000)  # Timeout after 30ms
    # 30000: This is a timeout value specified in microseconds. 
    # It means that time_pulse_us() will wait for a maximum of 30,000 microseconds (30 milliseconds) for the rising edge on the echo_pin.  If 
    #no pulse is detected within this time, the function will return -1. 


    # Calculate distance in centimeters
    distance_cm = (pulse_duration / 2) / 29.1  # Speed of sound is 343 m/s or 29.1 cm/us
    return distance_cm

while True:
    distance = measure_distance()
    print("Distance:", distance, "cm")

    #coding for trffic light
    #if 101 < distance < 200, yellow LED is ON, others are OFF
    #if 0 < distance < 100, red LED is ON, others are OFF
    #if distance > 201, green LED is ON, others are OFF
    if (distance < 100):
        set_servo_angle(30)
        led_yellow.value(1)
        led_green.value(0)
        led_red.value(0)
        time.sleep(2)  # Wait for 1 second before the next measurement
    if (distance < 200) and (distance > 100):
        set_servo_angle(90)
        led_red.value(1)
        led_yellow.value(0)
        led_green.value(0)
        time.sleep(2)  # Wait for 1 second before the next measurement
    if (distance > 200):
        set_servo_angle(150)
        led_green.value(1)
        led_red.value(0)
        led_yellow.value(0)
        time.sleep(2)  # Wait for 1 second before the next measurement