Magic Wand -Spresenseでつくる魔法の杖-

はじめに

ホグワーツレガシーというゲームをご存知でしょうか。
ハリーポッターの世界を舞台にしたオープンワールドゲームなのですが、このゲームで遊んでいたところ、すっかりハマってしまいました。
改めて映画ハリーポッターシリーズを全作見返し、ファンタスティックビーストシリーズも全て見て、今ではハリポタオタク？です。
さまざまな魔法を唱え難関を乗り越えていくのですが、中でも、「ルーモス」という杖に光を灯し、周囲を照らす魔法はさまざまなシーンで唱えられ、とても便利そうです。

ルーモスやりたい。ルーモスやりたすぎるぞ！！？

すっかりルーモスの虜です。これはもう、ルーモスを実現するしかありません。

果たしてどのようにして魔法を実現するか。
どうやら魔法を唱えるには杖の軌跡が重要らしく、ゲームではさまざまな軌跡を描き魔法を習得します。
魔法によって描く杖の軌跡が異なるのがポイントです。

また、杖には種類があり、長さや素材によって特性が異なるのだそうで、
魔法界史上最強の杖と云われる「ニワトコの杖」は一般的には不可能とされる魔法を繰り出すことができる、実に手に入れたい逸品です。

そこで、本稿ではマグル代表として、SpresenseのエッジAI機能と9DoFセンサアドオンボードを活用して最強の魔法の杖によるルーモスの実現を試みます。

動作

今回制作した魔法の杖です。

準備物

TFT液晶の組み付け

TFT液晶は杖の軌跡をデバイス上で確認するために使用します。
今回は、秋月電子で購入できるＩＬＩ９３４１搭載２．８インチＳＰＩ制御タッチパネル付ＴＦＴ液晶　ＭＳＰ２８０７を使用します。
今回使用するTFT液晶は3.3Vで動作するため、拡張ボードのインターフェイス電圧を3.3Vに切り替えます。Spresense拡張ボードのジャンパーピンを3.3Vにセットしてください。

Spresenseメインボードと拡張ボードの接続に関してはこちらを参照ください。嵌合が不完全でSDカードが読み込めないなどの現象が発生するので、しっかりと組み付けたことを確認します。

TFTの描画にはLGFXを使用しました。Adafluitのライブラリよりも高速に描画できます。
参考記事

ToFセンサの接続

床面からの距離を計測するためにToFセンサを使用します。ToFとはTime of Flightの略で、今回使用するMM-S50MVはレーザーと複数の受光素子により高精度3D距離測定が可能な代物です。また、裏面にカラーLEDが搭載されプログラムで色を可変させられます。今回は、杖が人を選ぶという劇中シナリオに準拠して、人によって演出を変える試みをします。具体的には、床面からの距離に応じて音色を可変するようにします。
https://shop.sunhayato.co.jp/products/mm-s50mv からマニュアルをダウンロードできます。

スイッチの接続

スイッチをメインボードのD21とGNDに接続します。

簡易ケースの作成

ToFセンサやTFT液晶がぷらぷら動くと、作業性が悪いので簡易ケースを製作します。濯いで乾かした牛乳パックがぴったりサイズです。TFT液晶はM3ねじ＋ナット、ToFは切り込みに嵌め込んで固定しました。

9DoF アドオンボードの組み付け

次に9DoFアドオンボードを組み付けます。9DoFとは9 degree of freedomの略で、加速度センサのxyz 3軸、角速度センサ(ジャイロセンサ)のxyz3軸、地磁気センサのxyz 3軸、合計9軸を意味します。今回使用するボードにはBMI270とAK09918の2つのセンサが搭載されています。
BMI270のセンサ値読み取りはArduinoライブラリがあるので、そちらを利用します。AK09918はhttps://github.com/Seeed-Studio/Seeed_ICM20600_AK09918 からAK09918.h、AK09918.cpp、I2Cdev.h、I2Cdev.cppをダウンロードして、作成しているプログラムと同じフォルダに入れることで利用できます。

ここまででハードウェアの準備は完了です。

杖軌跡検出の方針

さて、ここから杖の軌跡をどのように検出するか検討します。
もちろん、SpresenseのエッジAI機能を活用するわけですが、どのようなデータを学習させるのかが今回のポイントです。

ディープラーニングでなんらかのデータを扱うためには、なんらかの形で入力ニューロンにデータを入力する必要があります。
まず思いつくのは、波形の振幅をそのままニューラルネットワークの入力する方法です。
基本的にはEnd to end、つまり、前処理など人の手を介さずに生の入力データから、所望の出力を直接得られるように学習したほうが高い性能が得られるというセオリーがありますが、今回のように杖の軌跡を描く時間数×9軸センサのデータを入力すると、計算量やメモリ使用量が膨大になりSpresenseでは処理しきれなくなる可能性があります。
また、杖の軌跡データは一人で収集するため、収集できるデータ数が限られ、性能が出ないことも懸念されます。

そこで今回は、杖の軌跡を2D空間上にプロットし、プロットしたx,yの座標情報を入力データとして学習します。
この方法であれば、杖を振る速さのばらつきを学習する必要がなくなりますし、書き順を気にする必要もありません。
なにより、メモリと必要なデータ数を低減することが可能となります。
計測したデータを画像のように絵として確認することができるので、データをカテゴリわけするときに間違えるリスクを低減できるメリットもあります。
そして、これはCNN(Convolution Neural Network)による画像分類問題と同じ手法で解くことができます。この記事で使用している学習モデルの小変更で9DoFセンサデータの分類問題を解けるということです。

ソフトウェア実行の準備

Madgwickのインストール

MadgwickAHRSアルゴリズムを使用して姿勢を推定します。(AHRSはAttitude Heading Reference Systemの略)
Arduino LibraryからMgdgwickを検索しイントールすることで簡単に使用することができます。

加速度センサ、角速度(ジャイロ)センサの補正

正確な姿勢を検出するためにセンサの補正を行います。
加速度センサは個体ごとにばらつきを有するため、各方向へ向けて値を計測し補正式へ入れることで補正量を算出します。

xyz軸それぞれでgainとoffsetを補正値として持たせます。以下の公式に当てはめて計算します。
1G印加時の補正前センサ出力値：g＋,
-1G印加時の補正前センサ出力値：gー
offset = { (g＋ ) ＋ ( g- ) } / 2
gain = 2 / { (g＋)ー(g-) }

後述するプログラムに補正値を反映します。

IMU.ino
//加速度センサ補正値
float acc_gainX = 1.01010101010101;
float acc_gainY = 1.01010101010101;
float acc_gainZ = 1;
float acc_offsetX = 0;
float acc_offsetY = -0.01;
float acc_offsetZ = 0;

ジャイロセンサはゼロ点誤差(ドリフト)が生じるため、起動時に100回計測して平均値を用いてゼロ点補正を行います。

地磁気は屋内の金属や磁石に反応して誤差の原因となったため、使用していません。

以下のソフトをArduinoIDEで書き込みます。
ArduinoIDEとはオープンソースハードウェアArduino用の開発環境です。
SpresenseではArduinoIDEがサポートされており、今回はこれを使います。
ArduinoIDEのインストールについてはこちらをご参照ください。

ArduinoIDEと必要なライブラリのインストールができたらデータ記録用ソフトウェアを書き込みます。
このソフトは#define RECORD_MODE 1//0:推論モード,1:学習モードを切り替えることで、学習・推論両対応しています。
なお、記事掲載スペースの関係上、全てのプログラムを掲載すると膨大なのでTFT液晶の描画やスイッチ操作などを扱うクラスは割愛します。
Githubに全てのプログラムを掲載していますのでこちらを確認してください。

Spresense_MagicWand.ino
#define RECORD_MODE 1//0:推論モード,1:学習モード
//SD
#include <SDHCI.h>
SDClass  SD;
#include <File.h>

//IMU
#include "Arduino_BMI270_BMM150.h"

//TFT
#define TFT_LED 8       //LED接続端子
#define LGFX_AUTODETECT // 自動認識
#include <LovyanGFX.hpp>
#include "LGFX_SPRESENSE.hpp"
#include <LGFX_AUTODETECT.hpp>        // クラス"LGFX"を準備します
static LGFX_SPRESENSE_SPI_ILI9341 tft;// LGFXのインスタンスを作成

//ToF
#include <SPI.h>

//Canvas
#include "CANVAS.h"
CANVAS *canvas1;
CANVAS *canvas2;
CANVAS *canvas3;
CANVAS *canvas4;
CANVAS *canvas5;
CANVAS *canvas6;

//AK09918
#include "AK09918.h"
#include <Wire.h>
AK09918_err_type_t err;
int32_t x, y, z;
AK09918 ak09918;
int mx,my,mz;
int mx0,my0,mz0;

//SW
#include "SW.h"
SW *switch1;

//MadgWick
#include <MadgwickAHRS.h>
#define INTERVAL 100000 //us
float roll=0;
float pitch=0;
float heading=0;
Madgwick *filter;

//DNN
#include <DNNRT.h>
DNNRT dnnrt;
#define DNN_DATA_WIDTH 28
#define DNN_DATA_HEIGHT 28
DNNVariable input(DNN_DATA_WIDTH*DNN_DATA_HEIGHT);
static String const labels[4]= {"EIGHT", "CIRCLE", "MINUS", "NON"};
int command =3;

//mode
// モードの列挙型
enum MODE {
  MODE1,
  MODE2,
  MODE3,
  MODE4
};
MODE currentMode = MODE4;
MODE beforeMode = MODE4;

int TaskSpan;       //タスク実行間隔
uint32_t startTime; //開始時間
uint32_t cycleTime; //サイクルタイム
uint32_t deltaTime; //
uint32_t spentTime; //経過時間
bool _InitCondition=false;  //初期化状態
bool _DeinitCondition=false;  //終了時状態


//Audio
#include <Audio.h>
AudioClass *theAudio;
bool ErrEnd = false;

static void audio_attention_cb(const ErrorAttentionParam *atprm)
{
  puts("Attention!");
  
  if (atprm->error_code >= AS_ATTENTION_CODE_WARNING)
    {
      ErrEnd = true;
   }
}

File myFile;
bool audio=false;
void PlaySound(int i){
    /* Open file placed on SD card */
  if(i==1)myFile = SD.open("audio1.mp3");
  else if(i==2)myFile = SD.open("audio2.mp3");
  else if(i==3)myFile = SD.open("audio3.mp3");
  
  if (!myFile)
    {
      printf("File open error\n");
      exit(1);
    }
  Serial.println(myFile.name());

  /* Send first frames to be decoded */
  err_t err = theAudio->writeFrames(AudioClass::Player0, myFile);

  if (err == AUDIOLIB_ECODE_FILEEND)
  {
    theAudio->stopPlayer(AudioClass::Player0);
  }
  if ((err != AUDIOLIB_ECODE_OK) && (err != AUDIOLIB_ECODE_FILEEND))
    {
      printf("File Read Error! =%d\n",err);
      myFile.close();
      exit(1);
    }


  /* Main volume set -1020 to 120 */
  theAudio->setVolume(120);

  theAudio->startPlayer(AudioClass::Player0);
  audio = true;

  myFile.close();
  //usleep(40000);
}
int CheckCommand(){
  float *dnnbuf = input.data();
  int count=0;
  for (int i=0;i<DNN_DATA_WIDTH;i++) {
    for (int j=0;j<DNN_DATA_HEIGHT;j++) {
      dnnbuf[count] = canvas4->output[i + 28 * j];
      count++;
    }
  }

  dnnrt.inputVariable(input,0);
  dnnrt.forward();

  DNNVariable output = dnnrt.outputVariable(0);
  int index = output.maxIndex();
  return index;
}



void setup() {
  Serial.begin(9600);
  //TFT
  TFT_Init();
  
  switch1 = new SW(PIN_D21,INPUT_PULLUP);
  //タイマ割り込み
  attachTimerInterrupt(TimerInterruptFunction,INTERVAL);

  //CANVAS(tft,w,h,x,y)
  canvas1 = new CANVAS(&tft,80,40,20,240);      //ToFText
  canvas2 = new CANVAS(&tft,20,40,0,240);       //ToFドット
  //canvas3 = new CANVAS(&tft,140,40,100,240);    //グラフ
  canvas4 = new CANVAS(&tft,240,240,0,0);    //杖軌跡
  canvas5 = new CANVAS(&tft,240,280,0,280);   //テキスト
  canvas6 = new CANVAS(&tft,140,40,100,240);      //Text


  //SD
  SD.begin();
  //USB MSC
  if (SD.beginUsbMsc()) {
    Serial.println("USB MSC Failure!");
  } else {
    Serial.println("*** USB MSC Prepared! ***");
    Serial.println("Insert SD and Connect Extension Board USB to PC.");
  }
  //DNN
  File nnbfile = SD.open("model.nnb");
  int ret = dnnrt.begin(nnbfile);
  if (ret < 0) {
    Serial.println("dnnrt.begin failed" + String(ret));
  }
  
  //ToF
  SPI5.begin();
  SPI5.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3));
  TOF_Init();

  //IMU
  IMU_Init();

  //AK09918
  AK09918_Init();

  //ジャイロセンサ
  GyroInit();

  //MadgWick
  MadgWick_Init();

  //Audio
  theAudio = AudioClass::getInstance();
  theAudio->begin(audio_attention_cb);
  puts("initialization Audio Library");
  /* Set clock mode to normal ハイレゾかノーマルを選択*/
  theAudio->setRenderingClockMode(AS_CLKMODE_NORMAL);
  /*select LINE OUT */
  theAudio->setPlayerMode(AS_SETPLAYER_OUTPUTDEVICE_SPHP, AS_SP_DRV_MODE_LINEOUT);
  /*init player DSPファイルはmicroSD カードの場合は "/mnt/sd0/BIN" を、SPI-Flash の場合は "/mnt/spif/BIN" を指定します。*/
  err_t err = theAudio->initPlayer(AudioClass::Player0, AS_CODECTYPE_MP3, "/mnt/sd0/BIN", AS_SAMPLINGRATE_AUTO, AS_CHANNEL_STEREO);
  /* Verify player initialize */
  if (err != AUDIOLIB_ECODE_OK)
    {
      printf("Player0 initialize error\n");
      exit(1);
    }
    
}


void CANVAS_main(){
  //ToFセンサテキスト描画
  int* range = TOF_ReadDistance3d();
  canvas1->StringDrawL(String(TOF_ReadDistance())+"mm",TFT_WHITE);
  //ToFセンサドット描画
  canvas2->DotDraw(range);
  //姿勢グラフ描画
  //float value1[] = {roll,pitch,heading};
  //canvas3->GraphDraw(value1);
  //杖軌跡描画
  canvas4->WandDraw28(heading,roll);
  //IMU,地磁気センサ値テキスト描画
  canvas5->StringDraw("aX="+ String(IMU_ReadAccX())+",aY="+String(IMU_ReadAccY())+",aZ="+String(IMU_ReadAccZ())+"\ngX="+ String(IMU_ReadGyroX())+",gY="+String(IMU_ReadGyroY())+",gZ="+String(IMU_ReadGyroZ())+"\nmX="+ String(mx)+",mY="+String(my)+",mZ="+String(mz), TFT_YELLOW);
  //
  canvas6->StringDrawL(String(labels[command])+"",TFT_WHITE);
}




void mainloop(MODE m){
  //前回実行時からモードが変わってたら終了処理
  if(beforeMode != m)DeinitActive();
  beforeMode = m;
   //前回実行時からの経過時間を計算
  deltaTime = millis() - cycleTime;
  //経過時間を計算
  spentTime = millis() - startTime;

  if(deltaTime >= TaskSpan){
//    Serial.println("mainloop");
    if(!_InitCondition){//未初期化時実行
//      Serial.println("Init");
      InitFunction(m);
    }else if(_DeinitCondition){//修了処理時実行
//      Serial.println("Deinit");
      DeinitFunction();
    }else{
//      Serial.println("main()");
        //モードごとの処理
    switch (m) {
      case MODE1:
        //TOF_SetLED(255,255,255);      
        break;

      case MODE2:
        //TOF_SetLED(255,0,0);
        break;

      case MODE3:
        //TOF_SetLED(0,255,0);
        currentMode = MODE4;
        
        break;

      case MODE4:
        TOF_SetLED(0,0,0);
        break;
      }
      
    }
    //cycleTimeリセット
    cycleTime = millis();
  }
}

void DeinitActive(){//モード終了時に呼ぶ
  startTime = millis();
  //モードごとの処理
  _DeinitCondition=true;
}
void DeinitFunction(){//最後に呼ばれる
  
  _InitCondition=false;  //初期化状態
  _DeinitCondition=false;  //終了時状態
}
void InitFunction(MODE m){//初回呼ぶ
  //モードごとの処理
  switch (m) {
  case MODE1:
    TOF_SetLED(255,255,255);
    PlaySound(1);
    break;

  case MODE2:
    TOF_SetLED(255,0,0);
    PlaySound(2);
    break;

  case MODE3:
    TOF_SetLED(0,255,0);
    PlaySound(3);
    break;

  case MODE4:
    TOF_SetLED(0,0,0);
    break;
}
  startTime = millis();
  _InitCondition=true;  //初回フラグon
}

void Audio_main(){
  int err = theAudio->writeFrames(AudioClass::Player0, myFile);
  /*  Tell when player file ends */
  if (err == AUDIOLIB_ECODE_FILEEND)
    {
      //printf("Main player File End!\n");
    }

  /* Show error code from player and stop */
  if (err)
    {
      //printf("Main player error code: %d\n", err);
      if(audio == true){
        theAudio->stopPlayer(AudioClass::Player0);
        myFile.close();
        audio = false;
      }
    }
}


void loop() {
  IMU_main();         //IMUセンサ値更新
  TOF_main();         //TOFセンサ値更新
  AK09918_main();     //地磁気センサ更新
  CANVAS_main();      //描画更新
  if(RECORD_MODE == 0)command = CheckCommand(); //DNN
  SW_main();         //ボタンチェック(押下時Reset処理)
  Audio_main();      //オーディオ
  Serial_main();      //Arduinoシリアル操作

  //モード起動時処理
  if(RECORD_MODE == 0){
    if(IMU_CheckAccActive()&& TOF_ReadDistance()>50){
      if(command==0){
        currentMode = MODE1;
        ResetCanvas();
      }
      if(command==1){
        currentMode = MODE2;
        ResetCanvas();
      }
      if(command==2){
        currentMode = MODE3;
        ResetCanvas();
      }
    }
  }
  mainloop(currentMode);

  //所定の加速度より早い場合キャンバスを消す
  if(IMU_CalcAccVec(IMU_ReadAccX(),IMU_ReadAccY(),IMU_ReadAccZ())>1.5){
    //Serial.println(IMU_CalcAccVec);
    //currentMode = MODE4;
    ResetCanvas();
  }


}
//--------------------------------------
//  ToFセンサ値処理
//--------------------------------------
int TOF_dis = 0;
int TOF_range3d[8][4] = {{0}};

int ledr = 0;
int ledg = 0;
int ledb = 0;

static  int oldseq = -1;

const float alpha = 0.3;  //LED LPF
int r_filtered = 0;
int g_filtered = 0;
int b_filtered = 0;
void TOF_SetLED(int r,int g ,int b){

// フィルタ処理
  r_filtered = alpha * r + (1 - alpha) * r_filtered;
  g_filtered = alpha * g + (1 - alpha) * g_filtered;
  b_filtered = alpha * b + (1 - alpha) * b_filtered;
  
  ledr = r_filtered;
  ledg = g_filtered;
  ledb = b_filtered;
}
void TOF_SetLedNow(int r,int g ,int b){
  ledr = r;
  ledg = g;
  ledb = b;
}
int TOF_ReadDistance(){
  return TOF_dis;
}

int* TOF_ReadDistance3d(){
  return &TOF_range3d[0][0];
}
// SPIから1バイトデータを受信する関数
static  inline  int TOF_spigetb()
{
  return SPI5.transfer(0) & 0xff;
}

// SPIから2バイトデータを受信する関数
static  int TOF_spigeth()
{
  int i;
  
  i = TOF_spigetb() << 8;
  i |= TOF_spigetb();
  return i;
}

// SPIから4バイトデータを受信する関数
static  int TOF_spigetw()
{
  int i;
  
  i = TOF_spigetb() << 24;
  i |= TOF_spigetb() << 16;
  i |= TOF_spigetb() << 8;
  i |= TOF_spigetb();
  return i;
}

// 指定されたバイト数だけSPIからデータを受信し、読み捨てる関数
static  void  TOF_spiskip(int len)
{
  while (len > 0) {
    TOF_spigetb();
    len--;
  }
}
// モジュールにコマンドを送信する関数
static  void  TOF_spicommandinner(int cmd, int val)
{
  SPI5.transfer(0xeb);
  SPI5.transfer(cmd);
  SPI5.transfer(1);
  SPI5.transfer(val);
  SPI5.transfer(0xed);
}

// モジュールにコマンドを送信し、読み捨てる関数
static  void  TOF_spicommand(int cmd, int val)
{
  TOF_spicommandinner(cmd, val);
  TOF_spiskip(256 - 5);
}

void TOF_ledIndicateMain(int dis){
  //Spresense LED表示処理
    char  c;
    switch (dis / 200) {
      case    0:
        c = 0;
        break;
      case    1:
        c = 1;
        break;
      case    2:
        c = 3;
        break;
      case    3:
        c = 7;
        break;
      case    4:
        c = 0xf;
        break;
      case    5:
        c = 0xe;
        break;
      case    6:
        c = 0xc;
        break;
      case    7:
      default:
        c = 8;
        break;
    }
    digitalWrite(LED0, (c & 1)? HIGH : LOW);
    digitalWrite(LED1, (c & 2)? HIGH : LOW);
    digitalWrite(LED2, (c & 4)? HIGH : LOW);
    digitalWrite(LED3, (c & 8)? HIGH : LOW);
}
void TOF_Init(){
  Serial.println("TOF_Init");
  TOF_spiskip(256);
  delay(500);
  TOF_spicommand(0, 0xff);    // sync mode.
  TOF_spicommand(0x12,0);    // 6m mode.
  delay(500);
  TOF_spiskip(256);
  TOF_spiskip(TOF_spigetb());     // sync.
  TOF_spicommand(0, 0);       // normal mode.
  delay(500);

//  TOF_spicommand(0x10, 0x40); // 64frames/s
//  TOF_spicommand(0x10, 0x80); // 128frames/s
  TOF_spicommand(0x10, 0); // 256frames/s
//  TOF_spicommand(0x11, 0x40); // 320frames/s
//  TOF_spicommand(0x11, 1); // 5frames/s
  delay(500);
  TOF_spiskip(256);
}

void TOF_main(){

    static  int count = 0;

    //static  int ledr = 0;
    //static  int ledg = 0;
    //static  int ledb = 0;
  
    //SPI通信仕様に従う
    int magic0 = TOF_spigetb();//1byte
    int seq0 = TOF_spigetb();   //1byte
    TOF_spiskip(2);   //2byte
    int range = TOF_spigetw();//1Dの距離 //4byte(32bit)
    int light = TOF_spigeth();//1Dの光量 //2byte(16bit)

    for(int i=0;i<8;i++){
      //Serial.print("["+String(i)+"]");
      for(int j=0;j<4;j++){
        int range3d = TOF_spigetw();  //4byte*32(32bit*32)
        TOF_range3d[i][j] = range3d / (0x400000 / 1000);
        //Serial.print(TOF_range3d[i][j]);
        //Serial.print(" ");
      }
      //Serial.println("");
    }
    
//    TOF_spiskip(256 - 9); // 256 - 1 -8byte
    TOF_spicommandinner(0xc0, ledr);
    TOF_spicommandinner(0xc1, ledg);
    TOF_spicommandinner(0xc2, ledb);
    TOF_spiskip(256 - 9 - 2 - 4 * 32 - 5 * 3);
    
    int seq1 = TOF_spigetb();
    
    //Serial.println("magic=" + String(magic0) + ",seq0=" + String(seq0) + ",range=" + String(range) + ",seq1=" + String(seq1));
    //Check Value
    bool ResetFlag=false;
    if (magic0 != 0xe9)//データ同期失敗時
      ResetFlag=true;
    if (seq0 != seq1)//データブロック無効時
      ;//ResetFlag=true;
    if (oldseq < 0)
      ;
    else if (((oldseq - seq0) & 0x80) == 0)
      ;//ResetFlag=true;
    oldseq = seq0;

    if(ResetFlag){
      TOF_Init();
    }

    int dis = range / (0x400000 / 1000);
    TOF_dis = dis;
    if ((0)) {
      Serial.print(count++);
      Serial.print("(");
      Serial.print(seq0, HEX);
      Serial.print("): ");
      
      Serial.print(dis);
      Serial.print("mm\n");
    }
    TOF_ledIndicateMain(dis);
    
  
}
//--------------------------------------
//  9軸センサ値処理
//--------------------------------------

float IMU_x, IMU_y, IMU_z;
float IMU_mx,IMU_my,IMU_mz;
float IMU_gx,IMU_gy,IMU_gz;
float IMU_gxAve,IMU_gyAve,IMU_gzAve;
#define NUM 3 //移動平均回数
float IMU_gyroX[NUM];
float IMU_gyroY[NUM];
float IMU_gyroZ[NUM];
int IMU_index = 0;               // readings 配列のインデックス
float IMU_totalX = 0;               // 読み取り値の合計
float IMU_averageX = 0;             // 移動平均値
float IMU_totalY = 0;               // 読み取り値の合計
float IMU_averageY = 0;             // 移動平均値
float IMU_totalZ = 0;               // 読み取り値の合計
float IMU_averageZ = 0;             // 移動平均値
float IMU_offsetX = 0;
float IMU_offsetY = 0;
float IMU_offsetZ = 0;

//加速度センサ補正値
float acc_gainX = 1.01010101010101;
float acc_gainY = 1.01010101010101;
float acc_gainZ = 1;
float acc_offsetX = 0;
float acc_offsetY = -0.01;
float acc_offsetZ = 0;


void IMU_Init(){
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }
  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println(" Hz");
  Serial.print("Magnetic field sample rate = ");
  Serial.print(IMU.magneticFieldSampleRate());
  Serial.println(" uT");
  Serial.println();
  Serial.print("Gyroscope sample rate = ");
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Gyroscope in degrees/second");
  Serial.println("X\tY\tZ");
  
  //配列初期化
  for(int i=0;i<NUM;i++){
    IMU_gyroX[i]=0;
    IMU_gyroY[i]=0;
    IMU_gyroZ[i]=0;
  }
  Serial.print("Gyro init.Put Device");
  for(int i=0;i<NUM;i++){
    IMU_main();
    Serial.print(".");
    delay(10);
  }

  IMU_Reset();
  Serial.println("Finish");
}
//ジャイロセンサの平均値を求める
void IMU_CalcAverage(float valueX,float valueY,float valueZ){
  
  // 過去の読み取り値の合計から古い値を引く
  IMU_totalX = IMU_totalX - IMU_gyroX[IMU_index];
  IMU_totalY = IMU_totalY - IMU_gyroY[IMU_index];
  IMU_totalZ = IMU_totalZ - IMU_gyroZ[IMU_index];

  // 最新のセンサの値を readings 配列に追加
  IMU_gyroX[IMU_index] = valueX;
  IMU_gyroY[IMU_index] = valueY;
  IMU_gyroZ[IMU_index] = valueZ;

  // 過去の読み取り値の合計に新しい値を加える
  IMU_totalX = IMU_totalX + IMU_gyroX[IMU_index];
  IMU_totalY = IMU_totalY + IMU_gyroY[IMU_index];
  IMU_totalZ = IMU_totalZ + IMU_gyroZ[IMU_index];

  // 次の readings 配列のインデックスを計算
  IMU_index = (IMU_index + 1) % NUM;

  // 移動平均を計算
  IMU_averageX = IMU_totalX / NUM;
  IMU_averageY = IMU_totalY / NUM;
  IMU_averageZ = IMU_totalZ / NUM;
}

float IMU_CalcAccVec(float x,float y,float z){
    return abs(sqrt(x*x+y*y+z*z) - 1);
}

float CorrectAccX(float a){
  return acc_gainX*(a+acc_offsetX);
}
float CorrectAccY(float a){
  return acc_gainY*(a+acc_offsetY);
}
float CorrectAccZ(float a){
  return acc_gainZ*(a+acc_offsetZ);
}

float IMU_ReadAccX(){
  return CorrectAccX(IMU_x);
}
float IMU_ReadAccY(){
  return CorrectAccY(IMU_y);
}
float IMU_ReadAccZ(){
  return CorrectAccZ(IMU_z);
}

float IMU_ReadMagX(){
  return IMU_mx;
}
float IMU_ReadMagY(){
  return IMU_my;
}
float IMU_ReadMagZ(){
  return IMU_mz;
}
float IMU_ReadGyroX(){
  return IMU_gx;
}
float IMU_ReadGyroY(){
  return IMU_gy;
}
float IMU_ReadGyroZ(){
  return IMU_gz;
}
float IMU_ReadAveGyroX(){
  return IMU_averageX - IMU_offsetX;
}
float IMU_ReadAveGyroY(){
  return IMU_averageY - IMU_offsetY;
}
float IMU_ReadAveGyroZ(){
  return IMU_averageZ - IMU_offsetZ;
}
void IMU_Reset(){
  IMU_offsetX = IMU_averageX;
  IMU_offsetY = IMU_averageY;
  IMU_offsetZ = IMU_averageZ;
}

//IMU Reset Check
float IMU_startTime = 0;
bool countflag = false;
bool gflag =false;
const float AccValue = 0.04; //閾値
const int SpentTime = 150;

bool IMU_CheckAccActive(){
  return gflag;
}
void IMU_main(){

  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(IMU_x, IMU_y, IMU_z);
  }
  if (IMU.magneticFieldAvailable()) {
    IMU.readMagneticField(IMU_mx, IMU_my, IMU_mz);
    Serial.println("MagneticField_OK");
  }
    if (IMU.gyroscopeAvailable()) {
    IMU.readGyroscope(IMU_gx, IMU_gy, IMU_gz);

  }

  //ジャイロセンサ値移動平均処理
  IMU_CalcAverage(IMU_gx, IMU_gy, IMU_gz);
  
  //静止判定
  if(IMU_CalcAccVec(IMU_ReadAccX(),IMU_ReadAccY(),IMU_ReadAccZ()) < AccValue){
    if(!countflag){
      IMU_startTime = millis(); // カウンター開始前であればStartTimeを記録
      countflag = true;
    }
    if(millis() - IMU_startTime > SpentTime){
      gflag=true;
    }
    else{
      gflag=false;
    }
  }else{
    countflag = false; //閾値の以上なら
    gflag = false;     // IMUの値が閾値以上ならgflagをfalseに設定
  }

}
//--------------------------------------
//  9軸センサによる姿勢推定
//--------------------------------------

//センサからの読み出し値
float accx=0; //加速度x
float accy=0; //加速度y
float accz=0; //加速度z
float magx=0; //地磁気x
float magy=0; //地磁気y
float magz=0; //地磁気z
float avelx=0; //ジャイロセンサx
float avely=0; //ジャイロセンサy
float avelz=0; //ジャイロセンサz

//ジャイロセンサ補正値(Initで算出)
float avelx0=0;
float avely0=0;
float avelz0=0;

//姿勢値オフセット変数
float head0=0;
float roll0=0;
float pitch0=0;

//ジャイロセンサドリフト補正  
void GyroInit(){
  Serial.print("Gyro Init");
  for(int i=0 ;i<100;i++){
    //加算
    avelx0 +=avelx;
    avely0 +=avely;
    avelz0 +=avelz;
    Serial.print("x");
  }
  
    avelx0 = avelx0/100;
    avely0 = avely0/100;
    avelz0 = avelz0/100;
    //初期値を表示
    Serial.println("Finished");
    Serial.print("GyroX:"+ String(avelx0));
    Serial.print(",GyroY:"+ String(avely0));
    Serial.println(",GyroZ:"+ String(avelz0));
}

//タイマー割り込み関数
unsigned int TimerInterruptFunction() {
  avelx = IMU_ReadAveGyroX();
  avely = IMU_ReadAveGyroY();
  avelz = IMU_ReadAveGyroZ();
  accx = IMU_ReadAccX();
  accy = IMU_ReadAccY();
  accz = IMU_ReadAccZ();
  magx = 0;//mx;//屋内では地磁気センサ誤動作するため0
  magy = 0;//my;//屋内では地磁気センサ誤動作するため0
  magz = 0;//mz;//屋内では地磁気センサ誤動作するため0
  filter->update(avelx,avely,avelz,accx,accy,accz,magx,magy,magz);
  // Serial.print("heading:");
  // Serial.print(filter->getYaw());
  // Serial.print(",roll:");
  // Serial.println(filter->getRoll());
  roll = filter->getRoll()-roll0;
  pitch = filter->getPitch()-pitch0;
  heading = filter->getYaw()-head0;
  return INTERVAL;
}

//AK09918
void AK09918_Init() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();

    err = ak09918.initialize();
    ak09918.switchMode(AK09918_POWER_DOWN);
    ak09918.switchMode(AK09918_CONTINUOUS_100HZ);

    err = ak09918.isDataReady();
    while (err != AK09918_ERR_OK) {
        Serial.println("Waiting Sensor");
        delay(100);
        err = ak09918.isDataReady();
    }
    err = ak09918.isDataReady();
    // err = AK09918_ERR_OK;
    if (err == AK09918_ERR_OK) {
        err = ak09918.isDataSkip();
        if (err == AK09918_ERR_DOR) {
            //Serial.println(ak09918.strError(err));
        }
        err = ak09918.getData(&x, &y, &z);
        if (err == AK09918_ERR_OK) {
            Serial.print("X:");
            Serial.print(x);
            Serial.print(",");
            Serial.print("Y:");
            Serial.print(y);
            Serial.print(",");
            Serial.print("Z:");
            Serial.print(z);
            Serial.println("");
            mx0 = x;
            my0 = y;
            mz0 = z;
        } else {
            Serial.println(ak09918.strError(err));
        }
    } else {
        Serial.println(ak09918.strError(err));
    }
}
void AK09918_main() {
  err = ak09918.isDataReady();
  // err = AK09918_ERR_OK;
  if (err == AK09918_ERR_OK) {
      err = ak09918.isDataSkip();
      if (err == AK09918_ERR_DOR) {
          //Serial.println(ak09918.strError(err));
      }
      err = ak09918.getData(&x, &y, &z);
      if (err == AK09918_ERR_OK) {

          mx = x - mx0;
          my = y - my0;
          mz = z - mz0;
      } else {
          Serial.println(ak09918.strError(err));
      }
  } else {
      Serial.println(ak09918.strError(err));
  }
  //delay(100);
}
//--------------------------------------
//  TFT
//--------------------------------------
void TFT_Init(){
  pinMode(TFT_LED, OUTPUT);
  digitalWrite(TFT_LED, HIGH);
  tft.begin();
  tft.setRotation(2);
  tft.setSwapBytes(true);
}
//Canvasリセット
void ResetCanvas(){
    MadgWick_Init();     //フィルタおよび変数を初期化
    //canvas4->Reset();    //Canvasを初期化
    delete canvas4;
    canvas4 = new CANVAS(&tft,240,240,0,0);    //杖軌跡
}

//MadgWickフィルタ初期化
void MadgWick_Init(){
  filter = new Madgwick();
  filter->begin(1000000/INTERVAL);
  roll0=filter->getRoll();
  pitch0=filter->getPitch();
  head0=filter->getYaw();
  avelx0=0;
  avely0=0;
  avelz0=0;
  avelx=0;
  avely=0;
  avelz=0;
  accx=0;
  accy=0;
  accz=0;
}


//--------------------------------------
//  Switch
//--------------------------------------

void SW_main(){
  //SW状態を取得
  bool swflag = switch1->check_change();
  //スイッチの値に変化があった場合
  if(swflag){
    ResetCanvas();
  }
}

//--------------------------------------
//  Record Functions
//--------------------------------------
int number=0;
int label=0;
//Arudinoシリアル通信の受信値を処理
void Serial_main(){
  if (Serial.available() > 0) { // シリアルバッファにデータがあるか確認
    char receivedChar = Serial.read(); // 1バイト読み取り

    if (receivedChar == 's') { // もし受信したデータが's'なら
        SaveCSV();
        Serial.println("Done!");  
    }
    if (receivedChar == 'l') { // もし受信したデータが'l'ならラベル+1
        label++;
        if(label>5)label=0;
        Serial.print("label=");
        Serial.println(label);  
    }
    if (receivedChar == 'p') { // もし受信したデータが'p'なら
        canvas4->PrintSerial28();
    }
    if (receivedChar == 'r') { // もし受信したデータが'r'なら
        ResetCanvas();
    }


  }
}
//CSVにセーブする
void SaveCSV(){
  char filename[16];
  sprintf(filename, "%01d%03d.csv", label, number);
  SD.remove(filename);
  File myFile = SD.open(filename, FILE_WRITE);

  for (int i=0;i<28;i++) {
    for (int j=0;j<28;j++) {
      myFile.print(canvas4->output[i + 28 * j]);
      //Serial.print(canvas4->output[i + 28 * j]);
      if(j!=27)myFile.print(",");
      //Serial.print(",");
    }
    myFile.println("");
    //Serial.println("");
  }
  Serial.printf("%01d%03d.csv", label, number);
  myFile.close();
  number++;
}

このプログラムはArduinoのシリアルモニタ画面からアルファベットを送信することでデータの記録、ファイル名に付与するラベル名の変更を行います。

データセットの作成

学習用データの計測

作成したハードウェアにデータ収集用ソフトを書き込み1ジェスチャーごとに記録します。
今回は1ジェスチャーあたり100回分のデータを計測し、70回分を学習用データに、残りの30回分を検証用のデータにします。
以下の画像のようにNON,EIGHT,MINUS,CIRCLEの4種類のジェスチャーデータを計測します。
根気の勝負です。400回分計測します。ファイト！

Spresense上で実行する都合、学習モデルはあまり大きくできないので、できるだけ小さくなるように考慮が必要です。
今回はTFT液晶画面上のキャンバスを28 × 28ピクセルの正方形に見立て、データ化します。
杖軌跡の通過箇所を1,非通過箇所を0とし、28行、28列のcsvで保存します。
予め、データサイズを小さくしておくことで取り回しよく軽量モデルを作ることができます。

データ収集を終えたら、SDカードからcsvを取り出します。
SDカードをパソコンで読み込んでも良いですが、Spresense拡張ボードに実装されているマイクロusbコネクタへ接続するとUSB機器として認識しデータへアクセスすることができ便利です。

データは4桁の数値で構成され、最上位桁がラベル、下位３桁が通し番号です。

次に同じフォルダにtrain.csvとvalid.csvを作成します。
ファイルはx,yの2列で構成し、xに計測したcsvファイル名、yにラベル(=csvの最上位桁)を書き、学習するデータとラベルの組み合わせを定義します。

csvはexcelやnumbers、googleスプレッドシートなどお好きなソフトで作成してください。

Neural Network Consoleのインストール

SonyのNeural Network Console Windowsアプリ(以降、NNC)をインストールします。
こちらからダウンロードのうえインストールしてください。

NNCで学習の準備

NNCを起動し、新規プロジェクトを作成、データセット＞データセットを開く＞データセットを開く をクリックし、先ほど作成したtrain.csvとvalid.csvを読み込みます。

次にCNN(Convolution Neural Network)を作成します。
プロジェクト>新しいプロジェクトを開く で新しいプロジェクトを作成し、左側メニューから関数をドラッグ＆ドロップして以下のように並べます。

前述の通り、各ブロックで実行する処理は基本的に画像を分類するニューラルネットワークと同じです。
Inputは記録したデータにあわせ、28,28に設定し、RandomShiftでデータを水増し後、Reshape機能によって、1,28,28のデータ構造へ変換します。
このように変換を加えることで、28pixel×28pixelのモノクロ画像と同様に扱うことができCNNで学習できます。
AffineのOutShapeは4つのラベルに分けるため、4を設定しています。

この記事とほぼ同じニューラルネットワークで構成・実現していることがお分かりいただけるかと思います。
詳しい内容はこちらをご覧ください。

最後に上部のコンフィグをクリックしバッチサイズを16に変更します。

これで学習する準備が整いました。

NNCで学習

編集>実行(右上の青いボタン)で学習を実行します。

学習が始まると学習曲線が描かれ学習の様子を確認できます。

TrainingCompleted.が表示され、学習が完了したら評価を実行(右上の青ボタン)します。
すると学習済のモデルでvalidation用のデータセットを使った評価が実行されます。
評価が完了したら混同行列のラジオボタンをクリックし、Accuracyを確認します。
今回は1(=100%)と表示されており、validation用のデータでは全問正解したことを確認できました。

Accuracyの値が低い場合はCNNの設定や学習用のデータ数不足、カテゴリの分類ミスなどが考えられますので見直ししてみてください。

評価結果が問題なければ、学習済モデルをエクスポートします。
アクション>エクスポート>NNB(NNabla C Runtime file format)を選択します。
するとmodel.nnbが出力されます。

出来上がったmodel.nnbをSDカードのルートフォルダに書き込みます。
精度良くジェスチャーを認識することができました。

ジェスチャーの開始終了タイミングは加速度センサのユークリッドノルムに閾値を引いて求めています。前述のソフトに反映済です。

ここにPOSTが表示されます

スピーカーから音を出す準備

次にSpresenseから音を出す設定を行います。

DSPファイルインストール

まず、チュートリアルに従い、DSPファイルをインストールします。
DSPファイルをダウンロードしてSDカードへ書き込み読み込ませる方法と、DSPインストーラーを使用する方法がありますが、今回はSDカードへ書き込んで使用します。

MP3ファイルの準備

次にMP3ファイルを4つ準備しsound.mp3、sound2.mp3、sound3.mp3、sound4.mp3とファイル名に設定しSDカードのルートフォルダに書き込みます。
フリーの音源サイトが便利です。

ここで、4つのMP3ファイルは同じサンプリングレートに設定する必要があります。
今回はAudacityというソフトでサンプリングレートの確認と変更をしました。
サンプリングレート(サンプリング周波数)を確認し44100(下図左下参照)に変更し保存します。

Spresenseの拡張ボードに実装されているオーディオジャックにスピーカーを接続します。

杖の製作

コアな部分がほぼできたところで、杖を仕立てます。今回は高機能CAD Fusion360で設計しました。
Spresenseのハードウェア設計資料にSTPファイルなど必要な情報がありますので、Fusion360に読み込み、設計します。
ToFセンサの裏面にフルカラーLEDがついているため、この光源を、φ6mmのアクリル棒で導光することにします。
杖の3Dプリント用データはこちらからダウンロードできます。
アクリル棒に白い半透明マスキングテープを貼り付けると光が拡散し貼った部分が光ります。
劇中に出てくる、杖に亀裂が入り光が漏れ出すシーンに準え、杖の側面に切り込みを入れ、切り込み部分にマスキングテープを貼っています。
杖は3Dプリントし、超音波カッターで追加工して切り込みをいれました。

魔法の杖の完成！

ようやく魔法の杖が完成です。杖の軌跡を描くことでルーモスを唱え、周りを明るく照らすことに成功しました。

もっと杖らしくする

ここまでで杖の基本機能は実現しました。しかし、杖の先が光るだけで良いのでしょうか。
ディスプレイのついたデバイスを杖と呼んでいいのでしょうか。
ここからはディスプレイとSpresense拡張ボードを取り除き、コンパクトなメインボードを杖の柄に収めます。
最終的には以下の構成としました。

他の機器と通信するためにM5ATOMを追加します。
バッテリ(700mAh)の3.7V電源をDCDCで5Vへ昇圧し、SpresenseメインボードとM5ATOM両方に供給します。

M5ATOMはM5Stack社の販売するデバイスで、マイコンにESP32を搭載しています。
ESP32はArduinoとして動作でき、WiFiやBLE通信を行えます。電源はピンヘッダから5Vを給電することで動作します。
ESP32は独自のWifi通信ESP-NOWを使用でき、他のESP32搭載デバイスに対して無線で信号を送信することができます。
IoT機器としてWi-fiルータへ接続し、信号を送受信することも可能です。

赤外線で動作するデバイスを魔法の杖で操作できるようにすべく、赤外線通信機能を持たせます。
IRremoteというライブラリを使うと赤外線通信を簡単に実装できます。

SpresenseメインボードとM5ATOMの接続はGPIO2本(D27,D20)で接続しました。
Spresenseメインボードは1.8V系、M5ATOMは3.3V系と駆動電圧が異なるのでレベルシフト回路を基板に実装します。

杖の再設計

最終的に以下のデザインに落ち着きました。

学習済みモデルをSPI-Flashに書き込む

さて、ディスプレイ付きモデルではSDカードに学習済みモデルを書き込んでいましたが、
拡張ボードを無くした関係でSDカードから読み込むことができません。
そこで、メインボードのSPI-Flashに学習モデルを書き込みます。
書き込みにはSpresenseSDKインストール時に同梱されるflash.shを使用します。

sdk>toolsフォルダ内にflash.shが入っています。
同じフォルダ内にmodel.nnbを入れて、以下のコマンドで書き込みます。
./flash.sh -c (Spresenseポート名) -w model.nnb

SpresenseSDKは以下のセットアップガイドを参照します。
https://developer.sony.com/spresense/development-guides/sdk_set_up_ide_ja.html

ソフトの書き込み

以下のソフトをSpresenseへ書き込みます。
なお、記事掲載スペースの関係上、全てのプログラムを掲載すると膨大なのでTFT液晶の描画やスイッチ操作などを扱うクラスは割愛します。
Githubに全てのプログラムを掲載していますのでこちらを確認してください。

Spresense_MagicWand.ino
//サブコア誤書き込み防止
#ifdef SUBCORE
#error "Core selection is wrong!!"
#endif


#define RECORD_MODE 0  //0:推論モード,1:学習モード

#include <File.h>
#include <Flash.h>
//IMU
#include "Arduino_BMI270_BMM150.h"

//TFT
#define TFT_LED 8        //LED接続端子
#define LGFX_AUTODETECT  // 自動認識
#include <LovyanGFX.hpp>
#include "LGFX_SPRESENSE.hpp"
#include <LGFX_AUTODETECT.hpp>          // クラス"LGFX"を準備します
static LGFX_SPRESENSE_SPI_ILI9341 tft;  // LGFXのインスタンスを作成

//ToF
//#include <SPI.h>

//Canvas
#include "CANVAS.h"
//CANVAS *canvas1;
CANVAS *canvas2;
CANVAS *canvas3;
CANVAS *canvas4;
CANVAS *canvas5;
CANVAS *canvas6;

//AK09918
#include "AK09918.h"
#include <Wire.h>
AK09918_err_type_t err;
int32_t x, y, z;
AK09918 ak09918;
int mx, my, mz;
int mx0, my0, mz0;

//SW
#include "SW.h"
SW *switch1;

//MadgWick
#include <MadgwickAHRS.h>
#define INTERVAL 100000  //us
float roll = 0;
float pitch = 0;
float heading = 0;
Madgwick *filter;

//DNN
#include <DNNRT.h>
DNNRT dnnrt;
#define DNN_DATA_WIDTH 28
#define DNN_DATA_HEIGHT 28
DNNVariable input(DNN_DATA_WIDTH *DNN_DATA_HEIGHT);
static String const labels[4] = { "EIGHT", "CIRCLE", "MINUS", "NON" };
int command = 3;

//INTERFACE to M5ATOM
#define OUTPUT1_PIN 27
#define OUTPUT2_PIN 20

//mode
// モードの列挙型
enum MODE {
  MODE1,
  MODE2,
  MODE3,
  MODE4
};
MODE currentMode = MODE4;
MODE beforeMode = MODE4;

int TaskSpan;                   //タスク実行間隔
uint32_t startTime;             //開始時間
uint32_t cycleTime;             //サイクルタイム
uint32_t deltaTime;             //
uint32_t spentTime;             //経過時間
bool _InitCondition = false;    //初期化状態
bool _DeinitCondition = false;  //終了時状態


int CheckCommand() {
  float *dnnbuf = input.data();
  int count = 0;
  for (int i = 0; i < DNN_DATA_WIDTH; i++) {
    for (int j = 0; j < DNN_DATA_HEIGHT; j++) {
      dnnbuf[count] = canvas4->output[i + 28 * j];
      count++;
    }
  }

  dnnrt.inputVariable(input, 0);
  dnnrt.forward();

  DNNVariable output = dnnrt.outputVariable(0);
  int index = output.maxIndex();
  return index;
}

void InterfaceOutput(MODE m) {
  switch (m) {
    case MODE1:
      digitalWrite(OUTPUT1_PIN, LOW);
      digitalWrite(OUTPUT2_PIN, LOW);
      break;

    case MODE2:
      digitalWrite(OUTPUT1_PIN, LOW);
      digitalWrite(OUTPUT2_PIN, HIGH);
      break;

    case MODE3:
      digitalWrite(OUTPUT1_PIN, HIGH);
      digitalWrite(OUTPUT2_PIN, LOW);
      break;

    case MODE4:
      digitalWrite(OUTPUT1_PIN, HIGH);
      digitalWrite(OUTPUT2_PIN, HIGH);
      break;
  }
}


void setup() {
  Serial.begin(115200);

  //INTERFACE
  pinMode(OUTPUT1_PIN, OUTPUT);
  pinMode(OUTPUT2_PIN, OUTPUT);
  digitalWrite(OUTPUT1_PIN, HIGH);
  digitalWrite(OUTPUT2_PIN, HIGH);

  //TFT
  TFT_Init();

  switch1 = new SW(PIN_D21, INPUT_PULLUP);
  //タイマ割り込み
  attachTimerInterrupt(TimerInterruptFunction, INTERVAL);
  
  canvas4 = new CANVAS(&tft,240,240,0,0);    //杖軌跡

  //IMU
  IMU_Init();

  //AK09918
  AK09918_Init();

  //ジャイロセンサ
  GyroInit();

  //MadgWick
  MadgWick_Init();


  //SD->Flash
  Flash.begin();

  //DNN
  File nnbfile = Flash.open("model.nnb");
  int ret = dnnrt.begin(nnbfile);
  if (ret < 0) {
    Serial.println("dnnrt.begin failed" + String(ret));
  }

}

void CANVAS_main() {
  //杖軌跡描画
  canvas4->WandDraw28(heading, roll);
}

void mainloop(MODE m) {
  //前回実行時からモードが変わってたら終了処理
  if (beforeMode != m) DeinitActive();
  beforeMode = m;
  //前回実行時からの経過時間を計算
  deltaTime = millis() - cycleTime;
  //経過時間を計算
  spentTime = millis() - startTime;

  if (deltaTime >= TaskSpan) {
    //    Serial.println("mainloop");
    if (!_InitCondition) {  //未初期化時実行
                            //      Serial.println("Init");
      InitFunction(m);
    } else if (_DeinitCondition) {  //修了処理時実行
                                    //      Serial.println("Deinit");
      DeinitFunction();
    } else {
      //      Serial.println("main()");
      //モードごとの処理
      InterfaceOutput(m);
      Serial.println(m);
      switch (m) {
        case MODE1:
          break;

        case MODE2:
          //TOF_SetLED(255,0,0);
          break;

        case MODE3:
          //TOF_SetLED(0,255,0);
          currentMode = MODE4;

          break;

        case MODE4:
          //TOF_SetLED(0,0,0);
          break;
      }
    }
    //cycleTimeリセット
    cycleTime = millis();
  }
}

void DeinitActive() {  //モード終了時に呼ぶ
  startTime = millis();
  //モードごとの処理
  _DeinitCondition = true;
}
void DeinitFunction() {  //最後に呼ばれる

  _InitCondition = false;    //初期化状態
  _DeinitCondition = false;  //終了時状態
}
void InitFunction(MODE m) {  //初回呼ぶ
  //モードごとの処理
  switch (m) {
    case MODE1:
      //TOF_SetLED(255,255,255);
      //PlaySound(1);
      ledOn(LED0);
      ledOff(LED1);
      ledOff(LED2);
      ledOff(LED3);
      break;

    case MODE2:
      //TOF_SetLED(255,0,0);
      //PlaySound(2);
      ledOff(LED0);
      ledOn(LED1);
      ledOff(LED2);
      ledOff(LED3);
      break;

    case MODE3:
      //TOF_SetLED(0,255,0);
      //PlaySound(3);
      ledOff(LED0);
      ledOff(LED1);
      ledOn(LED2);
      ledOff(LED3);
      break;

    case MODE4:
      //TOF_SetLED(0,0,0);
      ledOff(LED0);
      ledOff(LED1);
      ledOff(LED2);
      ledOn(LED3);

      break;
  }
  startTime = millis();
  _InitCondition = true;  //初回フラグon
}

void loop() {

  IMU_main();  //IMUセンサ値更新
  //TOF_main();         //TOFセンサ値更新
  //AK09918_main();     //地磁気センサ更新
  CANVAS_main();                                   //描画更新
  if (RECORD_MODE == 0) command = CheckCommand();  //DNN
  SW_main();                                       //ボタンチェック(押下時Reset処理)
  //Audio_main();                                    //オーディオ

  Serial_main();  //Arduinoシリアル操作

  //モード起動時処理
  if (RECORD_MODE == 0) {
    if (IMU_CheckAccActive()) {
      if (command == 0) {
        currentMode = MODE1;
        ResetCanvas();
      }
      if (command == 1) {
        currentMode = MODE2;
        ResetCanvas();
      }
      if (command == 2) {
        currentMode = MODE3;
        ResetCanvas();
      }
    }
  }

  //所定の加速度より早い場合キャンバスを消す
  if (IMU_CalcAccVec(IMU_ReadAccX(), IMU_ReadAccY(), IMU_ReadAccZ()) > 1.5) {
    //Serial.println(IMU_CalcAccVec);
    currentMode = MODE4;
    //ResetCanvas();
  }

  mainloop(currentMode);
}
//--------------------------------------
//  9軸センサによる姿勢推定
//--------------------------------------

//センサからの読み出し値
float accx=0; //加速度x
float accy=0; //加速度y
float accz=0; //加速度z
float magx=0; //地磁気x
float magy=0; //地磁気y
float magz=0; //地磁気z
float avelx=0; //ジャイロセンサx
float avely=0; //ジャイロセンサy
float avelz=0; //ジャイロセンサz

//ジャイロセンサ補正値(Initで算出)
float avelx0=0;
float avely0=0;
float avelz0=0;

//姿勢値オフセット変数
float head0=0;
float roll0=0;
float pitch0=0;

//ジャイロセンサドリフト補正  
void GyroInit(){
  Serial.print("Gyro Init");
  for(int i=0 ;i<100;i++){
    //加算
    avelx0 +=avelx;
    avely0 +=avely;
    avelz0 +=avelz;
    Serial.print(".");
  }
  
    avelx0 = avelx0/100;
    avely0 = avely0/100;
    avelz0 = avelz0/100;
    //初期値を表示
    Serial.println("Finished");
    Serial.print("GyroX:"+ String(avelx0));
    Serial.print(",GyroY:"+ String(avely0));
    Serial.println(",GyroZ:"+ String(avelz0));
}

//タイマー割り込み関数
unsigned int TimerInterruptFunction() {
  avelx = IMU_ReadAveGyroX();
  avely = IMU_ReadAveGyroY();
  avelz = IMU_ReadAveGyroZ();
  accx = IMU_ReadAccX();
  accy = IMU_ReadAccY();
  accz = IMU_ReadAccZ();
  magx = 0;//mx;//屋内では地磁気センサ誤動作するため0
  magy = 0;//my;//屋内では地磁気センサ誤動作するため0
  magz = 0;//mz;//屋内では地磁気センサ誤動作するため0
  filter->update(avelx,avely,avelz,accx,accy,accz,magx,magy,magz);
  // Serial.print("heading:");
  // Serial.print(filter->getYaw());
  // Serial.print(",roll:");
  // Serial.println(filter->getRoll());
  roll = filter->getRoll()-roll0;
  pitch = filter->getPitch()-pitch0;
  heading = filter->getYaw()-head0;
  return INTERVAL;
}

//AK09918
void AK09918_Init() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();

    err = ak09918.initialize();
    ak09918.switchMode(AK09918_POWER_DOWN);
    ak09918.switchMode(AK09918_CONTINUOUS_100HZ);

    err = ak09918.isDataReady();
    while (err != AK09918_ERR_OK) {
        Serial.println("Waiting Sensor");
        delay(100);
        err = ak09918.isDataReady();
    }
    err = ak09918.isDataReady();
    // err = AK09918_ERR_OK;
    if (err == AK09918_ERR_OK) {
        err = ak09918.isDataSkip();
        if (err == AK09918_ERR_DOR) {
            //Serial.println(ak09918.strError(err));
        }
        err = ak09918.getData(&x, &y, &z);
        if (err == AK09918_ERR_OK) {
            Serial.print("X:");
            Serial.print(x);
            Serial.print(",");
            Serial.print("Y:");
            Serial.print(y);
            Serial.print(",");
            Serial.print("Z:");
            Serial.print(z);
            Serial.println("");
            mx0 = x;
            my0 = y;
            mz0 = z;
        } else {
            Serial.println(ak09918.strError(err));
        }
    } else {
        Serial.println(ak09918.strError(err));
    }
}
void AK09918_main() {
  err = ak09918.isDataReady();
  // err = AK09918_ERR_OK;
  if (err == AK09918_ERR_OK) {
      err = ak09918.isDataSkip();
      if (err == AK09918_ERR_DOR) {
          //Serial.println(ak09918.strError(err));
      }
      err = ak09918.getData(&x, &y, &z);
      if (err == AK09918_ERR_OK) {

          mx = x - mx0;
          my = y - my0;
          mz = z - mz0;
      } else {
          Serial.println(ak09918.strError(err));
      }
  } else {
      Serial.println(ak09918.strError(err));
  }
  //delay(100);
}
//--------------------------------------
//  TFT
//--------------------------------------
void TFT_Init(){
  pinMode(TFT_LED, OUTPUT);
  digitalWrite(TFT_LED, HIGH);
  tft.begin();
  tft.setRotation(2);
  tft.setSwapBytes(true);
}
//Canvasリセット
void ResetCanvas(){
    MadgWick_Init();     //フィルタおよび変数を初期化
    //canvas4->Reset();    //Canvasを初期化
    delete canvas4;
    canvas4 = new CANVAS(&tft,240,240,0,0);    //杖軌跡
}

//MadgWickフィルタ初期化
void MadgWick_Init(){
  filter = new Madgwick();
  filter->begin(1000000/INTERVAL);
  roll0=filter->getRoll();
  pitch0=filter->getPitch();
  head0=filter->getYaw();
  avelx0=0;
  avely0=0;
  avelz0=0;
  avelx=0;
  avely=0;
  avelz=0;
  accx=0;
  accy=0;
  accz=0;
}


//--------------------------------------
//  Switch
//--------------------------------------

void SW_main(){
  //SW状態を取得
  bool swflag = switch1->check_change();
  //スイッチの値に変化があった場合
  if(swflag){
    ResetCanvas();
  }
}

//--------------------------------------
//  Record Functions
//--------------------------------------
int number=0;
int label=0;
//Arudinoシリアル通信の受信値を処理
void Serial_main(){
  if (Serial.available() > 0) { // シリアルバッファにデータがあるか確認
    char receivedChar = Serial.read(); // 1バイト読み取り

    if (receivedChar == 's') { // もし受信したデータが's'なら
        //SaveCSV();
        Serial.println("no command");  
    }
    if (receivedChar == 'l') { // もし受信したデータが'l'ならラベル+1
        label++;
        if(label>5)label=0;
        Serial.print("label=");
        Serial.println(label);  
    }
    if (receivedChar == 'p') { // もし受信したデータが'p'なら
        canvas4->PrintSerial28();
    }
    if (receivedChar == 'r') { // もし受信したデータが'r'なら
        ResetCanvas();
    }


  }
}
//--------------------------------------
//  9軸センサ値処理
//--------------------------------------

float IMU_x, IMU_y, IMU_z;
float IMU_mx,IMU_my,IMU_mz;
float IMU_gx,IMU_gy,IMU_gz;
float IMU_gxAve,IMU_gyAve,IMU_gzAve;
#define NUM 3 //移動平均回数
float IMU_gyroX[NUM];
float IMU_gyroY[NUM];
float IMU_gyroZ[NUM];
int IMU_index = 0;               // readings 配列のインデックス
float IMU_totalX = 0;               // 読み取り値の合計
float IMU_averageX = 0;             // 移動平均値
float IMU_totalY = 0;               // 読み取り値の合計
float IMU_averageY = 0;             // 移動平均値
float IMU_totalZ = 0;               // 読み取り値の合計
float IMU_averageZ = 0;             // 移動平均値
float IMU_offsetX = 0;
float IMU_offsetY = 0;
float IMU_offsetZ = 0;

//加速度センサ補正値
float acc_gainX = 1.01010101010101;
float acc_gainY = 1.01010101010101;
float acc_gainZ = 1;
float acc_offsetX = 0;
float acc_offsetY = -0.01;
float acc_offsetZ = 0;


void IMU_Init(){
  Serial.println("IMU_Init");
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }
  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println(" Hz");
  Serial.print("Magnetic field sample rate = ");
  Serial.print(IMU.magneticFieldSampleRate());
  Serial.println(" uT");
  Serial.println();
  Serial.print("Gyroscope sample rate = ");
  Serial.print(IMU.gyroscopeSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Gyroscope in degrees/second");
  Serial.println("X\tY\tZ");
  
  //配列初期化
  for(int i=0;i<NUM;i++){
    IMU_gyroX[i]=0;
    IMU_gyroY[i]=0;
    IMU_gyroZ[i]=0;
  }
  Serial.print("Gyro init.Put Device");
  for(int i=0;i<NUM;i++){
    IMU_main();
    Serial.print(".");
    delay(10);
  }

  IMU_Reset();
  Serial.println("Finish");
  Serial.println("IMU_Init_OK");

}
//ジャイロセンサの平均値を求める
void IMU_CalcAverage(float valueX,float valueY,float valueZ){
  
  // 過去の読み取り値の合計から古い値を引く
  IMU_totalX = IMU_totalX - IMU_gyroX[IMU_index];
  IMU_totalY = IMU_totalY - IMU_gyroY[IMU_index];
  IMU_totalZ = IMU_totalZ - IMU_gyroZ[IMU_index];

  // 最新のセンサの値を readings 配列に追加
  IMU_gyroX[IMU_index] = valueX;
  IMU_gyroY[IMU_index] = valueY;
  IMU_gyroZ[IMU_index] = valueZ;

  // 過去の読み取り値の合計に新しい値を加える
  IMU_totalX = IMU_totalX + IMU_gyroX[IMU_index];
  IMU_totalY = IMU_totalY + IMU_gyroY[IMU_index];
  IMU_totalZ = IMU_totalZ + IMU_gyroZ[IMU_index];

  // 次の readings 配列のインデックスを計算
  IMU_index = (IMU_index + 1) % NUM;

  // 移動平均を計算
  IMU_averageX = IMU_totalX / NUM;
  IMU_averageY = IMU_totalY / NUM;
  IMU_averageZ = IMU_totalZ / NUM;
}

float IMU_CalcAccVec(float x,float y,float z){
    return abs(sqrt(x*x+y*y+z*z) - 1);
}

float CorrectAccX(float a){
  return acc_gainX*(a+acc_offsetX);
}
float CorrectAccY(float a){
  return acc_gainY*(a+acc_offsetY);
}
float CorrectAccZ(float a){
  return acc_gainZ*(a+acc_offsetZ);
}

float IMU_ReadAccX(){
  return CorrectAccX(IMU_x);
}
float IMU_ReadAccY(){
  return CorrectAccY(IMU_y);
}
float IMU_ReadAccZ(){
  return CorrectAccZ(IMU_z);
}

float IMU_ReadMagX(){
  return IMU_mx;
}
float IMU_ReadMagY(){
  return IMU_my;
}
float IMU_ReadMagZ(){
  return IMU_mz;
}
float IMU_ReadGyroX(){
  return IMU_gx;
}
float IMU_ReadGyroY(){
  return IMU_gy;
}
float IMU_ReadGyroZ(){
  return IMU_gz;
}
float IMU_ReadAveGyroX(){
  return IMU_averageX - IMU_offsetX;
}
float IMU_ReadAveGyroY(){
  return IMU_averageY - IMU_offsetY;
}
float IMU_ReadAveGyroZ(){
  return IMU_averageZ - IMU_offsetZ;
}
void IMU_Reset(){
  IMU_offsetX = IMU_averageX;
  IMU_offsetY = IMU_averageY;
  IMU_offsetZ = IMU_averageZ;
}

//IMU Reset Check
float IMU_startTime = 0;
bool countflag = false;
bool gflag =false;
const float AccValue = 0.04; //閾値
const int SpentTime = 150;

bool IMU_CheckAccActive(){
  return gflag;
}
void IMU_main(){

  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(IMU_x, IMU_y, IMU_z);
  }
  if (IMU.magneticFieldAvailable()) {
    IMU.readMagneticField(IMU_mx, IMU_my, IMU_mz);
    Serial.println("MagneticField_OK");
  }
    if (IMU.gyroscopeAvailable()) {
    IMU.readGyroscope(IMU_gx, IMU_gy, IMU_gz);

  }

  //ジャイロセンサ値移動平均処理
  IMU_CalcAverage(IMU_gx, IMU_gy, IMU_gz);
  
  //静止判定
  if(IMU_CalcAccVec(IMU_ReadAccX(),IMU_ReadAccY(),IMU_ReadAccZ()) < AccValue){
    if(!countflag){
      IMU_startTime = millis(); // カウンター開始前であればStartTimeを記録
      countflag = true;
    }
    if(millis() - IMU_startTime > SpentTime){
      gflag=true;
    }
    else{
      gflag=false;
    }
  }else{
    countflag = false; //閾値の以上なら
    gflag = false;     // IMUの値が閾値以上ならgflagをfalseに設定
  }

}

M5ATOM_MagicWand.ino
#include <IRremote.hpp>
#include <M5Atom.h>
#include <esp_now.h>
#include <WiFi.h>
#include "RINGLED.h"
#define LED_DATA_PIN 21
#define IR_RECEIVE_PIN 23
#define SEND_LED_PIN 19
#define INPUT1_PIN 25
#define INPUT2_PIN 22
#define ENABLE_LED_FEEDBACK true

RINGLED led = RINGLED();

enum MODE {
  MODE1,
  MODE2,
  MODE3,
  MODE4
};
MODE currentMode = MODE4;
MODE beforeMode = MODE4;
int TaskSpan;       //タスク実行間隔
uint32_t startTime; //開始時間
uint32_t cycleTime; //サイクルタイム
uint32_t deltaTime; //
uint32_t spentTime; //経過時間
bool _InitCondition=false;  //初期化状態
bool _DeinitCondition=false;  //終了時状態


void mainloop(MODE m){
  //前回実行時からモードが変わってたら終了処理
  if(beforeMode != m)DeinitActive();
  beforeMode = m;
   //前回実行時からの経過時間を計算
  deltaTime = millis() - cycleTime;
  //経過時間を計算
  spentTime = millis() - startTime;

  if(deltaTime >= TaskSpan){
//    Serial.println("mainloop");
    if(!_InitCondition){//未初期化時実行
//      Serial.println("Init");
      InitFunction(m);
    }else if(_DeinitCondition){//修了処理時実行
//      Serial.println("Deinit");
      DeinitFunction();
    }else{
//      Serial.println("main()");
        //モードごとの処理
    switch (m) {
      case MODE1:
        led.fire2(2,100);
        //TOF_SetLED(255,255,255);      
        break;

      case MODE2:
        //TOF_SetLED(255,0,0);
        //led.fire2(1,200);
        led.flash(100);
        break;

      case MODE3:
        //TOF_SetLED(0,255,0);
        //currentMode = MODE4;
        led.flash(200);
        break;

      case MODE4:
        led.pacifica();
        break;
      }
      
    }
    //cycleTimeリセット
    cycleTime = millis();
  }
}

void DeinitActive(){//モード終了時に呼ぶ
  startTime = millis();
  //モードごとの処理
  _DeinitCondition=true;
}
void DeinitFunction(){//最後に呼ばれる
  
  _InitCondition=false;  //初期化状態
  _DeinitCondition=false;  //終了時状態
}
void InitFunction(MODE m){//初回呼ぶ
  //モードごとの処理
  switch (m) {
  case MODE1:
    IrSender.sendNEC(0xEF00,0x3,1);//on
    //IrSender.sendNEC(0xEF00,0x4,1);//red

    //send_data(1,id,duty,hue,brightness);
    break;

  case MODE2:
    //IrSender.sendNEC(0xEF00,0x3,1);//on
    IrSender.sendNEC(0xEF00,0x7,1);//white
    //send_data(1,id,duty,hue,brightness);
    break;

  case MODE3:
    //IrSender.sendNEC(0xEF00,0x3,1);//on
    IrSender.sendNEC(0xEF00,0x5,1);//green
    //send_data(1,id,duty,hue,brightness);
    break;

  case MODE4:
    IrSender.sendNEC(0xEF00,0x2,1);//off
    break;
}
  startTime = millis();
  _InitCondition=true;  //初回フラグon
}


void setup() {
  M5.begin(true, false, true); 
  led.setup(LED_DATA_PIN);
  IrReceiver.begin(IR_RECEIVE_PIN, ENABLE_LED_FEEDBACK);
  IrSender.begin(SEND_LED_PIN);

  pinMode(INPUT1_PIN,INPUT_PULLUP);
  pinMode(INPUT2_PIN,INPUT_PULLUP);

  //ESP-NOW INITIAL
  ESPNOW_setup();
}

void loop() {
  M5.update();

  int input1 = digitalRead(INPUT1_PIN);
  int input2 = digitalRead(INPUT2_PIN);
  //モード起動時処理
  if(input1==0 && input2==0){
    currentMode = MODE1;
  }
  if(input1==0 && input2==1){
    currentMode = MODE2;
  }
  if(input1==1 && input2==0){
    currentMode = MODE3;
  }
  if(input1==1 && input2==1){
    currentMode = MODE4;
  }

  mainloop(currentMode);


  if (IrReceiver.decode()) {
    
    IrReceiver.printIRResultShort(&Serial);
    Serial.println(IrReceiver.decodedIRData.decodedRawData);
    if (IrReceiver.decodedIRData.decodedRawData == 0x95) {
      Serial.println("1");
    }
    IrReceiver.resume();
  }


}

おわりに

SpresenseのエッジAI機能で活用して魔法を実現することができました。
今回は4クラス分類で製作してみましたが、たくさんの魔法を学習させれば、もっと楽しい遊びが実現できそうです。
難しいジェスチャーを学習させて、強い魔法で相手を倒す、魔法バトルのようにしたら面白いかもしれません。

今回はM5ATOMを使用して通信機能を拡張しましたが、SPRESENSE用Wi-Fi add-onボードを使用すれば柄の部分をもっとコンパクトに実現できると思います。柄をもう少し小さくしたらスタイリッシュな杖を作れそうです。
今回は3Dプリントで製作しましたが、木材をCNC加工して仕立てたら、とても高級感のある魔法の杖ができるに違いありません。

これでマグルが魔法使いのように魔法を唱えることができますね。
技術で魔法を実現させられるって本当にエモい！

参考文献

• Interface 2021 9月号 第2部第2章 3次元姿勢をサッと求める！クオータニオンによる姿勢計算
• Interface 2023 10月号 第2部第2章 空中に描いた文字を6軸センサとTensorFlow Lite推論エンジンで認識