from machine import Pin, ADC, PWM
import time

# Khai báo chân
motor1_en = PWM(Pin(13), freq=1000)
motor1_in1 = Pin(12, Pin.OUT)
motor1_in2 = Pin(14, Pin.OUT)

motor2_en = PWM(Pin(27), freq=1000)
motor2_in3 = Pin(25, Pin.OUT)
motor2_in4 = Pin(26, Pin.OUT)

joy_vert = ADC(Pin(0))  # trục Y
joy_horz = ADC(Pin(4))  # trục X
joy_sel = Pin(16, Pin.IN, Pin.PULL_UP)  # nút bấm joystick (nếu cần)

# ADC cấu hình (0 - 4095)
joy_vert.atten(ADC.ATTN_11DB)
joy_horz.atten(ADC.ATTN_11DB)

deadzone = 200  # vùng chết

def motor1_forward():
    motor1_in1.on()
    motor1_in2.off()
    motor1_en.duty(1023)  # tốc độ tối đa

def motor1_backward():
    motor1_in1.off()
    motor1_in2.on()
    motor1_en.duty(1023)

def motor1_stop():
    motor1_in1.off()
    motor1_in2.off()
    motor1_en.duty(0)

def motor2_forward():
    motor2_in3.on()
    motor2_in4.off()
    motor2_en.duty(1023)

def motor2_backward():
    motor2_in3.off()
    motor2_in4.on()
    motor2_en.duty(1023)

def motor2_stop():
    motor2_in3.off()
    motor2_in4.off()
    motor2_en.duty(0)

while True:
    vert = joy_vert.read()
    horz = joy_horz.read()

    # Motor 1: theo trục Y
    if vert > 3000:
        motor1_forward()
    elif vert < 1000:
        motor1_backward()
    else:
        motor1_stop()

    # Motor 2: theo trục X
    if horz > 3000:
        motor2_forward()
    elif horz < 1000:
        motor2_backward()
    else:
        motor2_stop()

    time.sleep(0.1)