【Poliviabot】【Raspberry Pi Pico】ロボット制御入門⑥超音波センサで「距離」を測る【Python】

本サイトでは様々な環境で気軽に本格的なロボット教育を行ってもらうべく、独自のロボット教材を開発しました。

現在、一般販売へ向けて準備をしております。

本Webページではβ版の先行体験をしていただく方向けに公開している資料になっております。

一般の方は販売をお待ちください。

前回はこちらです

: 【Poliviabot】【Raspberry Pi Pico】ロボット制御入門③デジタル出力とPython入門【Python】
本サイトでは様々な環境で気軽に本格的なロボット教育を行ってもらうべく、独自のロボット教材を開発しました。現在、一般販売へ向けて準備をしております。本Webページではβ版の先行体験をしていただく方向 ...

1 この章の目的

この章では、PoliviaBot UMEに搭載された超音波センサを使って、

超音波で距離が測れる仕組み
Trig（送信）とEcho（受信）の役割
Pythonで「時間」を測り、「距離」に変換する方法
距離に応じてロボットの動作を変える考え方

を学びます。

最終的には、

ロボットが「近い」「遠い」を判断できる
状態を目指します。

2 超音波センサとは何か

超音波センサは、**人には聞こえない高い音（約40kHz）**を使って距離を測ります。

仕組みはとてもシンプルです。

センサが「ピッ」と音を出す（送信）
音が物に当たって跳ね返る
戻ってくるまでの時間を測る
時間から距離を計算する

これは、コウモリやイルカが周囲を認識する方法と同じです。

3 PoliviaBot UMEでのピン構成

この教材では、次のピンを使います。

Trig（送信）：GP7
Echo（受信）：GP6

役割は次の通りです。

ピン	役割	向き
Trig	超音波を出せ、という命令	出力
Echo	戻ってきた時間を知らせる	入力

4 距離が「時間」で決まる理由

音の速さは、空気中で約343 m/s（20℃付近）です。

センサは、
行き → 反射 → 帰り
の往復時間を測っています。

つまり、

距離 = (音速 × 時間) ÷ 2

となります。

5 Pythonで時間を測る準備

MicroPythonでは、マイクロ秒単位で時間を測ることができます。

よく使う関数：

time.ticks_us() → 現在の時刻（μs）
time.ticks_diff(a, b) → 時刻の差を安全に計算

6 TrigとEchoの動かし方

測定の流れは、毎回この順番です。

TrigをLOWにする（準備）
Trigを10μsだけHIGHにする（発射）
EchoがHIGHになるまで待つ
EchoがLOWになるまでの時間を測る

7 最初の距離測定プログラム

まずは、距離を数字で表示するだけのテストです。

from machine import Pin
import time

TRIG = Pin(7, Pin.OUT)
ECHO = Pin(6, Pin.IN)

def get_distance_cm():
    # 発射準備
    TRIG.value(0)
    time.sleep_us(2)

    # 10μsパルスを出す
    TRIG.value(1)
    time.sleep_us(10)
    TRIG.value(0)

    # EchoがHIGHになるまで待つ
    while ECHO.value() == 0:
        pass
    start = time.ticks_us()

    # EchoがLOWになるまで待つ
    while ECHO.value() == 1:
        pass
    end = time.ticks_us()

    # 時間差（μs）
    duration = time.ticks_diff(end, start)

    # 距離計算（cm）
    distance = (duration * 0.0343) / 2
    return distance

while True:
    d = get_distance_cm()
    print("Distance: {:.1f} cm".format(d))
    time.sleep(0.5)

重要なポイント

time.sleep_us(10)
→ 超音波を出す「合図」の長さ
while ECHO.value() == 0:
→ 返ってくるのを待つ
duration
→ 音が往復した時間

8 距離でLEDを制御する

距離が近いとLED1が点灯するようにします。

20cm未満 → LED1 ON
20cm以上 → LED1 OFF

from machine import Pin
import time

TRIG = Pin(7, Pin.OUT)
ECHO = Pin(6, Pin.IN)
LED1 = Pin(11, Pin.OUT)

def get_distance_cm():
    TRIG.value(0)
    time.sleep_us(2)

    TRIG.value(1)
    time.sleep_us(10)
    TRIG.value(0)

    while ECHO.value() == 0:
        pass
    start = time.ticks_us()

    while ECHO.value() == 1:
        pass
    end = time.ticks_us()

    duration = time.ticks_diff(end, start)
    return (duration * 0.0343) / 2

while True:
    d = get_distance_cm()
    if d < 20:
        LED1.value(1)
    else:
        LED1.value(0)

    print("Distance:", d)
    time.sleep(0.2)

超音波センサで動作を作る

以降の課題は、この関数で距離(cm)を取得します。
※無限待ち防止のためタイムアウトを入れています（実機で止まりにくくするため）。

共通：距離取得関数

from machine import Pin
import time

TRIG = Pin(7, Pin.OUT)
ECHO = Pin(6, Pin.IN)

SOUND_SPEED = 0.0343  # cm/us（約343m/s）

def get_distance_cm(timeout_us=30000):
    # Trig pulse
    TRIG.value(0)
    time.sleep_us(2)
    TRIG.value(1)
    time.sleep_us(10)
    TRIG.value(0)

    # wait for echo high
    t0 = time.ticks_us()
    while ECHO.value() == 0:
        if time.ticks_diff(time.ticks_us(), t0) > timeout_us:
            return None  # timeout

    start = time.ticks_us()

    # wait for echo low
    while ECHO.value() == 1:
        if time.ticks_diff(time.ticks_us(), start) > timeout_us:
            return None  # timeout

    end = time.ticks_us()
    duration = time.ticks_diff(end, start)  # us

    distance = (duration * SOUND_SPEED) / 2
    return distance

モータ制御（共通関数）

from machine import Pin

L_P = Pin(0, Pin.OUT)  # Motor_L+
L_M = Pin(1, Pin.OUT)  # Motor_L-
R_P = Pin(2, Pin.OUT)  # Motor_R+
R_M = Pin(3, Pin.OUT)  # Motor_R-

def stop():
    L_P.value(0); L_M.value(0)
    R_P.value(0); R_M.value(0)

def forward():
    L_P.value(1); L_M.value(0)
    R_P.value(1); R_M.value(0)

def turn_right():
    # 簡易：左だけ前進、右停止（右旋回）
    L_P.value(1); L_M.value(0)
    R_P.value(0); R_M.value(0)

def turn_left():
    # 簡易：右だけ前進、左停止（左旋回）
    L_P.value(0); L_M.value(0)
    R_P.value(1); R_M.value(0)

課題 9

障害物が近くなったら停止するプログラムを作成してください。

「距離がしきい値より小さい → 止まる」
という if文の基本で実現できます。
まずは安全装置として最重要の機能です。

距離が 20cm未満 → 停止
それ以外 → 前進

解答例はこちら

from machine import Pin
import time

# --- Ultrasonic ---
TRIG = Pin(7, Pin.OUT)
ECHO = Pin(6, Pin.IN)
SOUND_SPEED = 0.0343  # cm/us

def get_distance_cm(timeout_us=30000):
    TRIG.value(0); time.sleep_us(2)
    TRIG.value(1); time.sleep_us(10)
    TRIG.value(0)

    t0 = time.ticks_us()
    while ECHO.value() == 0:
        if time.ticks_diff(time.ticks_us(), t0) > timeout_us:
            return None
    start = time.ticks_us()

    while ECHO.value() == 1:
        if time.ticks_diff(time.ticks_us(), start) > timeout_us:
            return None
    end = time.ticks_us()

    duration = time.ticks_diff(end, start)
    return (duration * SOUND_SPEED) / 2

# --- Motors (ON/OFF) ---
L_P = Pin(0, Pin.OUT); L_M = Pin(1, Pin.OUT)
R_P = Pin(2, Pin.OUT); R_M = Pin(3, Pin.OUT)

def stop():
    L_P.value(0); L_M.value(0)
    R_P.value(0); R_M.value(0)

def forward():
    L_P.value(1); L_M.value(0)
    R_P.value(1); R_M.value(0)

THRESH = 20  # cm

while True:
    d = get_distance_cm()
    if d is None:
        # センサ異常時は安全に停止
        stop()
        print("Distance: None -> STOP")
    else:
        print("Distance:", round(d, 1), "cm")
        if d < THRESH:
            stop()
        else:
            forward()

    time.sleep(0.1)

課題10

障害物との距離を10cmに保つプログラムを作成してください。

目標距離（10cm）より、

遠い → 少し前進
近い → 少し後退
ちょうど良い → 停止

ここでは初心者向けに、まずはON/OFFで「段階制御」します。

目標値

目標：10cm
許容範囲：±2cm（8〜12cmはOK）

解答例はこちら

from machine import Pin
import time

# --- Ultrasonic ---
TRIG = Pin(7, Pin.OUT)
ECHO = Pin(6, Pin.IN)
SOUND_SPEED = 0.0343  # cm/us

def get_distance_cm(timeout_us=30000):
    TRIG.value(0); time.sleep_us(2)
    TRIG.value(1); time.sleep_us(10)
    TRIG.value(0)

    t0 = time.ticks_us()
    while ECHO.value() == 0:
        if time.ticks_diff(time.ticks_us(), t0) > timeout_us:
            return None
    start = time.ticks_us()

    while ECHO.value() == 1:
        if time.ticks_diff(time.ticks_us(), start) > timeout_us:
            return None
    end = time.ticks_us()

    dur = time.ticks_diff(end, start)
    return (dur * SOUND_SPEED) / 2

# --- Motors (ON/OFF) ---
L_P = Pin(0, Pin.OUT); L_M = Pin(1, Pin.OUT)
R_P = Pin(2, Pin.OUT); R_M = Pin(3, Pin.OUT)

def stop():
    L_P.value(0); L_M.value(0)
    R_P.value(0); R_M.value(0)

def forward():
    L_P.value(1); L_M.value(0)
    R_P.value(1); R_M.value(0)

def reverse():
    L_P.value(0); L_M.value(1)
    R_P.value(0); R_M.value(1)

TARGET = 10.0
BAND = 2.0   # 許容範囲 ±2cm

while True:
    d = get_distance_cm()
    if d is None:
        stop()
        print("Distance: None -> STOP")
        time.sleep(0.1)
        continue

    d = float(d)
    print("Distance:", round(d, 1), "cm")

    if d > TARGET + BAND:
        # 遠い → 前進
        forward()
    elif d < TARGET - BAND:
        # 近すぎ → 後退
        reverse()
    else:
        # ちょうど良い → 停止
        stop()

    time.sleep(0.1)

課題11

前進しながら、障害物を回避するプログラムを作成してください。

この課題では、距離が危険域に入ったら、その場で旋回して向きを変えるというシンプルで実用的な回避方法を実装します。

動作ルールは次の通りです。

30cm以上 → 前進
30cm未満 → 停止 → その場旋回 → 再測定
旋回後、距離が十分なら前進に戻る

「減速」は使わず、前進か回避（旋回）かの2状態で制御します。
これは、ロボット制御でよく使われる状態遷移（ステート）設計の基本形です。

ここでは「距離が近いときだけ旋回して逃げる」簡易版を作ってみましょう。

解答例はこちら

from machine import Pin
import time

# --- Ultrasonic ---
TRIG = Pin(7, Pin.OUT)
ECHO = Pin(6, Pin.IN)
SOUND_SPEED = 0.0343  # cm/us

def get_distance_cm(timeout_us=30000):
    TRIG.value(0); time.sleep_us(2)
    TRIG.value(1); time.sleep_us(10)
    TRIG.value(0)

    t0 = time.ticks_us()
    while ECHO.value() == 0:
        if time.ticks_diff(time.ticks_us(), t0) > timeout_us:
            return None
    start = time.ticks_us()

    while ECHO.value() == 1:
        if time.ticks_diff(time.ticks_us(), start) > timeout_us:
            return None
    end = time.ticks_us()

    dur = time.ticks_diff(end, start)
    return (dur * SOUND_SPEED) / 2

# --- Motors (ON/OFF) ---
L_P = Pin(0, Pin.OUT); L_M = Pin(1, Pin.OUT)
R_P = Pin(2, Pin.OUT); R_M = Pin(3, Pin.OUT)

def stop():
    L_P.value(0); L_M.value(0)
    R_P.value(0); R_M.value(0)

def forward():
    L_P.value(1); L_M.value(0)
    R_P.value(1); R_M.value(0)

def spin_right():
    """
    その場右旋回
    左モータ：正転
    右モータ：逆転
    """
    L_P.value(1); L_M.value(0)
    R_P.value(0); R_M.value(1)

SAFE = 30  # cm これ以上なら前進

while True:
    d = get_distance_cm()
    if d is None:
        stop()
        print("Distance: None -> STOP")
        time.sleep(0.2)
        continue

    d = float(d)
    print("Distance:", round(d, 1), "cm")

    if d >= SAFE:
        # 安全：前進
        forward()
    else:
        # 危険：停止 → その場旋回 → 再測定
        stop()
        time.sleep(0.1)

        spin_right()
        time.sleep(0.5)  # 向きを変える時間
        stop()
        time.sleep(0.1)

    time.sleep(0.05)

次回はこちら

: 【Poliviabot】【Raspberry Pi Pico】ロボット制御入門⑦I2C通信とSSD1306表示【Python】
本サイトでは様々な環境で気軽に本格的なロボット教育を行ってもらうべく、独自のロボット教材を開発しました。現在、一般販売へ向けて準備をしております。本Webページではβ版の先行体験をしていただく方向 ...

Follow me!