from machine import Pin, PWM
from time import sleep

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

left_servo = PWM(Pin(13))
right_servo = PWM(Pin(23))

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

# Giá trị tốc độ
speed_offset = 300


# =========================
# Hàm điều khiển servo
# =========================

def set_servo(servo, pulse_us):

    # Chu kỳ servo là 20ms
    duty = int(pulse_us * 65535 / 20000)

    servo.duty_u16(duty)


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

def forward():

    set_servo(left_servo, 1500 + speed_offset)
    set_servo(right_servo, 1500 - speed_offset)


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

def backward():

    set_servo(left_servo, 1500 - speed_offset)
    set_servo(right_servo, 1500 + speed_offset)


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

def turn_left():

    set_servo(left_servo, 1500 - speed_offset)
    set_servo(right_servo, 1500 - speed_offset)


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

def turn_right():

    set_servo(left_servo, 1500 + speed_offset)
    set_servo(right_servo, 1500 + speed_offset)


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

def stop_robot():

    set_servo(left_servo, 1500)
    set_servo(right_servo, 1500)


# =========================
# Demo tự động
# =========================

while True:

    print("Forward")
    forward()
    sleep(2)

    print("Backward")
    backward()
    sleep(2)

    print("Left")
    turn_left()
    sleep(2)

    print("Right")
    turn_right()
    sleep(2)

    print("Stop")
    stop_robot()
    sleep(2)