樹莓派超音波自動駕駛

超音波自動駕駛

import RPi.GPIO as GPIO
import time

# 定義超音波感測器的引腳
TRIG = 20
ECHO = 21

# 定義馬達控制引腳
PWM_R = 18  # 右輪馬達 PWM
DIR1_R = 23  # 右輪方向 1
DIR2_R = 24  # 右輪方向 2

PWM_L = 13  # 左輪馬達 PWM
DIR1_L = 5  # 左輪方向 1
DIR2_L = 6  # 左輪方向 2

# 初始化 GPIO 設置
GPIO.setmode(GPIO.BCM)
GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(PWM_R, GPIO.OUT)
GPIO.setup(DIR1_R, GPIO.OUT)
GPIO.setup(DIR2_R, GPIO.OUT)
GPIO.setup(PWM_L, GPIO.OUT)
GPIO.setup(DIR1_L, GPIO.OUT)
GPIO.setup(DIR2_L, GPIO.OUT)

# 初始化 PWM
pwm_right = GPIO.PWM(PWM_R, 1000)  # 右輪 PWM 設置為 1000Hz
pwm_left = GPIO.PWM(PWM_L, 1000)   # 左輪 PWM 設置為 1000Hz
pwm_right.start(0)  # 開始時關閉馬達
pwm_left.start(0)

# 設置馬達的方向
def set_motor_direction(motor, forward=True):
    if motor == 'right':
        GPIO.output(DIR1_R, GPIO.HIGH if forward else GPIO.LOW)
        GPIO.output(DIR2_R, GPIO.LOW if forward else GPIO.HIGH)
    elif motor == 'left':
        GPIO.output(DIR1_L, GPIO.HIGH if forward else GPIO.LOW)
        GPIO.output(DIR2_L, GPIO.LOW if forward else GPIO.HIGH)

# 設置馬達的速度
def set_motor_speed(motor, speed):
    if motor == 'right':
        pwm_right.ChangeDutyCycle(speed)
    elif motor == 'left':
        pwm_left.ChangeDutyCycle(speed)

# 停止馬達
def stop_motors():
    pwm_right.ChangeDutyCycle(0)
    pwm_left.ChangeDutyCycle(0)

# 使用超音波感測器檢測距離
def get_distance():
    # 發送超音波信號
    GPIO.output(TRIG, True)
    time.sleep(0.00001)
    GPIO.output(TRIG, False)

    # 計時回波信號
    while GPIO.input(ECHO) == 0:
        pulse_start = time.time()

    while GPIO.input(ECHO) == 1:
        pulse_end = time.time()

    # 計算距離
    pulse_duration = pulse_end - pulse_start
    distance = pulse_duration * 17150  # 乘以聲音速度的係數，單位為 cm
    distance = round(distance, 2)

    return distance

# 主程序邏輯
try:
    while True:
        # 取得障礙物的距離
        distance = get_distance()
        print(f"距離障礙物: {distance} cm")

        if distance < 30:
            # 若距離小於 30 cm，停止並向後移動
            print("障礙物太近，倒車中...")
            set_motor_direction('right', forward=False)
            set_motor_direction('left', forward=False)
            set_motor_speed('right', 50)
            set_motor_speed('left', 50)
            time.sleep(2)
            stop_motors()

            # 嘗試轉向避開障礙物
            print("左轉避開障礙物")
            set_motor_direction('right', forward=True)
            set_motor_direction('left', forward=False)
            set_motor_speed('right', 50)
            set_motor_speed('left', 50)
            time.sleep(1.5)
            stop_motors()
        else:
            # 若距離足夠，繼續前進
            print("路徑清楚，繼續前進...")
            set_motor_direction('right', forward=True)
            set_motor_direction('left', forward=True)
            set_motor_speed('right', 60)
            set_motor_speed('left', 60)

        time.sleep(0.1)

except KeyboardInterrupt:
    print("停止自動駕駛程序")
    stop_motors()
    GPIO.cleanup()

程式說明：
超音波感測器（HC-SR04）：

TRIG：觸發引腳，控制發送超音波。
ECHO：接收引腳，用來檢測返回的回波信號並計算距離。
馬達控制：

兩個馬達分別控制左右輪，使用兩個方向控制引腳 (DIR1 和 DIR2) 來決定馬達的方向（前進或倒退），使用 PWM 控制馬達的速度。
避障邏輯：

程式不斷檢測前方的障礙物距離。如果距離小於 30 cm，則馬達倒車並嘗試左轉避開障礙物。
如果路徑清楚，則馬達繼續前進。
主循環：

程式會持續檢測距離，根據距離數據決定車輛的運動（前進、後退、轉彎）。
硬件接線：
超音波感測器：

TRIG 連接到 GPIO 20
ECHO 連接到 GPIO 21
馬達驅動器（如 L298N）：

右輪的 PWM 控制連接到 GPIO 18，兩個方向控制引腳連接到 GPIO 23 和 GPIO 24。
左輪的 PWM 控制連接到 GPIO 13，兩個方向控制引腳連接到 GPIO 5 和 GPIO 6。
延伸功能：
攝像頭或其他感測器：可以添加 Pi Camera 或其他感測器進行圖像處理或更精細的導航。
更複雜的演算法：可以使用 A* 或 Dijkstra 等路徑規劃演算法進行更智能的導航。
此程式為基礎的避障自動駕駛系統示例，適合用於學習和測試 Raspberry Pi 上的自動駕駛應用。如果要實現更加高級的自動駕駛系統，則需要引入更多的感測器與複雜的控制算法。