M5BALAに超音波距離センサーを載せて障害物回避

#概要

M5BALAに超音波距離センサーを載せて障害物を検知し、右折して障害物を回避します。

• M5Stack FIRE に超音波距離センサーを接続
• 前方に障害物を検知し、設定した距離より近い場合は右折して障害物を回避
• 右折時はジャイロのＺ軸の回転角を参照して（ほぼ）９０度回転

##環境

• M5BALA + M5Stack FIRE
• M5UI.Flow (Firmware 0.9.0 で確認)
• 超音波距離センサー HC-SR04

#実行例

M5BALAと遊ぼう、今回は超音波距離センサーとの組合せ。障害物回避というよりは、イジメみたい？ 障害物を検知したら90度回転するようにしてみましたが回転角がずれるのでため、そのうち逃げ出します。
「M5BALAに超音波距離センサーを載せて障害物回避」https://t.co/vK7j1ViFeY#M5Stack #M5BALA pic.twitter.com/t9f5YgHr9Y

— 稲澤祐一 (@inasawa) 2018年11月12日

#超音波距離センサーの実装

超音波距離センサーをミニブレッドボードを使って PORT B に接続し、ブレッドボードを M5Stack FIRE の上に載せて輪ゴムで固定しました。

前	後

PORT B と HC-SR04 の接続は以下の通りです。

PORT B	HC-SR04
黒■（GND）	Gnd
赤■（5V）	Vcc
白■（G26）	Trig
黄■（G36）	Echo

接続時の注意点として、Grove に慣れている方は問題ないかもしれませんが、以下の左側の写真のように M5Stack FIRE 付属の説明書にはケーブルの色が左から、黒、赤、黄色、白と書かれており、M5GO付属のケーブルも実際その色の並びになっています。しかしながら、本来のGrove のケーブルは、黒、赤、白、黄色の順であり、今回接続用に購入したジャンパーピン変換ケーブルも黒、赤、白、黄色の順でした。私は最初、説明書を見て黄色が Pin 26、白が Pin 36 と思って接続したため、しばらく距離が取得できず悩むことになりました。

M5Stack Fire の説明書 M5GO付属のケーブル	ジャンパーピン変換ケーブル
黒、赤、黄色、白の順	黒、赤、白、黄色の順

#M5BALA制御用プログラム

以下のプログラムを M5UI.Flow から実行しました。

m5bala_hcsr04.py

from m5stack import *
from m5bala import M5Bala
import i2c_bus
import machine

# PORT B に接続する場合のピン定義
TRIGGER_Pin = 26
ECHO_Pin    = 36

# 距離を測定する時間間隔
DISTANCE_INTERVAL = 250 # ms

# 正面の障害物が以下の距離より近くなったら避ける
TURN_DISTANCE = 20 # cm この距離以内に障害物があれば避ける
TURN_ANGLE    = 80 # 度 回りたいのは90度だが、回りすぎるので控えめに設定

# 左右の動き＆前進のためのパラメータは実際の動きに合わせて要調整。
# 障害物に近づいたら右折する。
TURN = [15, -25]

# 障害物がなければ前進する。
# 値は 0 だが、重心が前にあるようにしているので前進する。
MOVE = [0,    0]

# M5BALA
m5bala = M5Bala(i2c_bus.get(i2c_bus.M_BUS))

# 距離センサー
trigger = machine.Pin(TRIGGER_Pin, machine.Pin.OUT)
trigger.value(0)
echo = machine.Pin(ECHO_Pin, machine.Pin.IN)

# LED
ledbar = machine.Neopixel(machine.Pin(15), 10)
ledbar.brightness(0)

distance_timer = 0
turn_in_progress = False
while True:
    # 障害物を見つけたら指定角度まで回転する
    if turn_in_progress:
        if m5bala.imu.angleZ < target_angle:
            turn_in_progress = False
            m5bala.left, m5bala.right = MOVE
            # 回転が終わったらLEDを消灯
            ledbar.set(1, lcd.BLACK, num=10)
    else:
        # 指定間隔で正面方向の距離を測定する
        if time.ticks_diff(time.ticks_ms(), distance_timer) > 0:
            distance_timer = time.ticks_add(time.ticks_ms(), DISTANCE_INTERVAL)
            
            # トリガーピンを 10us HIGH にすることで超音波を送信
            trigger.value(1)
            time.sleep_us(10)
            trigger.value(0)
            
            # エコーピンが HIGH になっている時間が超音波の応答までの時間
            # 応答にかかった時間から距離を計算する
            # 前方の障害物を判定するのでタイムアウトは短め
            # 10000us = 340cm / 2 = 170cm までの距離測定
            pulse_time = machine.time_pulse_us(echo, 1, 10000) 
            if pulse_time > 0:
                # 障害物までの距離(cm)を計算する
                distance = (pulse_time / 2) * 0.034 # cm
                if distance < TURN_DISTANCE:
                    # 障害物を検知したらLEDを点灯
                    ledbar.set(1, 0x070707, num=10)
                    turn_in_progress = True
                    target_angle = m5bala.imu.angleZ - TURN_ANGLE
                    m5bala.left, m5bala.right = TURN
                else:
                    m5bala.left, m5bala.right = MOVE
        
    m5bala.balance()

#おまけ

超音波距離センサーを横向きにして、バケツの周りを回るM5BALA。#M5Stack #M5BALA pic.twitter.com/eQ9eZW1Ozr

— 稲澤祐一 (@inasawa) 2018年11月15日