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

# Ultrasonic Sensor Pins
TRIG = Pin(15, Pin.OUT)
ECHO = Pin(14, Pin.IN)

# CSV file name
log_file = "/distance_log.csv"

sr_no = 1

# Create CSV file with header
with open(log_file, "w") as f:
    f.write("Reading,A,B,C,D,Distance(cm)\n")

# Motor pins
M1 = Pin(10, Pin.OUT)  # Motor 1 direction
M2 = Pin(8, Pin.OUT)   # Motor 2 direction
SM1 = PWM(Pin(21), freq=1000)  # Motor 1 speed control (PWM)
SM2 = PWM(Pin(22), freq=1000)  # Motor 2 speed control (PWM)

# Set up RF input pins (A, B, C, D signals)
rf_D = Pin(13, Pin.IN)  # RF signal for moving forward (A)
rf_C = Pin(20, Pin.IN)  # RF signal for turning left (B)
rf_B = Pin(19, Pin.IN)  # RF signal for turning right (C)
rf_A = Pin(18, Pin.IN)  # RF signal for moving backward (D)

def move_forward():  # Move robot forward
    SM1.duty_u16(32768)
    SM2.duty_u16(32768)
    M1.value(1)
    M2.value(1)

def move_backward():  # Move robot backward
    SM1.duty_u16(32768)
    SM2.duty_u16(32768)
    M1.value(0)
    M2.value(0)

def turn_left():  # Turn robot left
    SM1.duty_u16(32768)
    SM2.duty_u16(32768)
    M1.value(1)  # Stop motor 1 (right motor)
    M2.value(0)  # Move motor 2 (left motor)

def turn_right():  # Turn robot right
    SM1.duty_u16(32768)
    SM2.duty_u16(32768)
    M1.value(0)  # Move motor 1 (right motor)
    M2.value(1)  # Stop motor 2 (left motor)

def stop_robot():  # Stop robot
    SM1.duty_u16(0)
    SM2.duty_u16(0)

# Function to measure distance
def get_distance():
    TRIG.low()
    sleep_ms(2)

    TRIG.high()
    sleep_us(10)
    TRIG.low()

    duration = time_pulse_us(ECHO, 1, 30000)

    # If no echo is received
    if duration < 0:
        return -1

    # Convert time to distance in cm
    distance = (duration * 0.0343) / 2

    return round(distance, 2)

while True:
    # Read RF input signals (A, B, C, D)
    A = rf_A.value()  # Read signal A
    B = rf_B.value()  # Read signal B
    C = rf_C.value()  # Read signal C
    D = rf_D.value()  # Read signal D

    distance = get_distance()

    if A == 1:  # If 'A' is received, move forward
        move_forward()

    elif B == 1:  # If 'B' is received, turn left
        turn_left()

    elif C == 1:  # If 'C' is received, turn right
        turn_right()

    elif D == 1:  # If 'D' is received, move backward
        move_backward()

    else:
        stop_robot()

    # Save data to CSV
    with open(log_file, "a") as f:
        f.write("{},{},{},{},{},{}\n".format(
            sr_no, A, B, C, D, distance))

    # Display on Serial Monitor
    print("Reading", sr_no,
          "A =", A,
          "B =", B,
          "C =", C,
          "D =", D,
          "Distance =", distance, "cm")

    sr_no += 1

    # Small delay to avoid rapid continuous checks
    sleep(0.1)