피코 블루투스(BLE) 제어 - 2개 서보모터 테스트

from machine import Pin, PWM import bluetooth from ble_simple_peripheral import BLESimplePeripheral import utime # 블루투스 저에너지(BLE) 객체 생성 ble = bluetooth.BLE() # BLE 간단한 주변장치 객체 생성 sp = BLESimplePeripheral(ble) # 온보드 LED를 제어하기 위한 핀 설정 led = Pin("LED", Pin.OUT) led.value(1) # 연결 확인을 위해 초기 LED 켜짐 # 서보모터를 제어하기 위한 PWM 객체 생성 servo_horiz = PWM(Pin(3)) # 수평 방향 서보모터 (GP3) servo_vert = PWM(Pin(5)) # 수직 방향 서보모터 (GP5) # 서보모터의 주파수를 50Hz로 설정 servo_horiz.freq(50) servo_vert.freq(50) # 최소 및 최대 듀티 사이클 값 정의 min_duty = 1638 # 0도에 해당하는 듀티 사이클 max_duty = 8192 # 180도에 해당하는 듀티 사이클 def angle_to_duty(angle): # 주어진 각도(0~180도)를 듀티 사이클로 변환하는 함수 return int(min_duty + (max_duty - min_duty) * angle / 180) # 초기 각도를 90도로 설정 (중앙 위치) angle_horiz = 90 angle_vert = 90 # 서보모터를 초기 위치로 이동 servo_horiz.duty_u16(angle_to_duty(angle_horiz)) servo_vert.duty_u16(angle_to_duty(angle_vert)) # LED 상태 변수 led_state = 1 # BLE를 통해 수신된 데이터를 처리하는 콜백 함수 정의 def on_rx(data): global angle_horiz, angle_vert, led_state print("데이터 수신: ", data) # 수신된 데이터 출력 command = data.decode().strip() # 바이트 데이터를 문자열로 변환하고 공백 제거 if command == 'led': # LED 상태 토글 led.value(not led_state) led_state = 1 - led_state print("LED 상태 변경") elif command == 'w': # 위로 이동: 수직 각도를 증가 angle_vert += 10 if angle_vert > 180: angle_vert = 180 servo_vert.duty_u16(angle_to_duty(angle_vert)) print(f"위로 이동: 수직 각도 = {angle_vert}") elif command == 's': # 아래로 이동: 수직 각도를 감소 angle_vert -= 10 if angle_vert < 0: angle_vert = 0 servo_vert.duty_u16(angle_to_duty(angle_vert)) print(f"아래로 이동: 수직 각도 = {angle_vert}") elif command == 'a': # 왼쪽으로 이동: 수평 각도를 감소 angle_horiz -= 10 if angle_horiz < 0: angle_horiz = 0 servo_horiz.duty_u16(angle_to_duty(angle_horiz)) print(f"왼쪽으로 이동: 수평 각도 = {angle_horiz}") elif command == 'd': # 오른쪽으로 이동: 수평 각도를 증가 angle_horiz += 10 if angle_horiz > 180: angle_horiz = 180 servo_horiz.duty_u16(angle_to_duty(angle_horiz)) print(f"오른쪽으로 이동: 수평 각도 = {angle_horiz}") elif command == 'q': # 각도를 초기화하여 중앙 위치로 이동 angle_horiz = 90 angle_vert = 90 servo_horiz.duty_u16(angle_to_duty(angle_horiz)) servo_vert.duty_u16(angle_to_duty(angle_vert)) print("각도를 초기화합니다: 수평 각도 = 90, 수직 각도 = 90") else: print("유효하지 않은 명령입니다. 'w', 'a', 's', 'd', 'q' 또는 'led'를 사용하세요.") # BLE 연결 상태를 확인하고 데이터 수신 시 콜백 함수 등록 while True: if sp.is_connected(): sp.on_write(on_rx) # 데이터 수신 시 콜백 함수 호출 utime.sleep(0.1) # CPU 사용량을 줄이기 위한 짧은 지연

from machine import Pin, PWM from time import sleep import bluetooth from ble_simple_peripheral import BLESimplePeripheral import utime # 각도를 듀티 사이클로 변환하는 함수 def angle_to_duty(angle): min_duty = 1638 # 최소 듀티 (0도에 해당) max_duty = 8192 # 최대 듀티 (180도에 해당) duty = int(min_duty + (angle / 180.0) * (max_duty - min_duty)) return duty # 서보 모터 초기화 servos = {} # 서보 모터에 연결된 GPIO 핀 설정 servo_pins = { 'Leg1F': 4, 'Leg1B': 3, 'Leg2F': 2, 'Leg2B': 1, 'Leg3F': 8, 'Leg3B': 7, 'Leg4F': 6, 'Leg4B': 5, 'Headservo': 9 } for name, pin_num in servo_pins.items(): pwm = PWM(Pin(pin_num)) pwm.freq(50) # 서보 모터용 주파수 설정 servos[name] = pwm # 초기 각도 설정 LALeg1F = 80 LALeg1B = 100 LALeg2F = 100 LALeg2B = 80 LALeg3F = 80 LALeg3B = 100 LALeg4F = 100 LALeg4B = 80 LAHeadservo = 90 # 초기 머리 각도 # 서보 초기 상태 저장 motor_running = False # 초기에는 정지 상태 # 초기 위치로 서보 모터 이동 def initialize_position(): servos['Leg1F'].duty_u16(angle_to_duty(LALeg1F)) servos['Leg1B'].duty_u16(angle_to_duty(LALeg1B)) servos['Leg2F'].duty_u16(angle_to_duty(LALeg2F)) servos['Leg2B'].duty_u16(angle_to_duty(LALeg2B)) servos['Leg3F'].duty_u16(angle_to_duty(LALeg3F)) servos['Leg3B'].duty_u16(angle_to_duty(LALeg3B)) servos['Leg4F'].duty_u16(angle_to_duty(LALeg4F)) servos['Leg4B'].duty_u16(angle_to_duty(LALeg4B)) servos['Headservo'].duty_u16(angle_to_duty(LAHeadservo)) # 머리 서보를 초기 각도로 설정 initialize_position() sleep(2) # 초기화 상태 유지 시간 # 목표 각도 설정 TOLeg1F = LALeg1F TOLeg1B = LALeg1B TOLeg2F = LALeg2F TOLeg2B = LALeg2B TOLeg3F = LALeg3F TOLeg3B = LALeg3B TOLeg4F = LALeg4F TOLeg4B = LALeg4B TOHeadservo = LAHeadservo # 부드러운 움직임 설정 smoothrun = True smoothdelay = 0.005 # 딜레이를 줄여서 속도 증가 (5ms) # Headservo 움직임을 위한 변수 설정 HeadservoAngle = LAHeadservo # 초기 각도 HeadservoMin = 45 # 최소 각도 HeadservoMax = 135 # 최대 각도 HeadservoStep = 5 # 각도 변화량 HeadservoDirection = 1 # 1: 증가, -1: 감소 # 서보 모터 움직임 함수 def Servomovement(): if smoothrun: smoothmove() else: servos['Leg1F'].duty_u16(angle_to_duty(TOLeg1F)) servos['Leg1B'].duty_u16(angle_to_duty(TOLeg1B)) servos['Leg2F'].duty_u16(angle_to_duty(TOLeg2F)) servos['Leg2B'].duty_u16(angle_to_duty(TOLeg2B)) servos['Leg3F'].duty_u16(angle_to_duty(TOLeg3F)) servos['Leg3B'].duty_u16(angle_to_duty(TOLeg3B)) servos['Leg4F'].duty_u16(angle_to_duty(TOLeg4F)) servos['Leg4B'].duty_u16(angle_to_duty(TOLeg4B)) servos['Headservo'].duty_u16(angle_to_duty(TOHeadservo)) # 부드러운 움직임 구현 함수 def smoothmove(): global LALeg1F, LALeg1B, LALeg2F, LALeg2B, LALeg3F, LALeg3B, LALeg4F, LALeg4B, LAHeadservo maxstep = 0 diffs = [ abs(TOLeg1F - LALeg1F), abs(TOLeg1B - LALeg1B), abs(TOLeg2F - LALeg2F), abs(TOLeg2B - LALeg2B), abs(TOLeg3F - LALeg3F), abs(TOLeg3B - LALeg3B), abs(TOLeg4F - LALeg4F), abs(TOLeg4B - LALeg4B), abs(TOHeadservo - LAHeadservo) ] maxstep = max(diffs) if maxstep == 0: return # 움직임이 필요 없음 steps = int(maxstep) step_Leg1F = (TOLeg1F - LALeg1F) / steps step_Leg1B = (TOLeg1B - LALeg1B) / steps step_Leg2F = (TOLeg2F - LALeg2F) / steps step_Leg2B = (TOLeg2B - LALeg2B) / steps step_Leg3F = (TOLeg3F - LALeg3F) / steps step_Leg3B = (TOLeg3B - LALeg3B) / steps step_Leg4F = (TOLeg4F - LALeg4F) / steps step_Leg4B = (TOLeg4B - LALeg4B) / steps step_Headservo = (TOHeadservo - LAHeadservo) / steps for _ in range(steps): LALeg1F += step_Leg1F LALeg1B += step_Leg1B LALeg2F += step_Leg2F LALeg2B += step_Leg2B LALeg3F += step_Leg3F LALeg3B += step_Leg3B LALeg4F += step_Leg4F LALeg4B += step_Leg4B LAHeadservo += step_Headservo servos['Leg1F'].duty_u16(angle_to_duty(LALeg1F)) servos['Leg1B'].duty_u16(angle_to_duty(LALeg1B)) servos['Leg2F'].duty_u16(angle_to_duty(LALeg2F)) servos['Leg2B'].duty_u16(angle_to_duty(LALeg2B)) servos['Leg3F'].duty_u16(angle_to_duty(LALeg3F)) servos['Leg3B'].duty_u16(angle_to_duty(LALeg3B)) servos['Leg4F'].duty_u16(angle_to_duty(LALeg4F)) servos['Leg4B'].duty_u16(angle_to_duty(LALeg4B)) servos['Headservo'].duty_u16(angle_to_duty(LAHeadservo)) sleep(smoothdelay) # 최종 위치로 설정 LALeg1F = TOLeg1F LALeg1B = TOLeg1B LALeg2F = TOLeg2F LALeg2B = TOLeg2B LALeg3F = TOLeg3F LALeg3B = TOLeg3B LALeg4F = TOLeg4F LALeg4B = TOLeg4B LAHeadservo = TOHeadservo # 움직임 데이터 배열 walkF = [ [124, 146, 177, 150, 132, 115, 115], [94, 132, 178, 139, 112, 84, 84], [37, 112, 179, 139, 95, 42, 42], [22, 95, 150, 115, 78, 30, 30], [11, 78, 124, 92, 59, 13, 13], [13, 59, 92, 58, 36, 2, 2] ] walkB = [ [3, 34, 56, 65, 48, 30, 30], [2, 48, 86, 96, 68, 41, 41], [1, 68, 143, 138, 85, 41, 41], [30, 85, 158, 150, 102, 65, 65], [56, 102, 169, 167, 121, 88, 88], [88, 121, 167, 178, 144, 122, 122] ] Fheight = 5 Bheight = 5 walkstep = 1 # 블루투스 초기화 ble = bluetooth.BLE() sp = BLESimplePeripheral(ble) # 데이터 수신 시 호출되는 함수 def on_rx(data): global motor_running, Fheight, Bheight, walkstep command = data.decode().strip() print("데이터 수신: ", command) if command == 'w': motor_running = True print("전진 시작") elif command == 's': motor_running = False print("정지") elif command == 'a': motor_running = True Fheight = 2 # 좌회전을 위한 움직임 데이터 인덱스 조정 Bheight = 5 print("좌회전 시작") elif command == 'd': motor_running = True Fheight = 5 Bheight = 2 # 우회전을 위한 움직임 데이터 인덱스 조정 print("우회전 시작") else: print("유효하지 않은 명령입니다.") # 블루투스 연결 상태 확인 및 데이터 수신 설정 sp.on_write(on_rx) # 메인 루프 while True: if motor_running: # walkstep 감소 walkstep -= 1 if walkstep < 1: walkstep = 7 walkstep2 = walkstep - 4 if walkstep2 < 1: walkstep2 += 7 # 첫 번째 다리 각도 설정 rotate1 = walkF[Fheight][walkstep - 1] rotate2 = walkB[Fheight][walkstep - 1] rotate3 = walkF[Bheight][walkstep - 1] rotate4 = walkB[Bheight][walkstep - 1] TOLeg1F = rotate1 TOLeg1B = rotate2 TOLeg4F = 180 - rotate3 TOLeg4B = 180 - rotate4 # 두 번째 다리 각도 설정 rotate1_2 = walkF[Fheight][walkstep2 - 1] rotate2_2 = walkB[Fheight][walkstep2 - 1] rotate3_2 = walkF[Bheight][walkstep2 - 1] rotate4_2 = walkB[Bheight][walkstep2 - 1] TOLeg2F = 180 - rotate1_2 TOLeg2B = 180 - rotate2_2 TOLeg3F = rotate3_2 TOLeg3B = rotate4_2 # Headservo 각도 업데이트 HeadservoAngle += HeadservoStep * HeadservoDirection if HeadservoAngle >= HeadservoMax or HeadservoAngle <= HeadservoMin: HeadservoDirection *= -1 # 방향 반전 HeadservoAngle += HeadservoStep * HeadservoDirection # 각도 조정 TOHeadservo = HeadservoAngle # 목표 각도로 설정 Servomovement() sleep(0.01) # 필요에 따라 딜레이 조정 else: # 정지 상태에서는 현재 위치 유지 sleep(0.1) # 블루투스 데이터 수신 처리 if sp.is_connected(): sp.on_write(on_rx) utime.sleep(0.01)