M5Stack Core2 × IMU × PaHubで動作分析！複数センサの同時計測を試してみた

ガジェット系

M5Stack Core2 は、豊富な拡張モジュールを備えたマイコンボードです。とくに IMU（慣性計測ユニット） を活用すれば、手軽に姿勢角や動作情報を取得でき、動作分析やスポーツ評価にも応用が効きます。

プログラムはC+ベースのArduinoで書くことになりますが、比較的書きやすくわかりやすいと思っています。

今回は、PaHub を使って IMUを2台同時接続し、M5Stack Core2でのリアルタイム可視化＋判定処理まで行う作例を紹介します。私の旧ブログのリライト版になります。

IMUとは？

IMU（Inertial Measurement Unit）は、**加速度（3軸）と角速度（3軸）を取得できるセンサーです。取得したデータから物体の傾き（ロール・ピッチ）**などの姿勢を推定できます。

たとえば、身体各部にIMUを装着して運動を記録すれば、関節角度などの計測補助に応用可能。競技スポーツにおける動作の再現性評価などにも活用できます。

PaHubとは？

PaHubは、I2C通信のマルチプレクサです。複数のI2Cモジュール（IMUや距離センサなど）を切り替えながら使いたいときに重宝します。

  • M5Stack公式の拡張モジュール
  • 最大8台のI2Cデバイスを切替接続
  • I2Cのアドレス競合を回避できる

今回使ったIMUセンサー

  • 製品名：M5Stack Unit IMU (MPU6886搭載)
  • 特徴：3軸加速度＋3軸ジャイロ

🛠 配線構成

textコピーする編集するM5Stack Core2 ─┬─ PaHub（Port A）
                ├─ IMU1（PaHub Ch0）
                └─ IMU2（PaHub Ch1）

⚠️ 注意：M5Stack Core2では、Wire.begin(32, 33); でI2C初期化が必要です。よくある Wire.begin(21, 22); では動作しません。この部分をうまく処理できるかどうかがカギになりますので、確実に記載するようにしましょう。

Arduinoコード解説

このスケッチでは、以下の処理を実装しています。このコードは二つのIMUの姿勢角の一致度合いを、見るコードになっていて、以下のような処理のフローになります。

  • IMU2台からそれぞれロール・ピッチ角を取得
  • RCフィルタでなめらかに処理
  • M5Stack Core2の画面に数値表示
  • ボタンAでIMU1の姿勢角を記録
  • IMU2の角度が記録時の±10°内に入ったら「MATCH!」表示＋振動！
#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];
}

💡 応用例・発展

今回のコードは姿勢角の一致具合をみるためのコードですが、このコード改変したりしてやると、いろいろな解析をできる可能性があります。

  • バッティングや投球動作のフォーム比較
  • 姿勢保持トレーニング

装置が小さく、持ち運びがしやすいという点が現場でも動作計測に向いている思いますし、価格も安価であるので、コードさえかければ敷居が低いのも特徴だと思います。

注意点・改善の余地

  • 本体内蔵IMU（Core2）も使いたいが、同時接続時の区別が困難
  • 今回はPaHub経由のIMUのみ使用
  • センサ数が増える場合はMUX管理を工夫する必要あり
  • RCフィルタはあくまで簡易的な平滑化。もっと高度なフィルタ（Kalmanなど）も検討可能

まとめ

M5Stack Core2とPaHub、IMUセンサを使えば、小型・省配線でマルチセンサを使ったリアルタイム計測が可能です。

動作分析を手軽に始めたい方には、かなり実用性の高い構成だと思います。実装例も含め、ぜひ活用してみてください！

コメント

タイトルとURLをコピーしました