M5StickC Plus2でカーリングのスウィーピングを解析してみた

本記事の筆者は、「モーション解析」および「カーリング」について 全くの素人 です。内容には誤りや理解不足が含まれている可能性があります。もし不適切な点や改善すべき点がありましたら、ぜひご指摘いただけますと幸いです。

はじめに

カーリングにおいて、ブラシで氷を擦る「スウィーピング」は、ストーンの距離を伸ばし、曲がり幅を調整する重要な要素です。
トップ選手は凄まじい圧力と速度でスウィーピングを行いますが、そのパフォーマンスを定量的に計測する手段は一般的ではありません。

そこで今回は、M5StickC Plus2 を用いてスウィーピングの動き（ストローク強度、幅、周波数）をリアルタイムに計測・解析し、React Native で作成したモバイルアプリで可視化するシステムを構築しました。

システム構成

本システムは、ブラシに取り付ける「エッジデバイス」と、データを表示する「モバイルアプリ」で構成されます。

• エッジデバイス (M5StickC Plus2)
  • 6軸IMUで加速度・角速度を取得
  • Madgwickフィルタで姿勢推定
  • Velocity Zero-Cross法でストロークを検知
  • BLE (Notify) で解析データを送信
• モバイルアプリ (React Native)
  • BLEでデータを受信
  • リアルタイムな数値モニタリング
  • ログデータの保存とグラフ表示

開発環境

ハードウェア

• M5StickC Plus2: 小型で軽量、IMU内蔵で今回の用途に最適。

ファームウェア

• Framework: Arduino
• Language: C++（Arduino）
• Key Libraries:
  • M5StickCPlus2: デバイス制御
  • MadgwickAHRS: 姿勢推定 (Sensor Fusion)
  • ESP32 BLE Arduino: BLE通信

モバイルアプリ

• Framework: React Native (Expo SDK 50+)
• Language: TypeScript
• Key Libraries:
  • react-native-ble-plx: BLE通信
  • react-native-chart-kit: グラフ描画
  • @react-native-async-storage/async-storage: データ永続化

成果物

本システムを用いて計測した結果を以下に示します。
ここでは、スウィーピング動作における ストローク幅（Stroke Length） と 頻度（Frequency） を算出しています。
※Frequency の単位は spm（sweeps per minute） で、1 往復を 2 回としてカウントしています。

検証環境

M5StickC Plus2を取り付けたブラシ（に見立てたクイックルワイパー）を動かし、アプリ側からログの確認を行います。デバイスの取り付け位置は、先端から約30cmの地点としています。

計測データの検証

1. ストローク幅の検知 (30cm vs 15cm)

スウィーピングの幅を「30cm」と「15cm」で切り替えて振った際のデータです。
振れ幅の違いを明確に識別できていることが分かります。

※30cmの場合

※15cmの場合

2. 高周波 (高SPM) への追従性

素早い約30cmのスウィーピングです。

ストローク幅(Stroke Length)の計測値は比較的安定していますが、頻度のブレが大きいです。

3. 連続計測のトラッキング

一定時間連続でスウィーピングを行った際のストローク幅の推移グラフです。
安定して値をトラッキングできていることが確認できます。

実装（ファームウェア）

1. Madgwickフィルタによる姿勢推定と重力除去

IMU（加速度センサ）から得られる値には、常に「重力（1G）」が含まれています。
スウィーピングの「手の動き（運動加速度）」だけを取り出すには、「今、デバイスがどのくらい傾いているか（姿勢）」 を正確に知り、重力成分を引き算する必要があります。

// 1. センサデータで姿勢フィルタを更新
// accX, accY, accZ: 加速度
// gyroX, gyroY, gyroZ: 角速度
filter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ, dt);

// 2. 推定した姿勢から「重力ベクトル」を算出
float gravX, gravY, gravZ;
filter.getGravity(&gravX, &gravY, &gravZ);

// 3. 生データから重力を除去（＝純粋な運動加速度）
float linAx = accX - gravX;
float linAy = accY - gravY;
float linAz = accZ - gravZ;

このように MadgwickAHRS ライブラリを使うことで、複雑な行列計算を意識せずに「重力除去」が行えます。

2. Velocity Zero-Cross法によるストローク検知

「行って、戻る」という往復運動を検知するために、Velocity Zero-Cross（速度ゼロ交差）法 を実装しました。
これは「折り返し地点では一瞬だけ速度がゼロになる」という物理法則を利用したものです。

A. 速度の推定（積分とドリフト補正）

加速度を足し合わせれば（積分すれば）速度になりますが、センサの誤差も一緒に積み上がってしまうため、時間が経つと値がズレていきます（ドリフト）。
そこで今回は、「Leaky Integration（漏れ積分）」 という簡単な手法を使います。

// 減衰係数 (0.95): 過去の速度を少しずつ「忘れる」ことでドリフトを消す
float alpha = 0.95;

// 速度 = (前の速度 * 減衰) + (今の加速度 * 時間)
// ※ax_earth: 地球座標系(地面基準)での加速度。デバイスの傾き補正済み
// ※9.81: 重力加速度(m/s^2)。G単位をm/s^2に変換するために掛ける
vx_hpf = vx_hpf * alpha + ax_earth * 9.81 * dt;

B. ゼロクロス検知と距離算出

3軸それぞれについて速度の符号の反転をチェックし、「いずれかの軸で反転があった場合」 にストロークの区切りとして判定しています。

// 各軸で速度の符号反転をチェック
bool zeroCrossX = (vx_hpf > 0 && prev_vx <= 0) || (vx_hpf < 0 && prev_vx >= 0);
bool zeroCrossY = (vy_hpf > 0 && prev_vy <= 0) || (vy_hpf < 0 && prev_vy >= 0);
bool zeroCrossZ = (vz_hpf > 0 && prev_vz <= 0) || (vz_hpf < 0 && prev_vz >= 0);

// いずれかの軸で反転があれば「ストロークの区切り」とみなす
bool anyZeroCross = zeroCrossX || zeroCrossY || zeroCrossZ;

if (anyZeroCross) {
    // 3軸の移動距離を合成してストローク幅を算出
    float rawStroke = sqrt(dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
    
    // 1. 時間フィルタ: 前回の検知から一定時間(minStrokeInterval)経過しているか？
    // 2. 距離フィルタ: 移動距離(rawStroke)が閾値(minStrokeDist)を超えているか？
    // 両方を満たせば「有効なストローク」としてカウントし、周波数などを更新
    // 累積距離をリセット
} else {
    // 符号が変わらない間は、移動距離を足し続ける
    dist_x += abs(vx_hpf) * dt;
    dist_y += abs(vy_hpf) * dt;
    dist_z += abs(vz_hpf) * dt;
}

3. 動的閾値 (Dynamic Threshold) による誤検知防止

検証環境において、全力で擦る（High Intensity） 場合と 軽く流す（Low Intensity） 場合とで、ストローク回数の計測精度に明確な差が見られました。
特に、短距離（10〜15 cm）× 高周波（500〜700 spm）　のスウィーピング時に、実際よりもストローク回数が少なくカウントされるという問題が発生していました。

そこで本実装では、動的閾値（Dynamic Thresholding） を導入し、強い加速度が継続して観測される状況ではフィルタリングを緩和することで、この誤差をほぼ解消できました。
ただし、なぜこの現象が短距離・高周波の条件で顕著に起こるのかについては現段階では完全には解明されておらず、継続的な検証が必要です。

// 直近の加速度の平均値 (avgMag) を更新
avgMag = avgMag * 0.9 + accMag * 0.1;

// 平均加速度に応じて閾値を切り替え
if (avgMag > 3.0) {
    // 強いスウィーピング中：
    minStrokeInterval = 60; // ストローク間の最小インターバルは60ms、最大で1000spmまで
    minStrokeDist = 0.02; // 2cm以上の動きを検知
} else {
    // 弱いスウィーピング中：
        minStrokeInterval = 100; // ストローク間の最小インターバルは100ms=>最大で600spmまで
    minStrokeDist = 0.05; // 5cm以上の動きのみ検知
}

4. 状態管理とリセット

（前述の通り、スウィーピング終了時に strokeMA.reset() でフィルタを初期化し、次の動作に備えます）

実装のポイント（モバイルアプリ）

BLE通信とデータの可視化

React Native側では react-native-ble-plx を使用してBLE通信を行います。
M5StickC Plus2からのNotificationを受け取り、画面上の数値（ストローク幅、周波数）とログリストをリアルタイムに更新します。
また、保存された計測データは react-native-chart-kit を用いてグラフ化され、セッションごとの詳細な振り返りが可能です。

今後の課題

アルゴリズムの高度化

• 氷上での検証不足 (実環境ノイズ): 現在のアルゴリズムは机上での動作確認に留まっています。実際の氷上では、ブラシと氷（ペブル）の摩擦により独特な振動パターンやノイズが発生すると予想されますが、これらは現状考慮できていません。実地テストを通じたフィルタの再調整が必須です。
• 姿勢誤差のハンドリング: 激しい運動が長時間続くと、IMUの誤差が蓄積し姿勢推定が崩れる課題があります。ゲイン・スケジューリング（高G時に加速度補正を弱める）やZUPT（静止状態での速度リセット）の導入を検討しています。
• ストーン追随動作: 選手が移動しながらスウィーピングを行う際の「移動成分」の分離も課題です。

機能拡張

• スウィーピング圧の計測: 現在は加速度のみですが、感圧センサの導入や、振動解析による圧力推定の実装を検討しています。

まとめ

M5StickC Plus2とReact Nativeを組み合わせることで、実用的なスウィーピング解析デバイスのプロトタイプを作成できました。
今後は実リンクでのテストを重ね、アルゴリズムの精度向上に取り組んでいきます。

付録：ファームウェアのソースコード

本プロジェクトで使用したファームウェア（Arduino）の全ソースコードです。
M5StickC Plus2に書き込むことで動作します。

ソースコードを表示

/*
 * M5StickC Plus2 Curling Sweep Analyzer
 */

#include <M5StickCPlus2.h>
#include <math.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

// --- Madgwickフィルタの実装 (姿勢推定) ---
// 3次元空間での姿勢（クォータニオン）を推定するためのクラス
#define PI 3.14159265358979323846f

class Madgwick {
public:
    float beta;             // アルゴリズムのゲイン
    float q0, q1, q2, q3;   // クォータニオン
    float invSampleFreq;

    Madgwick() {
        beta = 0.1f;
        q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f;
        invSampleFreq = 1.0f / 100.0f; // デフォルト 100Hz
    }

    void begin(float sampleFrequency) {
        invSampleFreq = 1.0f / sampleFrequency;
    }

    // IMU更新関数
    // gx, gy, gz: 角速度 (deg/s)
    // ax, ay, az: 加速度
    void updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt) {
        float recipNorm;
        float s0, s1, s2, s3;
        float qDot1, qDot2, qDot3, qDot4;
        float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

        // deg/s -> rad/s 変換
        gx *= 0.0174533f;
        gy *= 0.0174533f;
        gz *= 0.0174533f;

        // クォータニオンの変化率計算
        qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
        qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
        qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
        qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

        // 加速度が有効な場合のみ補正計算
        if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
            // 正規化
            recipNorm = 1.0f / sqrtf(ax * ax + ay * ay + az * az);
            ax *= recipNorm;
            ay *= recipNorm;
            az *= recipNorm;

            // 補助変数
            _2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3;
            _4q0 = 4.0f * q0; _4q1 = 4.0f * q1; _4q2 = 4.0f * q2;
            _8q1 = 8.0f * q1; _8q2 = 8.0f * q2;
            q0q0 = q0 * q0; q1q1 = q1 * q1; q2q2 = q2 * q2; q3q3 = q3 * q3;

            // 勾配降下法による補正ステップ
            s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
            s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
            s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
            s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
            recipNorm = 1.0f / sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
            s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm;

            // フィードバック適用
            qDot1 -= beta * s0;
            qDot2 -= beta * s1;
            qDot3 -= beta * s2;
            qDot4 -= beta * s3;
        }

        // 積分
        q0 += qDot1 * dt;
        q1 += qDot2 * dt;
        q2 += qDot3 * dt;
        q3 += qDot4 * dt;

        // 正規化
        recipNorm = 1.0f / sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
        q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm;
    }
    
    // 重力ベクトルの取得
    void getGravity(float *gx, float *gy, float *gz) {
        *gx = 2.0f * (q1 * q3 - q0 * q2);
        *gy = 2.0f * (q0 * q1 + q2 * q3);
        *gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
    }
};

// --- 移動平均フィルタ ---
#define MA_SIZE 20
class MovingAverage {
public:
    float buffer[MA_SIZE];
    int index;
    int count;
    float sum;

    MovingAverage() {
        reset();
    }

    float update(float val) {
        sum -= buffer[index];
        buffer[index] = val;
        sum += val;
        index = (index + 1) % MA_SIZE;
        if (count < MA_SIZE) count++;
        return sum / count;
    }

    void reset() {
        index = 0;
        count = 0;
        sum = 0;
        for (int i = 0; i < MA_SIZE; i++) buffer[i] = 0;
    }
};


// 変数定義
float accX = 0.0F, accY = 0.0F, accZ = 0.0F;
float gyroX = 0.0F, gyroY = 0.0F, gyroZ = 0.0F;

// ゼロクロス法用変数
float vx_hpf = 0, vy_hpf = 0, vz_hpf = 0; // ハイパスフィルタ後の速度
float dist_x = 0, dist_y = 0, dist_z = 0;
float stroke_x = 0, stroke_y = 0, stroke_z = 0;
float prev_vx = 0, prev_vy = 0, prev_vz = 0;

// ストローク計測結果
float currentStroke = 0;
float smoothedStroke = 0;
float strokeRate = 0; // SPM
unsigned long totalStrokes = 0;

// 周波数計算用
#define FREQ_AVG_SIZE 5
float strokeIntervals[FREQ_AVG_SIZE];
int freqHead = 0;
int freqCount = 0;
unsigned long lastStrokeTime = 0;
unsigned long lastCountTime = 0;
#define MIN_STROKE_INTERVAL 100

// スウィーピング状態管理
bool isSweeping = false;
int consecutiveStrokes = 0;
#define SWEEP_TIMEOUT 1000 // ms
#define SWEEP_ENTRY_COUNT 2
#define SWEEP_ENTRY_ACC 2.0 // g

// 動的閾値
float avgMag = 0;
float peakStrokeMag = 0;
unsigned long minStrokeInterval = 100;
float minStrokeDist = 0.05;

MovingAverage strokeMA;
Madgwick filter;
unsigned long lastTime = 0;

// BLE設定
// ※注意: 以下のUUIDはサンプルです。実際に開発する際は、uuidgenなどで独自のUUIDを生成して使用してください。
#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
bool deviceConnected = false;
bool oldDeviceConnected = false;

class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
    }
};

void setup() {
    // M5StickC Plus2 初期化
    M5.begin();
    Serial.begin(115200);
    
    // 画面設定
    M5.Lcd.setRotation(3);
    M5.Lcd.setTextSize(2);
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(10, 10);
    M5.Lcd.println("Init IMU...");
    
    // フィルタ初期化 (50Hz)
    filter.begin(50);
    
    delay(1000);
    M5.Lcd.fillScreen(BLACK);
    
    // 履歴初期化
    for(int i=0; i<FREQ_AVG_SIZE; i++) strokeIntervals[i] = 0;

    lastTime = millis();

    // --- BLE 初期化 ---
    BLEDevice::init("M5StickC_BrushLogger");
    pServer = BLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());

    BLEService *pService = pServer->createService(SERVICE_UUID);

    pCharacteristic = pService->createCharacteristic(
                        CHARACTERISTIC_UUID,
                        BLECharacteristic::PROPERTY_READ   |
                        BLECharacteristic::PROPERTY_WRITE  |
                        BLECharacteristic::PROPERTY_NOTIFY |
                        BLECharacteristic::PROPERTY_INDICATE
                      );

    pCharacteristic->addDescriptor(new BLE2902());

    pService->start();

    BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
    pAdvertising->addServiceUUID(SERVICE_UUID);
    pAdvertising->setScanResponse(false);
    pAdvertising->setMinPreferred(0x0);
    BLEDevice::startAdvertising();
    Serial.println("Waiting a client connection to notify...");
}

void loop() {
    M5.update();
    
    // デルタタイム計算
    unsigned long currentTime = millis();
    float dt = (currentTime - lastTime) / 1000.0;
    if (dt <= 0) return;
    lastTime = currentTime;

    // IMUデータ取得
    M5.Imu.getAccelData(&accX, &accY, &accZ);
    M5.Imu.getGyroData(&gyroX, &gyroY, &gyroZ);

    // フィルタ更新
    filter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ, dt);

    // 重力ベクトルの取得
    float gravX, gravY, gravZ;
    filter.getGravity(&gravX, &gravY, &gravZ);

    // 線形加速度（重力除去）
    float linAx = accX - gravX;
    float linAy = accY - gravY;
    float linAz = accZ - gravZ;

    // 世界座標系への変換
    float q0 = filter.q0; float q1 = filter.q1; float q2 = filter.q2; float q3 = filter.q3;
    float ax_earth = (1 - 2*q2*q2 - 2*q3*q3)*linAx + (2*q1*q2 - 2*q0*q3)*linAy + (2*q1*q3 + 2*q0*q2)*linAz;
    float ay_earth = (2*q1*q2 + 2*q0*q3)*linAx + (1 - 2*q1*q1 - 2*q3*q3)*linAy + (2*q2*q3 - 2*q0*q1)*linAz;
    float az_earth = (2*q1*q3 - 2*q0*q2)*linAx + (2*q2*q3 + 2*q0*q1)*linAy + (1 - 2*q1*q1 - 2*q2*q2)*linAz;

    // ZUPT用マグニチュード
    float accMag = sqrt(linAx*linAx + linAy*linAy + linAz*linAz);
    
    // ピーク検出
    if (accMag > peakStrokeMag) {
        peakStrokeMag = accMag;
    }

    // 動的閾値ロジック
    avgMag = avgMag * 0.9 + accMag * 0.1;
    
    if (avgMag > 3.0) {
        minStrokeInterval = 60; // 強い動作: 閾値を下げる
        minStrokeDist = 0.02;   // 2cm
    } else {
        minStrokeInterval = 100; // 弱い動作: 閾値を上げる
        minStrokeDist = 0.05;    // 5cm
    }
    
    // --- ゼロクロス法によるストローク推定 ---
    
    // 1. 速度推定 (Leaky Integration)
    float alpha = 0.95;
    vx_hpf = vx_hpf * alpha + ax_earth * 9.81 * dt;
    vy_hpf = vy_hpf * alpha + ay_earth * 9.81 * dt;
    vz_hpf = vz_hpf * alpha + az_earth * 9.81 * dt;
    
    // 2. 距離積算
    dist_x += abs(vx_hpf) * dt;
    dist_y += abs(vy_hpf) * dt;
    dist_z += abs(vz_hpf) * dt;
    
    // 3. ゼロクロス検知
    bool zeroCrossX = (vx_hpf > 0 && prev_vx <= 0) || (vx_hpf < 0 && prev_vx >= 0);
    bool zeroCrossY = (vy_hpf > 0 && prev_vy <= 0) || (vy_hpf < 0 && prev_vy >= 0);
    bool zeroCrossZ = (vz_hpf > 0 && prev_vz <= 0) || (vz_hpf < 0 && prev_vz >= 0);
    
    // --- スウィーピング状態管理 ---
    // タイムアウト処理
    if (millis() - lastStrokeTime > SWEEP_TIMEOUT) {
        isSweeping = false;
        consecutiveStrokes = 0;
        // データリセット
        smoothedStroke = 0;
        currentStroke = 0;
        strokeRate = 0;
        freqCount = 0;
        strokeMA.reset();
    }

    if (zeroCrossX) { stroke_x = dist_x; dist_x = 0; }
    if (zeroCrossY) { stroke_y = dist_y; dist_y = 0; }
    if (zeroCrossZ) { stroke_z = dist_z; dist_z = 0; }
    
    bool anyZeroCross = zeroCrossX || zeroCrossY || zeroCrossZ;

    // ストローク幅の計算
    float rawStroke = sqrt(stroke_x*stroke_x + stroke_y*stroke_y + stroke_z*stroke_z);
    
    // ストローク確定時の処理
    if (anyZeroCross) {
        unsigned long now = millis();
        if (now - lastCountTime > minStrokeInterval) {
            
            // 開始判定用カウント
            if (now - lastStrokeTime < SWEEP_TIMEOUT) {
                if (peakStrokeMag > SWEEP_ENTRY_ACC) {
                    consecutiveStrokes++;
                } else {
                    consecutiveStrokes = 0;
                }
            } else {
                consecutiveStrokes = (peakStrokeMag > SWEEP_ENTRY_ACC) ? 1 : 0;
            }
            lastStrokeTime = now;

            // スウィーピング開始？
            if (consecutiveStrokes >= SWEEP_ENTRY_COUNT) {
                isSweeping = true;
            }
            
            peakStrokeMag = 0;

            // データ更新
            if (isSweeping) {
                if (rawStroke > minStrokeDist) {
                    totalStrokes++;
                    
                    // 周波数計算
                    if (lastCountTime > 0) {
                        float interval = (now - lastCountTime);
                        strokeIntervals[freqHead] = interval;
                        freqHead = (freqHead + 1) % FREQ_AVG_SIZE;
                        if (freqCount < FREQ_AVG_SIZE) freqCount++;
                        
                        float sum = 0;
                        for(int i=0; i<freqCount; i++) sum += strokeIntervals[i];
                        float avgInterval = sum / freqCount;
                        
                        if (avgInterval > 0) {
                            strokeRate = 60000.0f / avgInterval;
                        }
                    }
                    
                    currentStroke = rawStroke * 100.0; // cm変換
                    smoothedStroke = strokeMA.update(currentStroke);
                }
            }
            lastCountTime = now;
        }
    }

    // 周波数タイムアウト
    if (millis() - lastStrokeTime > 1500) {
        strokeRate = 0;
        freqCount = 0;
    }

    prev_vx = vx_hpf;
    prev_vy = vy_hpf;
    prev_vz = vz_hpf;
    
    // リセットボタン (BtnA)
    if (M5.BtnA.wasPressed()) {
        // 全状態リセット
        totalStrokes = 0;
        strokeRate = 0;
        smoothedStroke = 0;
        currentStroke = 0;
        freqCount = 0;
        isSweeping = false;
        consecutiveStrokes = 0;
        
        filter.q0 = 1.0f; filter.q1 = 0.0f; filter.q2 = 0.0f; filter.q3 = 0.0f;
        
        M5.Lcd.fillScreen(BLACK);
        M5.Lcd.setCursor(0, 0);
        M5.Lcd.println("RESET");
        delay(500);
        M5.Lcd.fillScreen(BLACK);
    }

    // --- 画面表示 ---
    M5.Lcd.setCursor(0, 0);
    
    uint16_t color = WHITE;
    if (accMag >= 1.0) color = RED;
    else if (accMag >= 0.2) color = YELLOW;
    
    M5.Lcd.setTextColor(color, BLACK);
    M5.Lcd.setTextSize(3);
    M5.Lcd.printf("Mag: %.2f g   \n", accMag);
    
    M5.Lcd.setTextSize(1);
    M5.Lcd.setTextColor(WHITE, BLACK);
    M5.Lcd.setCursor(0, 40);
    M5.Lcd.printf("SmoothStr: %.1f cm   \n", smoothedStroke);
    M5.Lcd.printf("Freq: %.1f spm   \n", strokeRate);
    M5.Lcd.printf("Count: %lu      \n", totalStrokes);
    
    if (deviceConnected) {
        M5.Lcd.setTextColor(GREEN, BLACK);
        M5.Lcd.println("BLE: Connected   ");
    } else {
        M5.Lcd.setTextColor(BLUE, BLACK);
        M5.Lcd.println("BLE: Advertising ");
    }

    delay(20); // ~50Hz

    // --- BLE通知 ---
    if (deviceConnected) {
        char jsonStr[128];
        snprintf(jsonStr, sizeof(jsonStr), "{\"m\":%.2f,\"s\":%.1f,\"f\":%.1f,\"c\":%lu,\"t\":%lu}", accMag, smoothedStroke, strokeRate, totalStrokes, millis());
        
        pCharacteristic->setValue(jsonStr);
        pCharacteristic->notify();
    }

    // 再接続処理
    if (!deviceConnected && oldDeviceConnected) {
        delay(500);
        pServer->startAdvertising();
        oldDeviceConnected = deviceConnected;
    }
    if (deviceConnected && !oldDeviceConnected) {
        oldDeviceConnected = deviceConnected;
    }
}