from machine import Pin, PWM, time_pulse_us
from time import sleep, sleep_us, ticks_us

# Motor direction pins
motor_left_dir = Pin(10, Pin.OUT)
motor_right_dir = Pin(8, Pin.OUT)

# Motor speed (PWM)
motor_left_speed = PWM(Pin(21), freq=1000)
motor_right_speed = PWM(Pin(22), freq=1000)

speed = 32768

# Ultrasonic sensor pins
trig = Pin(15, Pin.OUT)
echo = Pin(14, Pin.IN)

# IR sensors
ir_left = Pin(3, Pin.IN)
ir_right = Pin(6, Pin.IN)


# Get distance from ultrasonic sensor
def get_distance():
  trig.low()
  sleep_us(2)
  trig.high()
  sleep_us(10)
  trig.low()
 
  duration = time_pulse_us(echo, 1)
  distance_cm = int((duration * 0.034) / 2)
  return distance_cm


# Move forward
def move_forward():
    motor_left_dir.value(1)
    motor_right_dir.value(1)

    motor_left_speed.duty_u16(speed)
    motor_right_speed.duty_u16(speed)


# Stop robot
def stop_robot():
    motor_left_speed.duty_u16(0)
    motor_right_speed.duty_u16(0)


# Turn left
def turn_left():
    motor_left_dir.value(0)
    motor_right_dir.value(1)

    motor_left_speed.duty_u16(speed)
    motor_right_speed.duty_u16(speed)

    sleep(0.4)
    stop_robot()


# Turn right
def turn_right():
    motor_left_dir.value(1)
    motor_right_dir.value(0)

    motor_left_speed.duty_u16(speed)
    motor_right_speed.duty_u16(speed)

    sleep(0.4)
    stop_robot()


# Turn around (180)
def turn_180():
    motor_left_dir.value(1)
    motor_right_dir.value(0)

    motor_left_speed.duty_u16(speed)
    motor_right_speed.duty_u16(speed)

    sleep(1)
    stop_robot()


# Main loop
while True:

    distance = get_distance()

    left_state = ir_left.value()
    right_state = ir_right.value()

    print("Distance:", distance)

    # Object in front
    if distance < 20:
        print("Front obstacle detected")

        stop_robot()
        sleep(0.3)

        turn_180()

    # Obstacle on left
    elif left_state == 0:
        print("Obstacle on left")

        stop_robot()
        sleep(0.2)

        turn_right()

    # Obstacle on right
    elif right_state == 0:
        print("Obstacle on right")

        stop_robot()
        sleep(0.2)

        turn_left()

    # Path is clear
    else:
        move_forward()
        print("moving forward")

    sleep(0.1)