import time
from machine import Pin, PWM

# Define motor control pins for 4 motors
# Motor 1 (Right Front)
motor1_forward = Pin(2, Pin.OUT)
motor1_backward = Pin(3, Pin.OUT)
motor1_enable = PWM(Pin(15))

# Motor 2 (Left Rear)
motor2_forward = Pin(8, Pin.OUT)
motor2_backward = Pin(9, Pin.OUT)
motor2_enable = PWM(Pin(16))

# Motor 3 (Left Front)
motor3_forward = Pin(6, Pin.OUT)
motor3_backward = Pin(7, Pin.OUT)
motor3_enable = PWM(Pin(17))

# Motor 4 (Right Rear) - Changed pins
motor4_forward = Pin(12, Pin.OUT)  # Changed to GPIO 12
motor4_backward = Pin(13, Pin.OUT)  # Changed to GPIO 13
motor4_enable = PWM(Pin(18))

# Initialize PWM duty cycles
motor1_enable.duty_u16(32768)  # 50% duty cycle
motor2_enable.duty_u16(32768)
motor3_enable.duty_u16(32768)
motor4_enable.duty_u16(32768)

# Define Ultrasonic Sensor pins
trig = Pin(10, Pin.OUT)
echo = Pin(11, Pin.IN)

def measure_distance():
    # Trigger the sensor
    trig.off()
    time.sleep_us(2)
    trig.on()
    time.sleep_us(10)
    trig.off()

    # Measure the echo pulse duration
    while echo.value() == 0:
        pulse_start = time.ticks_us()
    while echo.value() == 1:
        pulse_end = time.ticks_us()

    pulse_duration = time.ticks_diff(pulse_end, pulse_start)
    distance = (pulse_duration * 0.0343) / 2  # Convert to cm
    return distance

def move_forward(duration):
    motor1_forward.on()
    motor4_forward.on()
    motor3_forward.on()
    motor2_forward.on()

    motor1_backward.off()
    motor4_backward.off()
    motor3_backward.off()
    motor2_backward.off()

    time.sleep(duration)
    stop_motors()

def move_clockwise(duration):
    motor1_backward.on()
    motor4_backward.on()
    motor3_forward.on()
    motor2_forward.on()

    motor1_forward.off()
    motor4_forward.off()
    motor3_backward.off()
    motor2_backward.off()

    time.sleep(duration)
    stop_motors()

def stop_motors():
    motor1_forward.off()
    motor1_backward.off()
    motor4_forward.off()
    motor4_backward.off()
    motor3_forward.off()
    motor3_backward.off()
    motor2_forward.off()
    motor2_backward.off()

# Main execution
if __name__ == "__main__":
    print("Starting robot...")
    while True:
        distance = measure_distance()
        print("Distance: {:.1f} cm".format(distance))
        if distance < 15:
            move_clockwise(0.25)
        elif distance > 15:
            move_forward(0.25)
        elif distance > 30:
            move_forward(0.25)
        time.sleep(0.2)