M5ATOM Liteから超音波センサーを3つ動かしてみた

ロボットの障害物検知用に超音波センサーを3～6つ動かしたくなったので、作成してみた。

環境

Windows 11
PlatformIO
M5ATOM Lite
M5Stack用Port A I2C拡張ハブユニット(PaHUB2 Unit)
M5Stack用超音波測距ユニット I2C 3つ

ハードウェアの選定と組み立て

お手軽に作りたいので、M5ATOM Lite(以下ATOM)とM5Stack用超音波測距ユニットI2Cをチョイス。

この測距ユニットはATOMとI2C通信でやり取りする。
そこまではいいけど、距離ユニットのI2Cアドレスが固定されているため、そのままではATOMに2つ以上を直接接続できない。

なので、M5Stack用Port A I2C拡張ハブユニット、略してPaHUB2も併せて使用。
PaHUB2は複数のI2Cデバイスを切り替えて動かせるので、同じアドレスを持つ複数のユニットをM5Stack(ATOM含む)に接続できる。

あとはケーブルでつなぐだけで、ハードウェア完成。簡単、簡単。

プログラム

ソフトウェアはUIFlowを使うのがお手軽でいいんだけど、いずれROS2化することも考慮して、C言語で作成。

platformio.ini

[env:m5stack-atom]
platform = espressif32
board = m5stack-atom
framework = arduino
lib_deps =
    m5stack/M5Unit-Sonic@^0.0.2
    closedcube/ClosedCube TCA9548A@^2020.5.21
    closedcube/ClosedCube I2C Driver@^2020.9.8
    m5stack/M5Unified@^0.1.12

main.cpp

#include <M5Unified.h>
#include <Unit_Sonic.h>
#include "ClosedCube_TCA9548A.h"

#define PaHub_I2C_ADDRESS 0x70
#define SONIC_NUM 3

ClosedCube::Wired::TCA9548A tca9548a;

SONIC_I2C sensor;

void setup()
{
    M5.begin();
    tca9548a.address(PaHub_I2C_ADDRESS);
    sensor.begin();
}

int updateFPS()
{
    static int psec = 0;
    static int fps = 0;
    static int frame_count = 0;

    int sec = millis() / 1000;
    frame_count++;

    if (psec != sec)
    {
        psec = sec;
        fps = frame_count;
        frame_count = 0;
    }

    return fps;
}

void loop()
{
    std::string output = "";
    char buffer[30];

    int fps = updateFPS();

    for (uint8_t channel = 0; channel < SONIC_NUM; channel++) {
        tca9548a.selectChannel(channel);
        float distance = sensor.getDistance();
        sprintf(buffer, "%d:%3.1fcm ", channel, distance / 10);
        output += buffer;
    }
    sprintf(buffer, "fps:%d\r\n", fps);
    output += buffer;

    Serial.printf(output.c_str());

    delay(10);
}

このプログラムの注意点とか

超音波センサーでの測定には時間がかかる。実際、getDistance()関数の中でも120ms待ってから測定結果を取得している。
3つのセンサーを順番に測定するとそれだけで360msの待ち時間が必要になる。
プログラムを見直せば並列で測定もできるかもしれないけど、現状でも3つのセンサーで毎秒2回程度測定できているので、まあいいかな。

リンク

PaHUB2のドキュメント
M5ATOM Liteのドキュメント
M5ATOMと超音波センサーを使ったサンプル
RCWL-9620のチップ情報(zip)