m5vibrationFFT.ino

#include <M5StickC.h>
#include "arduinoFFT.h"

#define SAMPLE_PERIOD 5    // サンプリング間隔(ミリ秒)

const uint16_t FFTsamples = 64; //This value MUST ALWAYS be a power of 2
double vReal[FFTsamples];
double vImag[FFTsamples];
const double samplingFrequency = 1000.0 / (double)SAMPLE_PERIOD; // 200Hz
arduinoFFT FFT = arduinoFFT(vReal, vImag, FFTsamples, samplingFrequency);  // FFTオブジェクトを作る

int Y0 = 10;
int _height = 80 - Y0;
int _width = 160;
float dmax = 1000.0;

void drawChart(int nsamples) {
    int band_width = floor(_width / nsamples);
    int band_pad = band_width - 1;

    for (int band = 0; band < nsamples; band++) {
        int hpos = band * band_width;
        float d = vReal[band];
        if (d > dmax) d = dmax;
        int h = (int)((d / dmax) * (_height));
        M5.Lcd.fillRect(hpos, _height - h, band_pad, h, WHITE);
        if ((band % 8) == 0) {
            M5.Lcd.setCursor(hpos, _height);
            M5.Lcd.printf("%dHz", (int)((band * 1.0 * samplingFrequency) / FFTsamples));
        }
    }
}

void setup() {
    M5.begin();
    M5.Lcd.setRotation(3);
    M5.MPU6886.Init();
}

void DCRemoval(double *vData, uint16_t samples) {
    double mean = 0;
    for (uint16_t i = 0; i < samples; i++) {
        mean += vData[i];
    }
    mean /= samples;
    for (uint16_t i = 0; i < samples; i++) {
        vData[i] -= mean;
    }
}

void loop() {
    for (int i = 0; i < FFTsamples; i++) {  // 振動をサンプリングする
        float ax, ay, az;  // 加速度データを読み出す変数
        long t = micros();
        M5.MPU6886.getAccelData(&ax,&ay,&az);  // MPU6886から加速度を取得
        vReal[i] = az * 1000;  // mGに変換
        vImag[i] = 0;
        delayMicroseconds(SAMPLE_PERIOD * 1000 - (micros() - t));
    }

    DCRemoval(vReal, FFTsamples);  // 直流分を除去
    FFT.Windowing(FFT_WIN_TYP_HAMMING, FFT_FORWARD);  // 窓関数
    FFT.Compute(FFT_FORWARD); // FFT処理(複素数で計算)
    FFT.ComplexToMagnitude(); // 複素数を実数に変換
    double x = FFT.MajorPeak();

    M5.Lcd.fillScreen(BLACK);  // 画面をクリア
    drawChart(FFTsamples / 2);
    M5.Lcd.setCursor(40, 0);
    M5.Lcd.printf("Peak: %.0fHz", x);
}