M5StackでtoioとBLE通信してみた

# はじめに
前回の記事の続きです。
[toioのjavascriptライブラリが公開されたのでexample動かしてみた](https://qiita.com/KatsuShun89/items/5f86a9811565267f6b30)

いつも趣味で使っているM5StackをBLE centralとして、toio(BLE peripheral)と通信してみました。
toioについては前回の記事を参照ください。

# M5Stackとは
Wi-FiとBLE moduleを搭載した低価格のマイコンESP32に加えて、LEDやボタン、各通信ポート、SDカードスロットなどを搭載した一体型のモジュールです。
これ一つあれば組み込み系で遊びたいことは結構な範囲で実現出来ます。

<img width="400" alt="0dffe09c-2da4-79d3-9c88-0417a31cfdb9.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/302503/0dffe09c-2da4-79d3-9c88-0417a31cfdb9.png">

拡張モジュールも多数出ており、いろいろな方がこれを使って開発されています。
[M5Stackであそぼう](https://mag.switch-science.com/2018/02/28/getting-started-with-m5stack/)

今回は、このM5Stackでtoioと通信してみます。

# 開発環境
- arduino IDE
- [arduino-esp32:1.0.1](https://github.com/espressif/arduino-esp32/releases/tag/1.0.2-rc2)
  - 後述のBLE周りの修正を加えています
- M5Stack library:0.2.5

（正直ちょっと古い環境です。最新にしても動作するかは後日確認予定です。）

### BLE周りの変更点
ESP32のBLE周りは不安定だと以前からよく話題に上がっています。
実際に私も今回は初めてBLE触ってみましたが、最初うまく動かず、2点修正を取り込んでいます。
どちらもPR採用されていそうですので、arduino-esp32の次回リリースには反映されるかもしれないです。

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">arduino_esp32 1.0.1 が数日前にリリースされましたが、ダイソーモバイルシャッター等の BLE デバイスのほとんどが使えなくなってしまいました。<br><br>私が ESP32_BLE_Arduino で見つけた致命的なバグを github に上げましたので、お困りの方は持って行ってくださいませ。<a href="https://t.co/9McEHG1cUF">https://t.co/9McEHG1cUF</a><a href="https://twitter.com/hashtag/ESP32?src=hash&amp;ref_src=twsrc%5Etfw">#ESP32</a></p>&mdash; こばさん (@wakwak_koba) <a href="https://twitter.com/wakwak_koba/status/1084245402281373696?ref_src=twsrc%5Etfw">2019年1月13日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

https://github.com/nkolban/ESP32_BLE_Arduino/compare/master...wakwak-koba:master

また、上記の修正を加えて、ESP32のBLE central用のスケッチ`BLE_Client.ino`を起動しても下記のエラーで落ちてしまいました。

```console
Characteristic: uuid: 10b20106-5b3b-4571-9508-cf3efcd7bbae, handle: 26 0x1a, props: broadcast: 0, read: 1, write_nr: 0, write: 0, notify: 1, indicate: 0, auth: 0CORRUPT HEAP: Bad head at 0x3ffe4634. Expected 0xabba1234 got 0x3ffe463c
assertion "head != NULL" failed: file "/Users/ficeto/Desktop/ESP32/ESP32/esp-idf-public/components/heap/multi_heap_poisoning.c", line 214, function: multi_heap_free
```

調べるとBLE通信やLoraでの通信で同様の問題が起きているらしく、ちょうどタイムリーに修正された方がいたようなのでその方の変更を取り込んだところ通信できました。

<blockquote class="twitter-tweet" data-lang="ja"><p lang="en" dir="ltr">Fixed multi_heap_free failed during setting the value of the characteristic in BLE Library. by chcbaram · Pull Request #2789 · espressif/arduino-esp32 <a href="https://t.co/0sArdsy7jL">https://t.co/0sArdsy7jL</a></p>&mdash; 전민수 (@lucid8096) <a href="https://twitter.com/lucid8096/status/1138198003548921861?ref_src=twsrc%5Etfw">2019年6月10日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

https://github.com/espressif/arduino-esp32/pull/2789/files

その結果`BLE_client.ino`が落ちなくなりました。

この環境をベースにtoioとの通信機能を追加していきます。

# BLE通信機能
M5StackをCentralとしたBLE通信に関してはこちらの記事とその中のリンクを参考にしました。
[NUCLEO BLEのアドバタイジングをM5STACKでスキャンしてみた話](https://qiita.com/ss_tom_jp/items/878d2cd318343be0d71d)

toio全機能の実装は正直しんどいので、とりあえず下記の3つの機能をお試し実装してみました。

- [モーター](https://toio.github.io/toio-spec/docs/ble_motor)
- [読み取りセンサー](https://toio.github.io/toio-spec/docs/ble_id)
- [サウンド](https://toio.github.io/toio-spec/docs/ble_sound)

通信仕様については上のリンク先の公式仕様を参照ください。
サウンドの説明は本記事では割愛します。（モーターと同じノリで設定するだけです。）

## toioのService UUIDの設定
toio自体をBLE Peripheralとして発見するためにService UUIDを設定します。
toio自体のService UUIDは[通信概要](https://toio.github.io/toio-spec/docs/ble_communication_overview#%E3%82%AD%E3%83%A5%E3%83%BC%E3%83%96%E3%81%AE%E7%99%BA%E8%A6%8B)に記載されています。

これを下記のようにこの値に設定します。
※変数名のルールなどは`BLE_Client.ino`と変更してありますがご了承ください。

```cpp:M5Stack_BLE_toio_controller.ino
// toio service
static BLEUUID service_UUID("10B20100-5B3B-4571-9508-CF3EFCD7BBAE");
```

これでM5側がtoioのadvertiseを受けて検出できるようになります。

次に各機能(モーター、読み取りセンサー）の追加です。

## モーター制御設定の送信
モーターの制御はM5Stackからtoioに制御値を送信します。

[モーター](https://toio.github.io/toio-spec/docs/ble_motor)の仕様を参考にcharacteristic UUIDを設定します。

```cpp:M5Stack_BLE_toio_controller.ino
// motor characteristic
static BLEUUID motor_char_UUID("10B20102-5B3B-4571-9508-CF3EFCD7BBAE");
```

今回は[時間指定付きモーター制御](https://toio.github.io/toio-spec/docs/ble_motor#%E6%99%82%E9%96%93%E6%8C%87%E5%AE%9A%E4%BB%98%E3%81%8D%E3%83%A2%E3%83%BC%E3%82%BF%E3%83%BC%E5%88%B6%E5%BE%A1)を使用しました。
このリンク先のデータを送信します。

```cpp:M5Stack_BLE_toio_controller.ino
static void sendMotorControl(void)
{
  uint8_t data[motor_data_size] = {0};
  data[0] = 0x02; //motor control with specifiled time
  data[1] = 0x01; //motor id 1 : left
  data[2] = 0x01; //direction
  data[3] = (is_req_move_forward ? 0x1E : 0x0); //speed
  data[4] = 0x02; //motor id 2 : right
  data[5] = 0x01; //direction
  data[6] = (is_req_move_forward ? 0x1E : 0x0); //speed
  data[7] = 0x0A; //control time
  //Serial.println("write value");

  motor_characteristic->writeValue(data, sizeof(data));
}
```

Aボタンが押されている間だけ、`is_req_move_forward = true` として前進するようにしています。

```cpp:M5Stack_BLE_toio_controller.ino
static void checkPushButton(void)
{
  static bool is_long_pressed = false;
  is_long_pressed = M5.BtnA.pressedFor(100);
  if(is_long_pressed){
    M5.Lcd.drawString("MOVE", 0, 180);
    is_req_move_forward = true;
  }else{
    M5.Lcd.drawString("STOP", 0, 180);
    is_req_move_forward = false;
  }
}

```

## 読み取りセンサー値の受信

読み取りセンサーで値を読み取るとtoioからそのデータが送信されます。
送信されるデータは[こちら](https://toio.github.io/toio-spec/docs/ble_id)を参照ください。
主に4つのデータがあります。
- [Position ID](https://toio.github.io/toio-spec/docs/ble_id#position-id)
- [Standard ID](https://toio.github.io/toio-spec/docs/ble_id#standard-id)
- [Position ID missed](https://toio.github.io/toio-spec/docs/ble_id#position-id-missed)
- [Standard ID missed](https://toio.github.io/toio-spec/docs/ble_id#standard-id-missed)

上の2つはマット、アイテムに乗せたときに読み取れた値です。
下の2つはマット、アイテムから離したときに通知されます。
特にPosition IDはマット上の座標、角度も取れるので、これで絶対的な位置、姿勢が常に把握できるようです。

toioから受信するデータはcallback関数で処理するかたちになっていました。
各データをパースして保持、M5StackのLED上に表示してみました。

```cpp:M5Stack_BLE_toio_controller.ino
typedef struct {
  uint16_t x_cube_center;
  uint16_t y_cube_center;
  uint16_t angle_cube_center;
  uint16_t x_read_sensor;
  uint16_t y_read_sensor;
  uint16_t angle_read_sensor;
} position_id_t;

typedef struct {
  uint32_t standard_id;
  uint16_t angle_cube;
} standard_id_t;

static position_id_t position_id = {0};
static standard_id_t standard_id = {0};
static bool is_missed_position_id = false;
static bool is_missed_standard_id = false;

static void readPositionID(uint8_t *data, size_t length)
{
  memcpy(&position_id, &data[1], sizeof(position_id));
}

static void readStandardID(uint8_t *data, size_t length)
{
  memcpy(&standard_id, &data[1], sizeof(standard_id));
}

static void readPositionIDMissed(uint8_t *data, size_t length)
{
  is_missed_position_id = true;
  position_id.x_cube_center = 0;
  position_id.y_cube_center = 0;
  position_id.angle_cube_center = 0;
  position_id.x_read_sensor = 0;
  position_id.y_read_sensor = 0;
  position_id.angle_read_sensor = 0;
}

static void readStandardIDMissed(uint8_t *data, size_t length)
{
  is_missed_standard_id = true;
  standard_id.standard_id = 0;
  standard_id.angle_cube = 0;
}

void (*readFunctionTable[])(uint8_t *data, size_t length) = {
    &readPositionID,
    &readStandardID,
    &readPositionIDMissed,
    &readStandardIDMissed
};

static void selectReadFunction(uint8_t *data, size_t length)
{
  uint8_t id = data[0];
  readFunctionTable[id - 1](data, length);
}

static void notifyReadCallback(BLERemoteCharacteristic* ble_remote_characteristic,
                            uint8_t* data,
                            size_t length,
                            bool is_notify) {
    selectReadFunction(data, length);
}

static void drawReadSensor(void)
{
  String x_cube_str = String("cube x  ") + String(position_id.x_cube_center) + String("     ");
  M5.Lcd.drawString(x_cube_str, 0, 0);
  String y_cube_str = String("cube y  ") + String(position_id.y_cube_center) + String("     ");
  M5.Lcd.drawString(y_cube_str, 0, 35);
  String angle_cube_str = String("cube angle  ") + String(position_id.angle_cube_center) + String("     ");
  M5.Lcd.drawString(angle_cube_str, 0, 70);
  String standard_id_str = String("std id  ") + String(standard_id.standard_id)+ String("              ");
  M5.Lcd.drawString(standard_id_str, 0, 105);
  String standard_id_angle_str = String("std angle  ") + String(standard_id.angle_cube) + String("    ");
  M5.Lcd.drawString(standard_id_angle_str, 0, 140);
}
```

最後にこれらの処理を`loop()`内で呼び出します。

```cpp:M5Stack_BLE_toio_controller.ino
void loop() {
  
  M5.update();

  // If the flag "do_connect" is true then we have scanned for and found the desired
  // BLE Server with which we wish to connect.  Now we connect to it.  Once we are 
  // connected we set the connected flag to be true.
  if (do_connect == true) {
    if (connectToServer()) {
      Serial.println("We are now connected to the BLE Server.");
    } else {
      Serial.println("We have failed to connect to the server; there is nothin more we will do.");
    }
    do_connect = false;
  }


  // If we are connected to a peer BLE Server, update the characteristic each time we are reached
  // with the current time since boot.
  if (connected) {
    checkPushButton();
    sendMotorControl();
    drawReadSensor();
  }else if(do_scan){
    BLEDevice::getScan()->start(0);  // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
  }
  
  delay(10); // Delay a second between loops.
} // End of loop

```

いくつか割愛している箇所もありますが、githubに公開しているこちらのコードを参照してください。
https://github.com/Katsushun89/M5Stack_BLE_toio_controller

# 動作結果
実際に動作させてみた結果はこちらです。

### モーター
Aボタンを押している間だけ進みます。

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> をBLE centralにして、<a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a> のモーター制御出来た！<br><br>コードはESP32のBLE_clientをベースにして公開されたモーター制御データを送信するようにしてる。 <a href="https://t.co/1bJlvBWVlf">pic.twitter.com/1bJlvBWVlf</a></p>&mdash; Katsu Shun (@katsushun89) <a href="https://twitter.com/katsushun89/status/1140302467093024769?ref_src=twsrc%5Etfw">2019年6月16日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


### 読み取りセンサー
LEDにマットとアイテムの読み取り値を表示させてみました。

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">昨日の続き、読み取りセンサーの値を表示してみた。離すとmissedって通知くるのでその時に値ゼロクリアしてみた。<br>マットから座標読めるって面白いな😎<a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a><a href="https://twitter.com/hashtag/M5stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5stack</a> <a href="https://t.co/hVpejRJIrw">pic.twitter.com/hVpejRJIrw</a></p>&mdash; Katsu Shun (@katsushun89) <a href="https://twitter.com/katsushun89/status/1141018051011211264?ref_src=twsrc%5Etfw">2019年6月18日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# さいごに
M5StackでtoioとBLE通信してみました。
M5Stackはかなり拡張性高いのでサーボやセンサーとかいろいろ組み合わせて遊べそうな予感がします。

コードは今後整理予定ですが、現状でもBLE送受信のイメージは掴めると思いますので、参考にしていただければと思います。