3-3. 라즈베리파이 피코 웹 블루투스

# 코드 작성자 : 상암고 교사 성원경(CHATGPT o3와 함께 코드 작성) # 메일 : wonking710@naver.com # 사용자 설정 영역 BT_NAME = "wonking" # 블루투스 출력 이름 바꿔서 쓰길 권장, 영문 8글자 이내 PIN_SERVO1 = 16 # 360° 서보 #1 (GP16) PIN_SERVO2 = 17 # 360° 서보 #2 (GP17) PIN_SERVO3 = 14 # 180° 서보 #3 (GP14) PIN_SERVO4 = 15 # 180° 서보 #4 (GP15) PIN_EXT_LED = 1 # 외부 LED 핀 PIN_RGB_RED = 19 # RGB LED Red 핀 PIN_RGB_GREEN = 20 # RGB LED Green 핀 PIN_RGB_BLUE = 21 # RGB LED Blue 핀 from machine import Pin, PWM from micropython import const import bluetooth import struct import time # ========== BLE Advertising 관련 코드 ========== _ADV_TYPE_FLAGS = const(0x01) _ADV_TYPE_NAME = const(0x09) _ADV_TYPE_UUID16_COMPLETE = const(0x3) _ADV_TYPE_UUID32_COMPLETE = const(0x5) _ADV_TYPE_UUID128_COMPLETE = const(0x7) _ADV_TYPE_UUID16_MORE = const(0x2) _ADV_TYPE_UUID32_MORE = const(0x4) _ADV_TYPE_UUID128_MORE = const(0x6) _ADV_TYPE_APPEARANCE = const(0x19) def advertising_payload(limited_disc=False, br_edr=False, name=None, services=None, appearance=0): payload = bytearray() def _append(adv_type, value): nonlocal payload payload += struct.pack("BB", len(value) + 1, adv_type) + value _append(_ADV_TYPE_FLAGS, struct.pack("B", (0x01 if limited_disc else 0x02) + (0x18 if br_edr else 0x04))) if name: _append(_ADV_TYPE_NAME, name) if services: for uuid in services: b = bytes(uuid) if len(b) == 2: _append(_ADV_TYPE_UUID16_COMPLETE, b) elif len(b) == 4: _append(_ADV_TYPE_UUID32_COMPLETE, b) elif len(b) == 16: _append(_ADV_TYPE_UUID128_COMPLETE, b) if appearance: _append(_ADV_TYPE_APPEARANCE, struct.pack("<h", appearance)) return payload def decode_field(payload, adv_type): i, result = 0, [] while i + 1 < len(payload): if payload[i + 1] == adv_type: result.append(payload[i + 2 : i + payload[i] + 1]) i += 1 + payload[i] return result def decode_name(payload): n = decode_field(payload, _ADV_TYPE_NAME) return str(n[0], "utf-8") if n else "" def decode_services(payload): services = [] for u in decode_field(payload, _ADV_TYPE_UUID16_COMPLETE): services.append(bluetooth.UUID(struct.unpack("<h", u)[0])) for u in decode_field(payload, _ADV_TYPE_UUID32_COMPLETE): services.append(bluetooth.UUID(struct.unpack("<d", u)[0])) for u in decode_field(payload, _ADV_TYPE_UUID128_COMPLETE): services.append(bluetooth.UUID(u)) return services # ========== BLE Simple Peripheral 관련 코드 ========== _IRQ_CENTRAL_CONNECT = const(1) _IRQ_CENTRAL_DISCONNECT = const(2) _IRQ_GATTS_WRITE = const(3) _FLAG_READ = const(0x0002) _FLAG_WRITE_NO_RESPONSE = const(0x0004) _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_READ | _FLAG_NOTIFY) _UART_RX = (bluetooth.UUID("6E400002-B5A3-F393-E0A9-E50E24DCCA9E"), _FLAG_WRITE | _FLAG_WRITE_NO_RESPONSE) _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._handle_tx, self._handle_rx),) = self._ble.gatts_register_services((_UART_SERVICE,)) self._connections = set() self._write_callback = None self._payload = advertising_payload(name=name, services=[_UART_UUID]) self._advertise() def _irq(self, event, data): if event == _IRQ_CENTRAL_CONNECT: conn, _, _ = data print("New connection", conn) self._connections.add(conn) elif event == _IRQ_CENTRAL_DISCONNECT: conn, _, _ = data print("Disconnected", conn) self._connections.remove(conn) self._advertise() elif event == _IRQ_GATTS_WRITE: _, value_handle = data value = self._ble.gatts_read(value_handle) if value_handle == self._handle_rx and self._write_callback: self._write_callback(value) def send(self, data): for h in self._connections: self._ble.gatts_notify(h, self._handle_tx, data) def is_connected(self): return bool(self._connections) def _advertise(self, interval_us=500000): print("Starting advertising") self._ble.gap_advertise(interval_us, adv_data=self._payload) def on_write(self, callback): self._write_callback = callback # ========== 간단한 큐 클래스 ========== class SimpleQueue: def __init__(self, maxsize): self.queue = [] self.maxsize = maxsize def put(self, item): if len(self.queue) < self.maxsize: self.queue.append(item) else: raise OverflowError("Queue is full") def get(self): if self.queue: return self.queue.pop(0) raise IndexError("Queue is empty") def empty(self): return not self.queue def full(self): return len(self.queue) >= self.maxsize # ========== 서보 설정 및 변수 ========== servo1 = PWM(Pin(PIN_SERVO1), freq=50) servo2 = PWM(Pin(PIN_SERVO2), freq=50) min_duty = 1638 # CCW 최대 mid_duty = 4915 # 정지 max_duty = 8192 # CW 최대 speed = 0.5 # 0.0~1.0 state = '1' # '1'=정지, '2'=CW, '3'=CCW def apply_servo(): if state == '2': d = int(mid_duty + speed * (max_duty - mid_duty)) elif state == '3': d = int(mid_duty - speed * (mid_duty - min_duty)) else: d = mid_duty servo1.duty_u16(d) servo2.duty_u16(d) servo3 = PWM(Pin(PIN_SERVO3), freq=50) servo4 = PWM(Pin(PIN_SERVO4), freq=50) servo3_angle = 90 servo4_angle = 90 def angle_to_duty(angle): # 0°≈1ms(3277), 180°≈2ms(6553) return int(3277 + (angle / 180) * (6553 - 3277)) def handle_servo3_left(): global servo3_angle servo3_angle = max(0, servo3_angle - 10) servo3.duty_u16(angle_to_duty(servo3_angle)) print(f"Servo3 ← {servo3_angle}°") def handle_servo3_right(): global servo3_angle servo3_angle = min(180, servo3_angle + 10) servo3.duty_u16(angle_to_duty(servo3_angle)) print(f"Servo3 → {servo3_angle}°") def handle_servo4_up(): global servo4_angle servo4_angle = min(180, servo4_angle + 10) servo4.duty_u16(angle_to_duty(servo4_angle)) print(f"Servo4 ↑ {servo4_angle}°") def handle_servo4_down(): global servo4_angle servo4_angle = max(0, servo4_angle - 10) servo4.duty_u16(angle_to_duty(servo4_angle)) print(f"Servo4 ↓ {servo4_angle}°") # ========== LED 설정 ========== onboard_led = Pin("LED", Pin.OUT) external_led = Pin(PIN_EXT_LED, Pin.OUT) rgb_red = PWM(Pin(PIN_RGB_RED), freq=1000) rgb_green = PWM(Pin(PIN_RGB_GREEN), freq=1000) rgb_blue = PWM(Pin(PIN_RGB_BLUE), freq=1000) led_blink_active = False led_blink_state = False last_blink_time = 0 # ========== 키 핸들러(1~9, a/d/w/s, x) ========== def handle_key_1(): global state state = '1'; apply_servo(); print("→ 360° 서보 정지") def handle_key_2(): global state state = '2'; apply_servo(); print(f"→ 360° 서보 CW ({speed:.1f})") def handle_key_3(): global state state = '3'; apply_servo(); print(f"→ 360° 서보 CCW ({speed:.1f})") def handle_key_4(): global speed old = speed; speed = max(0.0, speed - 0.1) print(f"→ 속도 느리게: {old:.1f}→{speed:.1f}"); apply_servo() def handle_key_5(): global speed old = speed; speed = min(1.0, speed + 0.1) print(f"→ 속도 빠르게: {old:.1f}→{speed:.1f}"); apply_servo() def handle_key_6(): global led_blink_active led_blink_active = not led_blink_active if not led_blink_active: onboard_led.off() print(f"내부 LED 깜빡이기: {'ON' if led_blink_active else 'OFF'}") def handle_key_7(): rgb_red.duty_u16(65535); rgb_green.duty_u16(0); rgb_blue.duty_u16(0) print("RGB LED: Red") def handle_key_8(): rgb_red.duty_u16(0); rgb_green.duty_u16(65535); rgb_blue.duty_u16(0) print("RGB LED: Green") def handle_key_9(): rgb_red.duty_u16(0); rgb_green.duty_u16(0); rgb_blue.duty_u16(65535) print("RGB LED: Blue") def handle_key_a(): handle_servo3_left() def handle_key_d(): handle_servo3_right() def handle_key_w(): handle_servo4_up() def handle_key_s(): handle_servo4_down() def handle_reset_all(): global state, speed, led_blink_active, led_blink_state state, speed = '1', 0.5 apply_servo() led_blink_active = False led_blink_state = False onboard_led.off(); external_led.off() rgb_red.duty_u16(0); rgb_green.duty_u16(0); rgb_blue.duty_u16(0) global servo3_angle, servo4_angle servo3_angle = servo4_angle = 90 servo3.duty_u16(angle_to_duty(servo3_angle)) servo4.duty_u16(angle_to_duty(servo4_angle)) print("All systems reset") # ========== 주기적 작업 처리 ========== def handle_periodic_tasks(): global last_blink_time, led_blink_state now = time.ticks_ms() if led_blink_active and time.ticks_diff(now, last_blink_time) > 500: led_blink_state = not led_blink_state onboard_led.value(led_blink_state) last_blink_time = now # ========== 데이터 처리 & BLE 수신 핸들러 ========== data_queue = SimpleQueue(10) def process_data(): while not data_queue.empty(): data = data_queue.get() cmd = chr(data[0]) if cmd == '1': handle_key_1() elif cmd == '2': handle_key_2() elif cmd == '3': handle_key_3() elif cmd == '4': handle_key_4() elif cmd == '5': handle_key_5() elif cmd == '6': handle_key_6() elif cmd == '7': handle_key_7() elif cmd == '8': handle_key_8() elif cmd == '9': handle_key_9() elif cmd == 'a': handle_key_a() elif cmd == 'd': handle_key_d() elif cmd == 'w': handle_key_w() elif cmd == 's': handle_key_s() elif cmd == 'x': handle_reset_all() else: print("Unknown cmd:", cmd) def on_rx(data): print("Received:", data) if not data_queue.full(): data_queue.put(data) # ========== 메인 ========== ble = bluetooth.BLE() sp = BLESimplePeripheral(ble) handle_reset_all() print(""" === Control Keys === 360° 서보: 1=Stop 2=CW 3=CCW 4=Slow↓ 5=Fast↑ 내부 LED Blink=6 RGB Red/Green/Blue=7/8/9 180° 서보: a=Servo3← d=Servo3→ w=Servo4↑ s=Servo4↓ x=Reset All Waiting for connections... """) while True: if sp.is_connected(): sp.on_write(on_rx) process_data() handle_periodic_tasks() else: time.sleep(0.1)