ミニ四駆用モーターチェッカーの作り方

ミニ四駆
2024.07.27

はじめに

ミニ四駆モーターチェッカーの作り方を公開します。
構成としては「M5StickC Plus2」を使用します。
今の機能だと加工なしで、プログラムの書き込みのみとなります。

ライセンス

CC BY-SA 4.0を適用します。
個人で使用する分には問題ないと思います。
下記がライセンスの内容となります。

あなたの従うべき条件は以下の通りです。

表示 — あなたは 適切なクレジット を表示し、ライセンスへのリンクを提供し、 変更があったらその旨を示さ なければなりません。これらは合理的であればどのような方法で行っても構いませんが、許諾者があなたやあなたの利用行為を支持していると示唆するような方法は除きます。

継承 — もしあなたがこの資料をリミックスしたり、改変したり、加工した場合には、あなたはあなたの貢献部分を元の作品と 同じライセンス の下に頒布しなければなりません。

追加的な制約は課せません — あなたは、このライセンスが他の者に許諾することを法的に制限するようないかなる法的規定も 技術的手段 も適用してはなりません。

機能

振動センサの値をFFT解析してピークの周波数を求めています。
これにより、モーターの回転数が算出できますので、ギア比・タイヤ径から速度を算出することができます。

使い方

測定モード

画像の通りです。
バッテリー電圧は3.0～4.2Vが使用範囲なので、大まかなバッテリー残量の目安にしてください。
本体左上にあるボダンでコンフィグモードに入ります。

コンフィグモード

本体左上のボタンで「項目戻し、項目送り、値減、値増」を切り替えます。
決定ボタンで対応した機能を実行します。
「値増減」に関しては長押しで早送りが出来ます。

各項目の説明
GearRatio
-> ギア比を変更します
TireDiameter
-> タイヤ径を変更します
RPM Upper
-> 回転数の上限を変更します
　　これにより、モーター以外の振動数をカットすることが出来ます。
RPM Lower
-> 回転数の下限を変更します
　　これにより、モーター以外の振動数をカットすることが出来ます。
Detection Level
-> 回転数を算出する最低限の振幅を設定します。
　　0の場合はどんな微弱な振動でも計測をします
MPU Direction
-> 振動を計測する向きを設定します
　　XYZ軸のいずれかを選択します
Version
-> バージョンを表示します
Back
-> 設定値を保存して測定モードに戻ります

構成部品

主な構成部品は下記の通りです。

下記はスイッチサイエンスでの購入をお勧めします。
（メジャーどころで一番安い）
https://www.switch-science.com/

M5StickC Plus2
M5StickCを大画面にしたアップグレード製品「M5StickC Plus」の最新バージョンです。ESP32-PICO-V3-02を搭載し、BluetoothとWi-Fi通信が可能です。コンパクトなボディに、赤外線、RTC、マイクロフォン...
www.switch-science.com

組み立て方

組み立ては必要ありません。
プログラムの書き込みのみです。

書き込み方法

使用ライブラリは下記の通りです。
・M5Unified.h -> version 0.1.16
・arduinoFFT.h -> version 2.0.2
・I2C_MPU6886_4kHz.h -> 最新のを使用してください

書込確認済環境
・VSCode + PlatformIO
・Arduino IDE

(M5StickCへの書き込み方は他の方の説明の方が詳しいので、、、)

プログラム

開発環境 VSCode + PlatformIO
検証環境 Arduino IDE

使用ライブラリ
・M5Unified.h
・arduinoFFT.h
・I2C_MPU6886_4kHz.h
　-> 自作ライブラリなので下記からダウンロード
　https://github.com/yururi-nonbiry/I2C_MPU6886_4kHz

※ 環境によってライブラリのインストールがうまくいかないとかあるっぽいので、その場合はincludeファイル(**.h、**.cpp)を直下に置いて#include <**.h>を#include “**.h”に変更してください。

#include <Arduino.h>
#include <M5Unified.h>
#include <ArduinoFFT.h>
#include <EEPROM.h>
#include "I2C_MPU6886_4kHz.h"
#include <math.h>

// version
#define VERSION "1.0.0"

// 各種固定値
#define EEPROM_VERSION "M5RPM1.0.0"

// 各種設定範囲
#define TFT_CS 5
#define TFT_DC 23
#define TFT_RST 18

// ボタンのピン
#define BtnApin 37
#define BtnBpin 39
#define BtnCpin 35
#define BTTpin 38

// 設定範囲
#define GearRatioMax 15.0 // ギア比最大値
#define GearRatioMin 2.0 // ギア比最小値
#define TireDiameterMax 40.0 // タイヤ径
#define TireDiameterMin 20.0 // タイヤ径
#define RPMMax 60000 // 回転数上限
#define RPMMin 0 // 回転数下限

// 振動FFT解析用
#define SAMPLE 2048 // 分解能と速度の背反
#define SAMPLING_FREQUENCY 2000 // サンプリング周期が2kHzだと60000rpmまでの測定が可

I2C_MPU6886_4kHz imu(I2C_MPU6886_DEFAULT_ADDRESS, Wire1);

double vReal[SAMPLE];
double vImag[SAMPLE];

ArduinoFFT<double> FFT = ArduinoFFT<double>(vReal, vImag, SAMPLE, SAMPLING_FREQUENCY);

// 描画関係
M5Canvas canvas;

// EEPROMの設定
struct EEPROMStruct
{ 
  char EEPROMVersion[11]; // EEPROMのバージョン確認用
  float GearRatio; // ギア比
  float TireDiameter; // タイヤ径
  float RPMUpper; // 回転数上限
  float RPMLower; // 回転数下限
  int DetectionLevel; // 検出レベル
  int MPUDirection; // 振動センサ サンプリング方向
};

EEPROMStruct EEPROMData;

// EEPROMへ書き込み
void eeprom_write()
{
  // EEPROMのデータを読み込み
  EEPROM.put<EEPROMStruct>(0, EEPROMData);
  EEPROM.commit();
}


// グローバル変数
int16_t sample_binary[SAMPLE]; // サンプリング記録用
uint8_t accel_mode = 0; // 測定軸の選択

int BtnAcounter = 0; // ボタンAの割り込み処理
int BtnBcounter = 0; // ボタンBの割り込み処理

int info_mode = 0; // 画面のインフォメーション情報の切替用
int config_mode = 0; // コンフィグの切替用
int config_select = 0; // コンフィグのセレクト用

// チャタリング防止
unsigned long BtnApushedTime = 0; // タッチパネル読取用
unsigned long BtnBpushedTime = 0; // タッチパネル読取用


// FFT解析
double FFT_run()
{
  double total = 0.0; // 振動の合計値
  double total_abs = 0.0; // 振動の合計値

  // 変数の初期化、データの変換・移替
  for (int i = 0; i < SAMPLE; i++)
  {
    vReal[i] = double(sample_binary[i]);
    vImag[i] = 0;
    total += double(sample_binary[i]);
  }

  double total_average = total / SAMPLE;

  for (int i = 0; i < SAMPLE; i++)
  {
    total_abs += double(abs(sample_binary[i] - total_average));
  }

  // 強度判定
  if(EEPROMData.DetectionLevel != 0 && total_abs / SAMPLE < pow(2, EEPROMData.DetectionLevel + 3 ))
    return 0.0;
 
  // FFT解析
  FFT.windowing(FFTWindow::Hamming, FFTDirection::Forward);	/* Weigh data */
  FFT.compute(FFTDirection::Forward); /* Compute FFT */
  FFT.complexToMagnitude(); /* Compute magnitudes */

  // 周波数を算出
  double peak_value = 0.0; // ピーク値
  double peak_frequency = 0.0; // ピーク周波数

  for(int i=2; i<(SAMPLE/2); i++)
  {
    double frequency = i * 1.0 * SAMPLING_FREQUENCY / SAMPLE;

    if(EEPROMData.RPMUpper < frequency * 60 || frequency * 60 < EEPROMData.RPMLower )
      continue;

    if(vReal[i] > peak_value)
    {
      peak_value = vReal[i];
      peak_frequency = frequency;
    }
  }

  return peak_frequency;

}

// サンプリング
float sampling()
{

  int16_t accel_binary;
  unsigned long start_time = micros(); // Keep start time

  for (int i = 0; i < SAMPLE; i++)
  {
    while(micros() - start_time < uint32_t(i * round(1000000.0 / SAMPLING_FREQUENCY)));

    // 1 axis binary
    if(EEPROMData.MPUDirection == 0)
    {
      imu.getAccel_x_binary(&accel_binary);
    }
    else if(EEPROMData.MPUDirection == 1)
    {
      imu.getAccel_y_binary(&accel_binary);
    }
    else
    {
      imu.getAccel_z_binary(&accel_binary);
    }

    sample_binary[i] = accel_binary;
  }

  unsigned long end_time = micros(); // Keep the end time

  return 1000000.0 / float(end_time - start_time) * SAMPLE;

}

// ボタン割り込み処理(チャタリング防止込み)
void BtnApush()
{
  if(BtnApushedTime + 10 < millis())
  {
    BtnApushedTime = millis();
    BtnAcounter ++;
  }
}

void BtnBpush()
{
  if(BtnBpushedTime + 10 < millis())
  {
    BtnBpushedTime = millis();
    BtnBcounter ++;
  }
}


// 範囲チェック
void RangeCheck()
{

  // ギア比
  if(GearRatioMin > EEPROMData.GearRatio)EEPROMData.GearRatio = GearRatioMin;
  if(EEPROMData.GearRatio > GearRatioMax)EEPROMData.GearRatio = GearRatioMax;

  // タイヤ径
  if(TireDiameterMin > EEPROMData.TireDiameter)EEPROMData.TireDiameter = TireDiameterMin;
  if(EEPROMData.TireDiameter > TireDiameterMax)EEPROMData.TireDiameter = TireDiameterMax;

  // 回転数上限
  if(RPMMin > EEPROMData.RPMUpper)EEPROMData.RPMUpper = RPMMin;
  if(EEPROMData.RPMUpper > RPMMax)EEPROMData.RPMUpper = RPMMax;

  // 回転数下限
  if(RPMMin > EEPROMData.RPMLower)EEPROMData.RPMLower = RPMMin;
  if(EEPROMData.RPMLower > RPMMax)EEPROMData.RPMLower = RPMMax;

  // 検出レベル
  if(0 > EEPROMData.DetectionLevel)EEPROMData.DetectionLevel = 0;
  if(EEPROMData.DetectionLevel > 10)EEPROMData.DetectionLevel = 10;

  // 振動センサ サンプリング方向
  if(0 > EEPROMData.MPUDirection)EEPROMData.MPUDirection = 0;
  if(EEPROMData.MPUDirection > 2)EEPROMData.MPUDirection = 2;

}

// 測定結果表示
void MeasurementDisplay()
{
  // ボタン判定
  if(BtnBcounter != 0)
  {
    config_mode = 1;
    config_select = 0;
    BtnAcounter = 0;
    BtnBcounter = 0;
    return;
  }

  float vcc = analogRead(BTTpin);
  BtnBcounter = 0; // analogread時の誤作動防止

  float sampling_count = sampling();
  double fft_peak = FFT_run();

  M5.Display.startWrite();
  canvas.clear(TFT_BLACK);   // 画面初期化
  canvas.setTextColor(WHITE, BLACK); // 文字の色
  canvas.setTextSize(1);

  // バッテリー残量
  canvas.setFont(&fonts::Font4);  // フォント
  canvas.setCursor(180,5);
  canvas.print(vcc * 2 / 1120, 1);
  canvas.print('V');


  // ギア比
  canvas.setFont(&fonts::Font4);  // フォント
  canvas.setCursor(5,5);
  canvas.print("Gear");
  canvas.setCursor(90,5);
  canvas.print(":");
  canvas.setCursor(110,5);
  canvas.drawRightString(String(EEPROMData.GearRatio, 1),155,5,&fonts::Font4);

  // タイヤ径
  canvas.setFont(&fonts::Font4);  // フォント
  canvas.setCursor(5,30);
  canvas.print("Tire");
  canvas.setCursor(90,30);
  canvas.print(":");
  canvas.setCursor(110,30);
  canvas.drawRightString(String(EEPROMData.TireDiameter, 1),155,30,&fonts::Font4);
  canvas.setCursor(160,30);
  canvas.print(" mm");

  // 速度
  float speed_culc = fft_peak * 60 * 60 * EEPROMData.TireDiameter * M_PI / EEPROMData.GearRatio / 1000000;
  canvas.setFont(&fonts::Font4);  // フォント
  canvas.setCursor(5,55);
  canvas.print("Speed");
  canvas.setCursor(90,55);
  canvas.print(":");
  canvas.setCursor(110,55);
  canvas.drawRightString(String(speed_culc, 1),155,55,&fonts::Font4);
  canvas.setCursor(160,55);
  canvas.print(" km/h");

  // 振動強度
  canvas.setFont(&fonts::Font4);  // フォント
  canvas.setCursor(5,82);
  if(EEPROMData.MPUDirection == 0)
  {
    canvas.print("Dir : X");
  }
  else if(EEPROMData.MPUDirection == 1)
  {
    canvas.print("Dir : Y");
  }
  else
  {
    canvas.print("Dir : Z");
  }

  canvas.setCursor(5,105);
  canvas.print("rpm :");
  canvas.setCursor(70,90);
  canvas.drawRightString(String(fft_peak * 60, 0),220,90,&fonts::Font6);
 
  canvas.pushSprite(&M5.Display, 0, 0);
  M5.Display.endWrite();

}

// コンフィグ表示
void ConfigDisplay()
{
  // ボタン判定
  if(BtnAcounter != 0 || (digitalRead(BtnApin) == LOW && BtnApushedTime + 1000 < millis()))
  {
    if(config_select == 0)
    {
      config_mode --;
      if(config_mode <= 0)config_mode = 1;
    }
    else if(config_select == 1)
    {
      config_mode ++;
      if(config_mode >= 9)config_mode = 8;
    }
    else if(config_select == 2)
    {
      if(config_mode == 1)EEPROMData.GearRatio -= 0.1;
      if(config_mode == 2)EEPROMData.TireDiameter -= 0.1;
      if(config_mode == 3)EEPROMData.RPMUpper -= 1000;
      if(config_mode == 4)EEPROMData.RPMLower -= 1000;
      if(config_mode == 5)EEPROMData.DetectionLevel--;
      if(config_mode == 6)EEPROMData.MPUDirection--;
      if(config_mode == 8)
      {
        config_mode = 0;
        eeprom_write();
        return;
      }
      RangeCheck();
    }
    else if(config_select == 3)
    {
      if(config_mode == 1)EEPROMData.GearRatio += 0.1;
      if(config_mode == 2)EEPROMData.TireDiameter += 0.1;
      if(config_mode == 3)EEPROMData.RPMUpper += 1000;
      if(config_mode == 4)EEPROMData.RPMLower += 1000;
      if(config_mode == 5)EEPROMData.DetectionLevel++;
      if(config_mode == 6)EEPROMData.MPUDirection++;  
      RangeCheck();        
    }

    BtnAcounter = 0;
  }

  if(BtnBcounter != 0)
  {
    config_select++;
    if(config_mode == 7 && config_select >= 2)config_select = 0;
    if(config_mode == 8 && config_select >= 3)config_select = 0;
    if(config_select >= 4)config_select = 0;
    BtnBcounter = 0;
  }

  String MenuList[] = {"GearRatio", "TireDiameter", "RPM Upper", "RPM Lower", "Detection Level", "MPU Direction", "Version", "Back"};
  String MPUDirectionList[] = {"X", "Y", "Z"};
  // 描画関係
  M5.Display.startWrite();
  canvas.clear(TFT_BLACK);   // 画面初期化

  canvas.setTextSize(1);
  canvas.setTextColor(WHITE, BLACK); // 文字の色
  canvas.setFont(&fonts::Font4);  // フォント
  canvas.setCursor(5,5);
  canvas.print(MenuList[config_mode-1]);
  
  // 設定値表示
  canvas.setTextSize(2);
  canvas.setCursor(10,50);
  if(config_mode == 1)canvas.print(EEPROMData.GearRatio, 1);
  if(config_mode == 2)canvas.print(EEPROMData.TireDiameter, 1);
  if(config_mode == 3)canvas.print(EEPROMData.RPMUpper, 0);
  if(config_mode == 4)canvas.print(EEPROMData.RPMLower, 0);
  if(config_mode == 5)canvas.print(EEPROMData.DetectionLevel);
  if(config_mode == 6)canvas.print(MPUDirectionList[EEPROMData.MPUDirection]);  
  if(config_mode == 7)canvas.print(VERSION);


  // コントローラ表示
  canvas.setTextSize(2);
  if(config_select == 0)
  {
    canvas.setTextColor(GREEN, BLACK); // 文字の色
  }
  else
  {
    canvas.setTextColor(WHITE, BLACK); // 文字の色
  }
  canvas.setCursor(10,90);
  canvas.print("<-");

  if(config_select == 1)
  {
    canvas.setTextColor(GREEN, BLACK); // 文字の色
  }
  else
  {
    canvas.setTextColor(WHITE, BLACK); // 文字の色
  }
  canvas.setCursor(70,90);
  canvas.print("->");

  if(config_select == 2)
  {
    canvas.setTextColor(GREEN, BLACK); // 文字の色
  }
  else
  {
    canvas.setTextColor(WHITE, BLACK); // 文字の色
  }

  if(config_mode == 7)
  {
    // 表示なし
  }
  else if(config_mode == 8)
  {
    canvas.setCursor(140,100);
    canvas.setTextSize(1);
    canvas.print("Save");
  }
  else
  {
    canvas.setCursor(140,90);
    canvas.print("-");
  }

  if(config_select == 3)
  {
    canvas.setTextColor(GREEN, BLACK); // 文字の色
  }
  else
  {
    canvas.setTextColor(WHITE, BLACK); // 文字の色
  }

  canvas.setCursor(200,90);
  if(config_mode == 7)
  {
    // 表示なし
  }
  else if(config_mode == 8)
  {
    // 表示なし
  }
  else
  {
    canvas.print("+");
  }

  canvas.pushSprite(&M5.Display, 0, 0);
  M5.Display.endWrite();

}

void setup() {

  // 動作周波数変更
  setCpuFrequencyMhz(120);

  // EEPROMセット
  EEPROM.begin(1024); // EEPROM開始(サイズ指定)
  EEPROM.get<EEPROMStruct>(0, EEPROMData);

  // EEPROMのバージョンが違っていたら初期値に置き換え
  if(strcmp(EEPROMData.EEPROMVersion, EEPROM_VERSION) != 0)
  {
    strcpy(EEPROMData.EEPROMVersion, EEPROM_VERSION); // EEPROM version
    EEPROMData.GearRatio = 3.5; // ギア比
    EEPROMData.TireDiameter = 23.5; // タイヤ径
    EEPROMData.RPMUpper = 40000.0; // 回転数上限
    EEPROMData.RPMLower = 20000.0; // 回転数下限
    EEPROMData.DetectionLevel = 3; // 検出レベル
    EEPROMData.MPUDirection = 0; // 振動センサ サンプリング軸
  }

  RangeCheck(); // 範囲チェック

  // 画面初期化
  auto cfg = M5.config(); // 設定用の構造体を代入。
  cfg.pmic_button = false;
  cfg.internal_imu = false;
  cfg.internal_rtc =false;
  M5.begin(cfg); // 設定した値でデバイスを開始する。

  canvas.setPsram(false);
  canvas.createSprite(M5.Display.height(), M5.Display.width());

  M5.Display.setRotation(1);          // 画面の向き

  canvas.setColorDepth(8);
  canvas.setFont(&fonts::Font4);             // フォント
  canvas.setTextSize(1);
  canvas.setTextDatum(0);            // 文字の位置
  canvas.setTextColor(WHITE, BLACK); // 文字の色
 
  // imu 初期化
  Wire1.begin(25, 21);    // I2C pin settings
  Wire1.setClock(400000); // Set the I2C speed to 400kHz

  // Sensor range setting (do before begin)
  // ACCEL_CONFIG
  // 0:2g 1:4g 2:8g 3:16g [default 2:8g]
  imu.accel_config = 2;
  // GYRO_CONFIG
  // 0:250dps 1:500dps 2:1000dps 3:2000dps [default 3:2000dps]
  imu.gyro_config = 3;

  imu.begin(); // Initialization of MPU6886

  // ボタンの割り込み設定
  pinMode(BtnApin,INPUT_PULLUP);
  pinMode(BtnBpin,INPUT_PULLUP);
  pinMode(BTTpin,INPUT);
   
  //2番ピンが押された瞬間に関数Pushed()が実行されるように設定する
  attachInterrupt(digitalPinToInterrupt(BtnApin), BtnApush, FALLING);
  attachInterrupt(digitalPinToInterrupt(BtnBpin), BtnBpush, FALLING);

}

void loop() 
{

  if(config_mode == 0)
  {
    MeasurementDisplay(); // 測定結果表示
  }
  else
  {
    ConfigDisplay();
  }

  // ボタン押し込み判定
  delay(100);

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