Arduino M5Stack プログラミング 初心者

M5Stack Core2のPaHubの作例。二個のIMUの姿勢角を表示するArudinoコード。

M5Stack Core2はいろんなモジュールがあって、簡単にいろいろなデータを記録することができます。筆者は主に動作分析をよくやっているのですが、そのなかでも慣性センサー（IMU）を使うことが多いです。

IMUは三軸方向の加速度と角速度を計測することができるユニットで、これらの情報を使うことによってIMUを貼付した物体の姿勢角を計測することが可能です。

動作分析においては、身体の各セグメントにIMUを貼付することによってそのセグメントの関節角度情報を算出する補助として約に立ちます。

さらに、物体の姿勢角が重要なスポーツ競技での動作の再現性の確認などにも応用できる可能性もあるといえます。

今回はIMUとM5Stack Core2の拡張ハブユニットであるPaHubというものと使って、二台のIMUを同時計測してM5Stack Core2に画面表示させるという作例を作っていきたいと思います。

※あくまでも動くというだけで、もっといい書き方は絶対にあるとおもうので参考までに

Pahubとは何ぞや

そもそもいきなり出てきたPaHubとはいったいどういったものなのでしょうか？PaHubとは

M5stackの拡張モジュールのひとつで、I2Cと呼ばれる通信方法で接続されるモジュール（IMUとか赤外線センサーとか）を複数台同時に動かすためのモジュールになります。スイッチサイエンスでも販売しており、価格も手軽なのでM5Stackを扱ううえで筆者もいつもお世話になっているモジュールです（スイッチサイエンスURL：https://www.switch-science.com/products/7588?srsltid=AfmBOoo45dpjToP-gNtTxzwFKBjHZogPhKtMipD_tWiR5fW1cjL7fGk2）。

この拡張されたモジュールに複数台のIMUをつなげてやることで同時計測をすることができるという算段になります。これがかなり便利で組み合わせIMUだけ複数台でもいいし、他にも、圧力センサーをつないだり、距離センサーをつないだりと多種多様に使用することができます。当然一台でも使用することはできるのですが、今後の拡張性を考慮して今回のコードでは二台での作例を紹介しようと思います（２台も3台もあんまり変わらないのである笑）

ちなみに今回使っているIMUセンサーはこちらのものになります（スイッチサイエンス：https://www.switch-science.com/products/6623?srsltid=AfmBOop2s-mxZTlfYombIdHuXo36OuLK2ZP8Igx8TRkch8RtiZ0HqI_H）。

M5Stack Core2でPaHubと複数台のIMUを使ったArudinoコード作例

実際につないだ感じはこんな感じです（PaHubが映ってなくてすみません）。

そしてコードはこんな感じです。作例としては、二台のIMUの姿勢角をストリームで流しつつ、M5Stack Core2の左ボタンを押したときにIMUの一台目の姿勢角を記録して、一台目のIMUの姿勢角が、そのボタンを押したときの姿勢角の±10°以内の姿勢角になった場合に振動するというコードになります。二台目のIMUのほうは、まだ使い道を考えていないので、姿勢角がそのままストリーミングされているだけの状態になります。

#include <M5Core2.h>
#include <I2C_MPU6886.h>
#include <SparkFun_I2C_Mux_Arduino_Library.h>
#include <Wire.h>
#include <AXP192.h>
I2C_MPU6886 imu(I2C_MPU6886_DEFAULT_ADDRESS, Wire);
QWIICMUX i2cMux;


AXP192 power;
float ax = 0.0, ay = 0.0, az = 0.0;
float gx = 0.0, gy = 0.0, gz = 0.0;
float roll = 0.0, pitch = 0.0;

float ax2 = 0.0, ay2 = 0.0, az2 = 0.0;
float gx2 = 0.0, gy2 = 0.0, gz2 = 0.0;
float roll2 = 0.0, pitch2 = 0.0;

double data1 = 0.0, data2 = 0.0, data3 = 0.0, data4 = 0.0;

#define ENV_CH1 0
#define ENV_CH2 1
const int PaHub_I2C_ADDRESS = 0x70;

float recordedRoll2 = 0.0;   // 記録されたroll2
float recordedPitch2 = 0.0;  // 記録されたpitch2
bool matchDisplayed = false; // MATCHが表示されているかどうか

void setup() {
    M5.begin();
    Wire.begin(32, 33);
    i2cMux.begin(PaHub_I2C_ADDRESS, Wire);

    i2cMux.setPort(ENV_CH1);
    imu.begin();
    i2cMux.setPort(ENV_CH2);
    imu.begin();

    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setTextColor(WHITE);
    M5.Lcd.setTextSize(2);  // デフォルトのテキストサイズを大きめに設定

    // 静的なテキストの表示
    M5.Lcd.setCursor(10, 10);
    M5.Lcd.println("IMU1 Roll: ");
    M5.Lcd.setCursor(10, 40);
    M5.Lcd.println("IMU1 Pitch: ");
    M5.Lcd.setCursor(10, 70);
    M5.Lcd.println("IMU2 Roll: ");
    M5.Lcd.setCursor(10, 100);
    M5.Lcd.println("IMU2 Pitch: ");
    M5.Lcd.setCursor(10, 130);
    M5.Lcd.println("Press A to record");
}

void loop() {
    M5.update();
    M5.Lcd.setTextColor(WHITE);
    // IMUデータの取得
    getIMU();
    getIMU2();

    // ボタンAが押された場合、roll2とpitch2を記録
    if (M5.BtnA.wasPressed()) {
        recordedRoll2 = roll2;
        recordedPitch2 = pitch2;

        // 記録値を表示
        M5.Lcd.setCursor(10, 160);
        M5.Lcd.fillRect(10, 160, 300, 20, BLACK); // 古いデータを消す
        M5.Lcd.printf("Recorded: %.2f, %.2f", recordedRoll2, recordedPitch2);
    }


    if (abs(roll2 - recordedRoll2) <= 10.0 && abs(pitch2 - recordedPitch2) <= 10.0) {
        if (!matchDisplayed) {
            // MATCHを表示して音を鳴らす
            M5.Lcd.setCursor(60, 200);
            M5.Lcd.setTextColor(WHITE, BLACK); // 背景を黒にして消去しやすく
            M5.Lcd.setTextSize(3);
            M5.Lcd.setTextColor(YELLOW);
            M5.Lcd.println("MATCH!");

            // 振動入れる
            power.SetLDOEnable(3, true);   

            matchDisplayed = true;
        }
    } else {
        if (matchDisplayed) {
            // MATCHを消去
            M5.Lcd.fillRect(60, 200, 200, 30, BLACK);
            power.SetLDOEnable(3, false); 
            M5.Lcd.setTextColor(WHITE);
            matchDisplayed = false;
        }
    }

    // IMU1 Roll と IMU1 Pitch の表示（大きい数字）
    M5.Lcd.setTextSize(2);  // 生データはやや小さい文字
    M5.Lcd.setCursor(130, 10);
    M5.Lcd.fillRect(130, 10, 100, 20, BLACK);  // 古い値を消去
    M5.Lcd.printf("%.2f", data1);

    M5.Lcd.setCursor(130, 40);
    M5.Lcd.fillRect(130, 40, 100, 20, BLACK);  // 古い値を消去
    M5.Lcd.printf("%.2f", data2);

    // IMU2 Roll と IMU2 Pitch の表示（大きい数字）
    M5.Lcd.setCursor(130, 70);
    M5.Lcd.fillRect(130, 70, 100, 20, BLACK);  // 古い値を消去
    M5.Lcd.printf("%.2f", data3);

    M5.Lcd.setCursor(130, 100);
    M5.Lcd.fillRect(120, 100, 100, 20, BLACK);  // 古い値を消去
    M5.Lcd.printf("%.2f", data4);

    delay(100);
}

void getIMU() {
    i2cMux.setPort(ENV_CH1);
    imu.getAccel(&ax, &ay, &az);
    imu.getGyro(&gx, &gy, &gz);

    // RollとPitchの計算
    roll = atan2(ay, az) * 180.0 / PI;
    pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / PI;

    // RCフィルタの適用
    static float rolldata1[2] = {0};
    static float pitchdata1[2] = {0};

    rolldata1[1] = 0.95 * rolldata1[0] + 0.05 * roll;
    rolldata1[0] = rolldata1[1];
    data1 = rolldata1[0];

    pitchdata1[1] = 0.95 * pitchdata1[0] + 0.05 * pitch;
    pitchdata1[0] = pitchdata1[1];
    data2 = pitchdata1[0];
}

void getIMU2() {
    i2cMux.setPort(ENV_CH2);
    imu.getAccel(&ax2, &ay2, &az2);
    imu.getGyro(&gx2, &gy2, &gz2);

    // RollとPitchの計算
    roll2 = atan2(ay2, az2) * 180.0 / PI;
    pitch2 = atan2(-ax2, sqrt(ay2 * ay2 + az2 * az2)) * 180.0 / PI;

    // RCフィルタの適用
    static float rolldata2[2] = {0};
    static float pitchdata2[2] = {0};

    rolldata2[1] = 0.95 * rolldata2[0] + 0.05 * roll2;
    rolldata2[0] = rolldata2[1];
    data3 = rolldata2[0];

    pitchdata2[1] = 0.95 * pitchdata2[0] + 0.05 * pitch2;
    pitchdata2[0] = pitchdata2[1];
    data4 = pitchdata2[0];
}

多分余計な部分も結構あるとは思いますが、とりあえずは動きます。注意したいのがM5Stack Core2の場合、Wire.begin(32, 33);であるという点です。よく作例では、Wire.begin(21, 22);　になっているとは思うのですが、今回のような構成だと動かなかったです。

またIMUのデータをなめらかにするためにRCフィルタというものを入れています。今回のRCフィルタは２フレーム分のデータを貯めてその二点の値を95:5の割合で混ぜた値を出力するという感じです。まあ気休めれレベルではあります。

実はM5Stack Core2本体にもIMUが入っているのですが、今回のシステム構成で使用すると、他のIMUセンサーと区別することができなくて利用することができませんでした（多分うまい人はそれを使うのもできるとは思うのですが筆者の技術力ではダメでした(´;ω;｀)）。

まああくまで作例のひとつとして備忘録に残しておきます。役に立つ部分が少しでもあれば幸いです。