ESP32 AI 피지컬 컴퓨팅 & 로봇 프로젝트

from machine import ADC, Pin, PWM
import time

# 360도 무한회전 서보 2개
left_servo = PWM(Pin(13), freq=50)   # D13 S핀
right_servo = PWM(Pin(14), freq=50)  # D14 S핀

# 조이스틱: VCC는 5V, VRx/VRy는 저항 분압 후 ESP32 ADC로 입력
joy_x = ADC(Pin(34))  # VRx
joy_y = ADC(Pin(35))  # VRy
joy_sw = Pin(32, Pin.IN, Pin.PULL_UP)

for adc in (joy_x, joy_y):
    adc.atten(ADC.ATTN_11DB)
    adc.width(ADC.WIDTH_12BIT)

# 서보마다 정지값이 조금 다릅니다. 중립에서 움직이면 75~78 사이에서 조정하세요.
LEFT_STOP = 77
RIGHT_STOP = 77

MIN_DUTY = 40
MAX_DUTY = 115

# 조이스틱 중립 근처 흔들림을 무시하는 범위입니다.
RAW_DEAD_ZONE = 180
CENTER_SAMPLES = 30

def clamp(value, low=-100, high=100):
    return max(low, min(high, value))

def read_center(adc, samples=CENTER_SAMPLES):
    total = 0
    for _ in range(samples):
        total += adc.read()
        time.sleep_ms(10)
    return total // samples

def axis_to_speed(raw, center):
    """
    조이스틱 ADC 값을 -100~100 속도값으로 변환합니다.
    중립이면 0, 한쪽으로 밀면 양수, 반대쪽으로 밀면 음수가 됩니다.
    """
    delta = raw - center

    if abs(delta) < RAW_DEAD_ZONE:
        return 0

    if delta > 0:
        span = max(1, 4095 - center)
    else:
        span = max(1, center)

    speed = int(delta / span * 100)
    return clamp(speed)

def set_servo(servo, speed, stop_duty):
    """
    speed: -100(역방향 최대) ~ 0(정지) ~ 100(정방향 최대)
    """
    speed = clamp(speed)

    if speed == 0:
        duty = stop_duty
    elif speed > 0:
        duty = int(stop_duty + (MAX_DUTY - stop_duty) * speed / 100)
    else:
        duty = int(stop_duty - (stop_duty - MIN_DUTY) * abs(speed) / 100)

    servo.duty(duty)

def stop_all():
    set_servo(left_servo, 0, LEFT_STOP)
    set_servo(right_servo, 0, RIGHT_STOP)

print("조이스틱을 가운데에 놓고 기다리세요.")
center_x = read_center(joy_x)
center_y = read_center(joy_y)
print("중립값:", "x =", center_x, "y =", center_y)

stop_all()
time.sleep(1)

try:
    while True:
        turn = axis_to_speed(joy_x.read(), center_x)
        drive = -axis_to_speed(joy_y.read(), center_y)  # 방향이 반대면 -를 제거하세요.

        # 버튼을 누르면 즉시 정지
        if joy_sw.value() == 0:
            stop_all()
            time.sleep(0.1)
            continue

        # 중립이면 drive=0, turn=0이 되어 두 서보가 멈춥니다.
        # 레버를 많이 기울일수록 speed 절댓값이 커져 더 빠르게 회전합니다.
        left_speed = clamp(drive + turn)
        right_speed = clamp(drive - turn)

        # 오른쪽 서보가 왼쪽과 마주 보는 방향으로 붙어 있으면 부호를 반대로 줍니다.
        set_servo(left_servo, left_speed, LEFT_STOP)
        set_servo(right_servo, -right_speed, RIGHT_STOP)

        print("turn:", turn, "drive:", drive, "left:", left_speed, "right:", right_speed)
        time.sleep(0.05)

except KeyboardInterrupt:
    pass

finally:
    stop_all()
    left_servo.deinit()
    right_servo.deinit()