from machine import Pin, PWM
from time import sleep

# =========================
# Khởi tạo servo
# =========================

left_servo = PWM(Pin(2))
right_servo = PWM(Pin(28))

# Servo hoạt động ở 50Hz
left_servo.freq(50)
right_servo.freq(50)


# =========================
# Hàm chuyển pulse sang duty
# =========================

def set_servo_angle(servo, angle):

    # Chuyển góc sang xung PWM
    min_us = 500
    max_us = 2500

    pulse_us = min_us + (angle / 180) * (max_us - min_us)

    duty = int(pulse_us * 65535 / 20000)

    servo.duty_u16(duty)


# =========================
# Robot đi tới
# =========================

def forward():

    set_servo_angle(left_servo, 135)
    set_servo_angle(right_servo, 45)

    print("Robot moving forward")


# =========================
# Robot đi lùi
# =========================

def backward():

    set_servo_angle(left_servo, 45)
    set_servo_angle(right_servo, 135)

    print("Robot moving backward")


# =========================
# Robot rẽ trái
# =========================

def turn_left():

    set_servo_angle(left_servo, 45)
    set_servo_angle(right_servo, 45)

    print("Robot turning left")


# =========================
# Robot rẽ phải
# =========================

def turn_right():

    set_servo_angle(left_servo, 135)
    set_servo_angle(right_servo, 135)

    print("Robot turning right")


# =========================
# Robot dừng
# =========================

def stop_robot():

    set_servo_angle(left_servo, 90)
    set_servo_angle(right_servo, 90)

    print("Robot stopped")


# =========================
# Loop demo tự động
# =========================

while True:

    forward()
    sleep(2)

    backward()
    sleep(2)

    turn_left()
    sleep(2)

    turn_right()
    sleep(2)

    stop_robot()
    sleep(2)