안내 영상 꼭보세요
리모콘 사이트 : 반드시 크롬앱에서 열어주세요. 안드로이드 폰 혹은 태블릿, 윈도우 노트북 등
아래 피코 코드, 반드시 블루투스 이름과 핀 연결 확인하세요.
# ====== 설정: 이름, 핀번호 ======
BT_NAME = "swk"
# 180도 서보(걷기)
PIN_SERVO1 = 10
PIN_SERVO2 = 11
PIN_SERVO3 = 14
PIN_SERVO4 = 15
# 360도 서보(바퀴)
PIN_SERVO_FL_360 = 2
PIN_SERVO2_BL_360 = 3
PIN_SERVO3_FR_360 = 6
PIN_SERVO4_BR_360 = 7
from machine import Pin, PWM
import time
# ====== 180도 서보(걷기) ======
servo1 = PWM(Pin(PIN_SERVO1)); servo1.freq(50)
servo2 = PWM(Pin(PIN_SERVO2)); servo2.freq(50)
servo3 = PWM(Pin(PIN_SERVO3)); servo3.freq(50)
servo4 = PWM(Pin(PIN_SERVO4)); servo4.freq(50)
neutral = 90
phase = 0
curr_angle = {'servo1': neutral, 'servo2': neutral, 'servo3': neutral, 'servo4': neutral}
STEP_CONFIG = {
1: {'speed': 0.4, 'big': 16, 'small': 5},
2: {'speed': 0.7, 'big': 24, 'small': 7},
3: {'speed': 1.0, 'big': 32, 'small': 9}, # 기본
4: {'speed': 1.3, 'big': 40, 'small': 11},
5: {'speed': 1.6, 'big': 48, 'small': 13}
}
speed_step = 3 # 기본값 3단계
last_drive_cmd = None
def angle_to_duty(angle):
min_duty, max_duty = 1638, 8192
return int(min_duty + (angle / 180) * (max_duty - min_duty))
def safe_set_servo(servo, name, target_angle, step=4, delay=0.004):
current = curr_angle[name]
target_angle = int(target_angle)
if abs(target_angle - current) < step:
servo.duty_u16(angle_to_duty(target_angle))
else:
direction = 1 if target_angle > current else -1
for angle in range(current, target_angle, step * direction):
servo.duty_u16(angle_to_duty(angle))
time.sleep(delay)
servo.duty_u16(angle_to_duty(target_angle))
curr_angle[name] = target_angle
def reset_servos_180():
for s, n in zip((servo1, servo2, servo3, servo4), ('servo1', 'servo2', 'servo3', 'servo4')):
safe_set_servo(s, n, neutral)
def set_all_servos_180(angle):
for s, n in zip((servo1, servo2, servo3, servo4), ('servo1', 'servo2', 'servo3', 'servo4')):
safe_set_servo(s, n, angle)
def add_all_servos_180(delta):
for n, s in zip(('servo1', 'servo2', 'servo3', 'servo4'), (servo1, servo2, servo3, servo4)):
new_angle = min(180, max(0, curr_angle[n] + delta))
safe_set_servo(s, n, new_angle)
def move_legs(direction):
global phase, speed_step
SWING_BIG = STEP_CONFIG[speed_step]['big']
SWING_SMALL = STEP_CONFIG[speed_step]['small']
big = SWING_BIG if phase == 0 else -SWING_BIG
small = SWING_SMALL if phase == 0 else -SWING_SMALL
if direction == "forward":
angles = [neutral + big, neutral - big, neutral + big, neutral - big]
elif direction == "backward":
angles = [neutral - big, neutral + big, neutral - big, neutral + big]
elif direction == "left":
angles = [neutral + small, neutral - big, neutral + small, neutral - big]
elif direction == "right":
angles = [neutral + big, neutral - small, neutral + big, neutral - small]
else:
angles = [neutral, neutral, neutral, neutral]
safe_set_servo(servo1, 'servo1', angles[0])
safe_set_servo(servo2, 'servo2', angles[1])
safe_set_servo(servo3, 'servo3', angles[2])
safe_set_servo(servo4, 'servo4', angles[3])
phase = 1 - phase
# ====== 360도 서보(바퀴) ======
servo_FL = PWM(Pin(PIN_SERVO_FL_360)); servo_FL.freq(50)
servo_BL = PWM(Pin(PIN_SERVO2_BL_360)); servo_BL.freq(50)
servo_FR = PWM(Pin(PIN_SERVO3_FR_360)); servo_FR.freq(50)
servo_BR = PWM(Pin(PIN_SERVO4_BR_360)); servo_BR.freq(50)
def stop_all_servos_360():
stop = angle_to_duty(90)
for s in (servo_FL, servo_BL, servo_FR, servo_BR):
s.duty_u16(stop)
def reset_all():
global last_drive_cmd
reset_servos_180()
stop_all_servos_360()
last_drive_cmd = None # 바퀴 상태 리셋
print("모든 서보(다리/바퀴) 정지 및 초기화")
def drive_servo(mode):
global last_drive_cmd, speed_step
speed_scale = STEP_CONFIG[speed_step]['speed']
stop = angle_to_duty(90)
def scaled(val):
return int(90 + (val - 90) * speed_scale)
left_forward = angle_to_duty(scaled(120))
left_backward = angle_to_duty(scaled(60))
right_forward = angle_to_duty(scaled(60))
right_backward = angle_to_duty(scaled(120))
left_slow_fwd = angle_to_duty(scaled(100))
left_slow_bwd = angle_to_duty(scaled(80))
right_slow_fwd = angle_to_duty(scaled(80))
right_slow_bwd = angle_to_duty(scaled(100))
if mode == '2': # 전진 (왼쪽:정방향, 오른쪽:역방향)
servo_FL.duty_u16(left_forward)
servo_BL.duty_u16(left_forward)
servo_FR.duty_u16(right_forward)
servo_BR.duty_u16(right_forward)
print("바퀴 전진 (대칭)")
last_drive_cmd = mode
elif mode == '5': # 후진 (왼쪽:역방향, 오른쪽:정방향)
servo_FL.duty_u16(left_backward)
servo_BL.duty_u16(left_backward)
servo_FR.duty_u16(right_backward)
servo_BR.duty_u16(right_backward)
print("바퀴 후진 (대칭)")
last_drive_cmd = mode
elif mode == '4': # 좌회전 (왼쪽 느린 역방향, 오른쪽 약한 정방향)
servo_FL.duty_u16(left_slow_bwd)
servo_BL.duty_u16(left_slow_bwd)
servo_FR.duty_u16(right_forward)
servo_BR.duty_u16(right_forward)
print("바퀴 좌회전 (완만, 대칭)")
last_drive_cmd = mode
elif mode == '6': # 우회전 (오른쪽 느린 역방향, 왼쪽 약한 정방향)
servo_FL.duty_u16(left_forward)
servo_BL.duty_u16(left_forward)
servo_FR.duty_u16(right_slow_bwd)
servo_BR.duty_u16(right_slow_bwd)
print("바퀴 우회전 (완만, 대칭)")
last_drive_cmd = mode
else: # 정지(8 등)
stop_all_servos_360()
print("바퀴 정지")
last_drive_cmd = None # 정지상태에서는 drive_cmd도 None
def set_speed(cmd):
global speed_step
changed = False
if cmd == '1': # DOWN
if speed_step > 1:
speed_step -= 1
changed = True
elif cmd == '3': # UP
if speed_step < 5:
speed_step += 1
changed = True
elif cmd in ['2', '4', '5']: # 직접 단계 설정
step = int(cmd)
if 1 <= step <= 5:
speed_step = step
changed = True
print(f"[속도단계] {speed_step} / 바퀴배율 {STEP_CONFIG[speed_step]['speed']} / 보폭 {STEP_CONFIG[speed_step]['big']}")
# 180, 360 둘 다 "동작 중"일 때만 즉시 적용
# state가 'forward', ... 등 걷기 중이거나
# last_drive_cmd가 2/4/5/6 중 하나면 바퀴 동작중
if changed:
if state in ("forward", "backward", "left", "right"):
# 걷기 중이면 걷기 동작에만 반영 (move_legs 호출되면서 적용됨)
pass # move_legs에서 자동 반영됨
elif last_drive_cmd in ['2', '4', '5', '6']:
# 바퀴 동작 중일 때만 반영
drive_servo(last_drive_cmd)
# else: 모두 정지 중이면 반영 X (상태만 바뀜, 모터 동작 X)
reset_servos_180()
stop_all_servos_360()
state = "stop"
# ====== BLE/블루투스(아래에 위치) ======
from micropython import const
import bluetooth, struct
_ADV_TYPE_FLAGS = const(0x01)
_ADV_TYPE_NAME = const(0x09)
_ADV_TYPE_UUID128_COMPLETE = const(0x07)
def advertising_payload(name=None, services=None):
payload = bytearray()
def _append(adv_type, value):
payload.extend(struct.pack("BB", len(value)+1, adv_type) + value)
_append(_ADV_TYPE_FLAGS, b'\x06')
if name: _append(_ADV_TYPE_NAME, name.encode())
if services:
for uuid in services:
_append(_ADV_TYPE_UUID128_COMPLETE, bytes(uuid))
return payload
_IRQ_CENTRAL_CONNECT = const(1)
_IRQ_CENTRAL_DISCONNECT = const(2)
_IRQ_GATTS_WRITE = const(3)
_FLAG_WRITE = const(0x0008)
_FLAG_NOTIFY = const(0x0010)
_UART_UUID = bluetooth.UUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
_UART_TX = (bluetooth.UUID("6E400003-B5A3-F393-E0A9-E50E24DCCA9E"), _FLAG_NOTIFY)
_UART_RX = (bluetooth.UUID("6E400002-B5A3-F393-E0A9-E50E24DCCA9E"), _FLAG_WRITE)
_UART_SERVICE = (_UART_UUID, (_UART_TX, _UART_RX))
class BLESimplePeripheral:
def __init__(self, ble, name=BT_NAME):
self._ble = ble
self._ble.active(True)
self._ble.irq(self._irq)
((self._tx_handle, self._rx_handle),) = self._ble.gatts_register_services((_UART_SERVICE,))
self._connections = set()
self._write_callback = None
self._advertise(name)
def _advertise(self, name):
payload = advertising_payload(name=name, services=[_UART_UUID])
self._ble.gap_advertise(500000, adv_data=payload)
def _irq(self, event, data):
if event == _IRQ_CENTRAL_CONNECT:
conn_handle, _, _ = data
self._connections.add(conn_handle)
elif event == _IRQ_CENTRAL_DISCONNECT:
conn_handle, _, _ = data
self._connections.remove(conn_handle)
self._advertise(BT_NAME)
elif event == _IRQ_GATTS_WRITE:
conn_handle, value_handle = data
value = self._ble.gatts_read(value_handle)
if self._write_callback:
try:
self._write_callback(value)
except Exception as e:
print(f"Write callback exception: {e}")
def on_write(self, callback):
self._write_callback = callback
def handle_cmd(data):
global state, phase, last_drive_cmd
try:
cmd = data.decode().strip().lower()
# 180도 서보 걷기
if cmd in ["w", "s", "a", "d"]:
state, phase = {
"w": ("forward", 0),
"s": ("backward", 0),
"a": ("left", 0),
"d": ("right", 0)
}[cmd]
print(f"걷기 {cmd}")
elif cmd == "x":
state = "stop"
reset_all()
elif cmd in ['2', '5', '4', '6']:
drive_servo(cmd)
elif cmd in ['1', '2', '3', '4', '5']:
set_speed(cmd)
elif cmd == "8":
set_all_servos_180(90)
print("180도 전체 90도로 초기화")
elif cmd == "7":
add_all_servos_180(-15)
print("180도 전체 15도 감소")
elif cmd == "9":
add_all_servos_180(15)
print("180도 전체 15도 증가")
else:
print(f"알 수 없는 명령: {cmd}")
except Exception as e:
print(f"BLE 핸들러 오류: {e}")
ble = bluetooth.BLE()
sp = BLESimplePeripheral(ble, name=BT_NAME)
sp.on_write(handle_cmd)
print(f"BLE 180도/360도 서보 통합({BT_NAME}) - 7/8/9: 180도 각도 제어, 1:DOWN, 3:UP, 2/4/5:직접 단계, 5단계")
print("w/s/a/d/x: 다리 걷기, 2/5/4/6: 바퀴, 1:DOWN, 3:UP, 2/4/5:직접 단계, 7:15°감소, 8:90°, 9:15°증가")
while True:
if state in ("forward", "backward", "left", "right"):
move_legs(state)
time.sleep(0.14)
else:
time.sleep(0.05)
Python
복사