Python Raspberry Pi Pico プログラム 回路 電子工作 高専3年生

【Raspberry Pi Pico】ロボット制御入門③~障害物回避~【Python】

前回はライブラリの作成について学びました。

今回はセンサー入力を使って自動制御の一旦を体験してみましょう。

超音波センサとは

超音波センサーは、その名前の通り、音の一種である「超音波」を使って周囲の物体を感知する電子部品です。

超音波センサーには「発信部(Trigger)」と「受信部(Echo)」があります。

まず、発信部から超音波が発射されます。

この音波が物体にぶつかると跳ね返り、受信部でキャッチされます。

このとき、超音波が発信されてから受信されるまでの時間を計測することで、物体までの距離を測ることができます。

(距離 = 超音波の速度 × 測定時間)

超音波センサの使い方

音波センサーの理論をプログラムで実践してみましょう。

以下のコードをPythonファイルとして保存し、実行してみてください。

from machine import Pin  # machineモジュールからPin機能をインポート
import utime  # 時間を扱うためにutimeモジュールをインポート

# 超音波センサーのトリガー(発信)とエコー(受信)用のピンを設定
trigger = Pin(5, Pin.OUT)  # GPIO 5番ピンをトリガーとして設定
echo = Pin(4, Pin.IN)  # GPIO 4番ピンをエコーとして設定

# 距離を測定する関数を定義
def read_distance():
    trigger.low()  # トリガーをLowにして準備
    utime.sleep_us(2)  # 2マイクロ秒待機
    trigger.high()  # トリガーをHighにして超音波を発信
    utime.sleep(0.00001)  # 10マイクロ秒間超音波を発信
    trigger.low()  # 再度トリガーをLowにする
    
    # エコーが反応を返すまでの時間を計測
    while echo.value() == 0:  # エコー信号が来るのを待つ
        signaloff = utime.ticks_us()  # 反応なしの時間を記録
    
    while echo.value() == 1:  # エコー信号が来るのを待つ
        signalon = utime.ticks_us()  # 反応が返ってきた時間を記録

    # 反応時間を計算し、距離を求める
    timepassed = float(signalon) - float(signaloff)
    distance = (timepassed * 0.0343) / 2  # 距離を計算(音速は約0.0343 cm/μs、片道なので2で割る)
    
    # 測定した距離を表示
    print("Distance: ", distance, "cm")

# メインループで繰り返し距離を測定
while True:
    read_distance()  # 距離を測定
    utime.sleep_ms(50)  # 50ミリ秒待機

RasPiPicoより検出した距離のデータがPCへ送られてくると思います。

解説してゆきます。

超音波センサーのピン設定

trigger = Pin(5, Pin.OUT)  # GPIO 5番ピンをトリガー(出力)として設定
echo = Pin(4, Pin.IN)  # GPIO 4番ピンをエコー(入力)として設定
  • triggerは超音波センサーの発信部を制御するためのピンで、超音波を発信する役割を持っています。
  • echoは超音波が物体に当たって跳ね返ってきた信号を受信するためのピンです。

距離を測定する関数

def read_distance():
    trigger.low()  # トリガーピンをLowに設定して準備
    utime.sleep_us(2)  # 2マイクロ秒待機
    trigger.high()  # トリガーピンをHighにして超音波を発信
    utime.sleep(0.00001)  # 10マイクロ秒間超音波を発信
    trigger.low()  # トリガーピンをLowに戻す
  • 超音波センサーの発信部(トリガー)を制御します。trigger.low()で超音波を出さない状態にした後、trigger.high()で10マイクロ秒間超音波を発信します。その後、trigger.low()に戻すことで発信を停止します。この短い信号で物体に向かって超音波を送信します。
    while echo.value() == 0:  # エコーが0(Low)の間待機
        signaloff = utime.ticks_us()  # エコー信号が来ない間の時間を記録
  • echoが0の間、信号が届かない時間をsignaloffに記録します。これは超音波がまだ反射して返ってきていないことを示します。
    while echo.value() == 1:  # エコーが1(High)の間待機
        signalon = utime.ticks_us()  # エコー信号が来た時間を記録
  • echoが1になると、超音波が物体に反射して戻ってきたことを示します。このときの時間をsignalonに記録します。

距離の計算

    timepassed = float(signalon) - float(signaloff)  # エコーが届くまでの時間差を計算
    distance = (timepassed * 0.0343) / 2  # 超音波が往復した時間を元に距離を計算(音速: 0.0343 cm/μs)
  • timepassedは、超音波が物体に跳ね返ってエコーに戻ってくるまでの時間を計算します。
  • 距離の計算には、音速(約0.0343 cm/マイクロ秒)を使い、往復した時間を片道の時間にするために2で割ります。

測定結果の表示

    print("Distance: ", distance, "cm")
  • 計算した距離をセンチメートル単位で表示します。

メインループ

while True:
    read_distance()  # 距離を測定する関数を呼び出す
    utime.sleep_ms(50)  # 50ミリ秒待機してから次の測定を行う
  • while Trueループで、read_distance()関数を繰り返し呼び出して距離を測定します。各測定の間に50ミリ秒の待機を入れて、次の測定を行います。

障害物回避

今回は超音波センサを使った、ロボットの障害物回避に挑戦してみましょう。

実現するにはどうすればいいでしょうか。

まずは物体との距離が一定以上に近づいたことを検知できるようにしてみましょう。

以下のようにWhileの中身に検出するための条件分岐を追加します。

from machine import Pin
import utime

# トリガーとエコーのピン設定
trigger = Pin(5, Pin.OUT)
echo = Pin(4, Pin.IN)

def read_distance():
    # トリガーを操作して距離を測定するコード
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep(0.00001)
    trigger.low()

    while echo.value() == 0:
        signaloff = utime.ticks_us()
    while echo.value() == 1:
        signalon = utime.ticks_us()

    timepassed = float(signalon) - float(signaloff)
    distance = (timepassed * 0.0343) / 2  # 距離計算(cm単位)

    print("Distance: ", distance, "cm")
    return distance

# 一定距離のしきい値を設定
const_distance = 20  # 単位はセンチメートル

while True:
    distance = read_distance()  # 距離を測定
    utime.sleep_ms(50)
    # 条件分岐を追加
    if distance < const_distance:
        print("近づきすぎです!")

説明

  1. 定数の設定
    • const_distanceという変数を用意し、しきい値として距離を設定しました。これにより、コード内で距離を変更したい場合はこの変数だけを変更すれば済みます。
    • 定数を使うことで、コードが長くなっても値を見つけやすく、変更もしやすくなります。
  2. 条件分岐
    • if distance < const_distance:の部分で、物体が一定の距離以内に近づいた場合に「近づきすぎです!」と出力します。これにより、ロボットが障害物に接近しすぎた場合に警告を出すことができます。

それではロボットが近くに障害物を検出した場合、90度旋回するプログラムを作ってゆきます。

90度回転するためにロボットを制御しよう

ロボットを90度回転させるためには、どれくらいモーターを回せば良いのでしょうか?まずは基本的な考え方を説明します。

ロボットが回転する際には、左右のモーターの動きによって動作が変わります。

以下の図のように、90度回転するためには左側のモーターがある距離だけ進む必要があります。

具体的な計算としては、左側のモーターが2π * (x/2) / 4 [mm]、右側のモーターが-πx/4 [cm]だけ動く必要があります。

しかし、実際にプログラムで距離を直接制御することはできません。

私たちが制御できるのは、PWM(モーターの速度を調整するもの)や動かす時間の設定です。

そのため、まずはロボットが1秒間でどれくらい進むかを調べてみましょう。

これによって、必要な距離を動かすための時間がわかります。

1秒間に進む距離を測るプログラム

次のプログラムを使って、ロボットが1秒間にどれくらい進むかを測定してみましょう。

以前作成したRobotクラスを使用してロボットを前進させます。

# 作成したライブラリからRobotクラスをインポート
from motor import Robot
import utime

# Robotクラスのインスタンスを作成
robot = Robot()

# 計測のための時間を初期化
init_time = utime.ticks_us()
now_time = utime.ticks_us()

try:
    while now_time - init_time < 1000000:  # 1秒間(1000000マイクロ秒)だけ動作
        now_time = utime.ticks_us()
        # ロボットを前進させる
        robot.forward()
finally:
    # 動作終了時にロボットを停止
    robot.stop()

測定の手順

  1. このプログラムを実行すると、ロボットは1秒間だけ前進します。
  2. 実行後に、ロボットが進んだ距離を定規やメジャーで測定してください。単位はミリメートル(mm)で記録すると便利です。
  3. 測定した距離から、1秒間にどれくらい進むかを知ることができます。

測定結果を使った計算

  • 測定した距離を使って、ロボットが90度回転するために必要な時間を計算できます。
  • 例えば、1秒で100mm進んだ場合、90度回転に必要な距離が50mmであれば、0.5秒(500ms)でその距離を進むことがわかります。

このようにして、ロボットが正確に回転するように時間やモーターの動作を調整していきます。

ここで使っているutime.ticks_us()という関数は、RPi Picoに電源が入ってからの経過時間をマイクロ秒単位で返してくれるものです。

以前の距離を測定する関数でも使っていました。

この関数の目的は経過時間を測ることで、今回は1秒(=1000000マイクロ秒)を測るために使いました。

もしticks_us()ticks_ms()に変えると、ミリ秒単位で経過時間を測定するようになります。

距離と時間の関係を使った計算

ロボットが1秒間に進んだ距離をyとします。これは次の式で表されます。

1 [秒] × A [mm/s] = y [mm]
⇔ A = y [mm] / 1 [秒]

ここで、Aはロボットが1秒間に進む速度です。

この速度Aを使って、必要な回転時間tを計算します。

A × t = πx / 4
⇔ t = (πx / 4) / A

この計算から得られた時間だけ、right()もしくはleft()の関数を使ってロボットを動かすことで、90度回転を実現できます。

計算が正しいかどうか確認するために、次のサンプルプログラムを実行してみましょう。

xxxxxxxの部分には、先ほど求めたtをマイクロ秒に変換した値(t * 10^6)を入れてください。

# 作成したライブラリからRobotクラスをインポート
from motor import Robot
import utime

# Robotクラスのインスタンスを作成
robot = Robot()

# 時間を初期化
init_time = utime.ticks_us()
now_time = utime.ticks_us()
print(init_time)

try:
    while now_time - init_time < xxxxxxxx:  # xxxxxxxxに計算した時間(マイクロ秒単位)を入れる
        now_time = utime.ticks_us()
        # 右回転にする場合はrobot.right()に変更してもOK
        robot.left()
finally:
    # 停止
    robot.stop()

それではこれを障害物検知と組み合わせてみましょう。やることはプログラムを合体させるだけです。

障害物検知と90度回転を組み合わせたロボット制御プログラム

このプログラムでは、ロボットが超音波センサーを使って前方の障害物までの距離を測定し、一定の距離よりも近づいた場合に90度回転して障害物を避けるように動作します。

from machine import Pin  # RPi Picoのピンを制御するためのモジュール
from motor import Robot  # ロボットを制御するための自作ライブラリ
import utime, time  # 時間管理用のモジュール

# 超音波センサーの設定
trigger = Pin(5, Pin.OUT)  # トリガーピン(超音波の発信)
echo = Pin(4, Pin.IN)  # エコーピン(超音波の受信)

# 障害物検知のしきい値(距離:20cm)
const_distance = 20
time.sleep(1)  # 初期化のための1秒待機
print("システム準備完了")

# ロボットのインスタンスを作成
robot = Robot()

# 超音波センサーで距離を測定する関数
def read_distance():
    trigger.low()  # トリガーをLowに設定
    utime.sleep_us(2)  # 2マイクロ秒待機
    trigger.high()  # トリガーをHighに設定して超音波を発信
    utime.sleep(0.00001)  # 10マイクロ秒間超音波を発信
    trigger.low()  # トリガーをLowに戻す

    # 反射した超音波を受け取る時間を記録
    while echo.value() == 0:  # 反射波が来るまで待機
        signaloff = utime.ticks_us()
    while echo.value() == 1:  # 反射波を受け取るまでの時間を記録
        signalon = utime.ticks_us()

    # 距離を計算(音速に基づく)
    timepassed = float(signalon) - float(signaloff)
    distance = (timepassed * 0.0343) / 2  # 距離を計算(単位: cm)
    print("距離: ", distance, "cm")
    return distance

# メインループ
try:
    while True:
        utime.sleep_ms(50)  # 少し待機
        robot.forward()  # ロボットを前進させる
        distance = read_distance()  # 距離を測定する

        # 距離がしきい値より小さい場合(障害物が近い場合)
        if distance < const_distance:
            print("障害物を検知!")
            robot.stop()  # ロボットを停止
            utime.sleep_ms(10)  # 少し待機

            # ロボットを左に90度回転
            init_time = utime.ticks_us()
            now_time = utime.ticks_us()
            while now_time - init_time < 1104000:  # 1.104秒間回転
                now_time = utime.ticks_us()
                robot.left()  # 左回転

finally:
    robot.stop()  # プログラム終了時にロボットを停止

課題

障害物を検出したら90度右旋回し、1秒進んだら90度左旋回をし、元の進行方向へ戻るプログラムを作成してください。

次回はこちら

Follow me!

-Python, Raspberry Pi Pico, プログラム, 回路, 電子工作, 高専3年生
-, , , , ,

PAGE TOP