서보모터 테스트 회로 구성도

• 3번, 5번 gpio에 서보모터 연결

코드 다운로드

pico_bluetooth_servo.zip

• main.py 코드

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 사용량을 줄이기 위한 짧은 지연

• 코드 출처

ChatGPT

활용 예시

라즈베리파이 피코 블루투스 RC카 만들기(내장 통신모듈 이용 BLE)

컨트롤러(안드로이드 앱)

4족 pico 로봇 블루투스 통신코드

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)