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

#概要

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

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

##環境

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

#実行例

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">M5BALAと遊ぼう、今回は超音波距離センサーとの組合せ。障害物回避というよりは、イジメみたい？ 障害物を検知したら90度回転するようにしてみましたが回転角がずれるのでため、そのうち逃げ出します。<br>「M5BALAに超音波距離センサーを載せて障害物回避」<a href="https://t.co/vK7j1ViFeY">https://t.co/vK7j1ViFeY</a><a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://twitter.com/hashtag/M5BALA?src=hash&amp;ref_src=twsrc%5Etfw">#M5BALA</a> <a href="https://t.co/t9f5YgHr9Y">pic.twitter.com/t9f5YgHr9Y</a></p>&mdash; 稲澤祐一 (@inasawa) <a href="https://twitter.com/inasawa/status/1061814739687288833?ref_src=twsrc%5Etfw">2018年11月12日</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script> 

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

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

| 前 | 後 |
|:--:|:--:|
| ![IMG_1903-s.jpeg](https://qiita-image-store.s3.amazonaws.com/0/133064/15af81cc-f0ef-9314-edd8-4bdb84715ddd.jpeg) | ![IMG_1904-s.jpeg](https://qiita-image-store.s3.amazonaws.com/0/133064/65c493fe-bf5d-7329-3d38-eda17abf0a1a.jpeg) |

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

| PORT B | HC-SR04 |
|:-------|:--------:|
| 黒<font color="black">■</font>（GND） | Gnd |
| 赤<font color="red">■</font>（5V） | Vcc |
| 白<font color="white">■</font>（G26） | Trig |
| 黄<font color="yellow">■</font>（G36） | Echo |

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

| M5Stack Fire の説明書<br>M5GO付属のケーブル | ジャンパーピン変換ケーブル |
|:--:|:--:|
| 黒、赤、黄色、白の順 | 黒、赤、白、黄色の順 |
| ![IMG_1899.jpeg](https://qiita-image-store.s3.amazonaws.com/0/133064/cafede87-2af8-b968-36df-53eacae76f9b.jpeg) | ![IMG_1900.jpeg](https://qiita-image-store.s3.amazonaws.com/0/133064/9cfd3900-0f22-efc7-70cc-6567517ab3f0.jpeg) |

#M5BALA制御用プログラム

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

```python: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()
```

#おまけ

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">超音波距離センサーを横向きにして、バケツの周りを回るM5BALA。<a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://twitter.com/hashtag/M5BALA?src=hash&amp;ref_src=twsrc%5Etfw">#M5BALA</a> <a href="https://t.co/eQ9eZW1Ozr">pic.twitter.com/eQ9eZW1Ozr</a></p>&mdash; 稲澤祐一 (@inasawa) <a href="https://twitter.com/inasawa/status/1062910958694068225?ref_src=twsrc%5Etfw">2018年11月15日</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>