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)