toioとM5StackをNimBLEで接続する

Amazonのセールでtoioが安くなっていたので購入、
せっかくなのでNimBLEで接続して操作してみました。

<blockquote class="twitter-tweet" data-media-max-width="560"><p lang="ja" dir="ltr">取引 <a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a> をサクッと繋いで遊ぶならこんな感じかな？<br><br>左ボタンで接続したらプレイマットに置いて再度左で整列します<br>中と右ボタンで操作して左でもどります<br><br>githubに上げるんでtoioをたくさん持ってる人は試してみて！<a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/WyUXXVilsb">pic.twitter.com/WyUXXVilsb</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1740823500485783759?ref_src=twsrc%5Etfw">December 29, 2023</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

githubにplatformioのプロジェクトごとアップしてあるので、
VsCodeのWORKSPACEに読み込んでビルド→アップロードするだけで実行できます

https://github.com/coppercele/toio-with-NimBLE

M5Unifiedで書かれているのでM5シリーズで3ボタンある機種なら動くと思います。
M5Stack Core,Core2で動作確認されています。

## サンプルスケッチについて

デフォルトで入ってるスケッチは
・M5Stackの電源が入るとtoioのスキャンを開始
・toioが見つかると画面下に数を表示
・左ボタンを押すと接続処理開始
接続後
・左ボタン：プレイマットがあればプレイマット上に整列する
・中ボタン：前進
・右ボタン：その場で右回転
・プレイマットがあれば1秒ごとにPositionIDを表示

という最低限の動作が実装されています。
main.cppのupdateToios()とloop()内に処理が書かれているので変更して遊んでみるの良いのではないかと思います。

## toioの同時接続数について

NimBLEはデフォルトでは２台までしか同時接続できないようになっています。
3台以上接続する場合は

projectRoot/libdeps/NimBLE-Arduino/src/nimconfig.h

内の

#ifndef CONFIG_BT_NIMBLE_MAX_CONNECTIONS
#define CONFIG_BT_NIMBLE_MAX_CONNECTIONS 3
#endif

を10に変更してください。
NimBLEの制限により最大9台接続できます。

https://x.com/mongonta555/status/1741075503791161490

タカオ(Takao) @mongonta555 さん情報ありがとうございます。

7台までは接続確認済みです。

https://x.com/okimoku1/status/1741180149645836690

おきもく @okimoku1 さん確認ありがとうございます

## どうやって繋ぐ？

基本的にはこちらの記事に書いてあることと同じなのですが、
toioに接続するために細かいところが変わっています。

https://qiita.com/coppercele/items/4ce0e8858a92410c81e3

接続にはNimBLEを使っています。
Arduino-ESP32のBLEスタックより安定してるしスケッチの大きさが半分くらいになるので色々便利です。

h2zero/NimBLE-Arduino: A fork of the NimBLE library structured for compilation with Arduino, for use with ESP32, nRF5x.
https://github.com/h2zero/NimBLE-Arduino

githubに上げてあるplatformio.iniに
lib_deps = 
	h2zero/NimBLE-Arduino@^1.4.1
と指定してあるので、
platformioでビルドすれば自動でダウンロードされます。

## 蛇足のような説明

どんな流れで接続しているのかつらつらと書いてみます。
これを読み終わったらカスタマイズしてお手元のBLEデバイスに接続してみると楽しいかも？

まずこちらがtoioの通信仕様です

https://toio.github.io/toio-spec/docs/ble_communication_overview

こちらに載っているService UUIDとCharacteristic UUIDを定義します。
基本的にこのUUIDを変えてやればどんなBLEデバイスにも繋がります(多分)

```cpp:ToioUUID.h

#pragma once

#include <NimBLEDevice.h>

// BLE UUIDを列挙
const NimBLEUUID serviceUUID = NimBLEUUID("10b20100-5b3b-4571-9508-cf3efcd7bbae");
const NimBLEUUID chrUuidIdInf = NimBLEUUID("10b20101-5b3b-4571-9508-cf3efcd7bbae");
const NimBLEUUID chrUuidMotor = NimBLEUUID("10b20102-5b3b-4571-9508-cf3efcd7bbae");
const NimBLEUUID chrUuidLight = NimBLEUUID("10b20103-5b3b-4571-9508-cf3efcd7bbae");
const NimBLEUUID chrUuidSound = NimBLEUUID("10b20104-5b3b-4571-9508-cf3efcd7bbae");
const NimBLEUUID chrUuidMotion = NimBLEUUID("10b20106-5b3b-4571-9508-cf3efcd7bbae");
const NimBLEUUID chrUuidButton = NimBLEUUID("10b20107-5b3b-4571-9508-cf3efcd7bbae");
const NimBLEUUID chrUuidBattery = NimBLEUUID("10b20108-5b3b-4571-9508-cf3efcd7bbae");
const NimBLEUUID chrUuidConfig = NimBLEUUID("10b201FF-5b3b-4571-9508-cf3efcd7bbae");

```

toioのクラスを宣言しておきます。
これがtoio一つ一つに対応します

```cpp:Toio.h
#pragma once

#include <Arduino.h>
#include <string.h>
#include <NimBLEDevice.h>

class Toio {
private:
  std::string bleAddress;

public:
  std::string name;

  uint8_t id = 0;
  uint16_t x = 0;
  uint16_t y = 0;
  uint16_t direction = 0;

  NimBLERemoteCharacteristic *pChrMotor;
  NimBLERemoteCharacteristic *pChrButton;
  NimBLERemoteCharacteristic *pChrLight;
  NimBLERemoteCharacteristic *pChrBattery;
  NimBLERemoteCharacteristic *pChrPosition;
  NimBLERemoteCharacteristic *pChrMotion;
  NimBLERemoteCharacteristic *pChrIdInf;
  NimBLERemoteCharacteristic *pChrConfig;

  Toio();
  void commandMotor(bool forwardLeft, uint8_t speedLeft, bool forwardRight, uint8_t speedRight);

  void setBleAdress(std::string address);
  std::string getBleAdress();
  bool isMatch(std::string address);
  void light(uint8_t mode, uint8_t msec, uint8_t red, uint8_t green, uint8_t blue);
  int battrey();
  void goTo(u_int16_t x, uint16_t y, uint16_t direction);
  void wait(void *pvParameters);
  void synchronizedMotor(bool forwardLeft, uint8_t speedLeft, bool forwardRight, uint8_t speedRight, ulong now, ulong buffer);
  void update();
};

Toio::Toio() {
}

// 実装部は省略
```

NimbleManarger.h内のnimbleSetup()を実行するとBLEデバイスをスキャンし始めます。(NimBLEのサンプルそのままだから省略)

NimBLEのスキャンが始まってからBLEデバイスのAdvertiseが見つかるごとにAdvertisedDeviceCallbacksのonResult()が実行されるので、
serviceUUIDをチェックしてtoioかどうか判断します。

もしもtoioならばNimBLEAdvertisedDeviceをstd::vectorに入れておいてあとで接続処理を行います。
また、ここで重複チェックも行います。

```cpp

    // BLEのAdvertiseが見つかるたびに実行される
  void onResult(NimBLEAdvertisedDevice *advertisedDevice) {

    if (advertisedDevice->isAdvertisingService(serviceUUID)) {
      // advertizeDeviceのUUIDがtoioならば
      Serial.println("Found Our Service");
      // 登録済みDeviceの数チェック
      if (advDevices.size() == 0) {
        // 登録数が0ならvectorに追加
        advDevices.push_back(advertisedDevice);
        Serial.printf("onResult:myDevices=%d\n", advDevices.size());
      }
      else {
        for (int i = 0; i < advDevices.size(); i++) {
          // すでに登録されていれば重複チェック
          if (advDevices.at(i)->getAddress().equals(advertisedDevice->getAddress())) {
            Serial.printf("onResult:device already added\n");
            // 重複していたらreturn
            return;
          }
        }
        // 重複がなければvectorに登録
        // advDevice = advertisedDevice;
        advDevices.push_back(advertisedDevice);
        Serial.printf("onResult:myDevices=%d\n", advDevices.size());
      }
      // 画面下にメッセージ表示
      std::string msg = "L button connect " + std::to_string(advDevices.size()) + " toio";
      msg += advDevices.size() == 1 ? "" : "s";
      bottomMessage(msg);
    }
  }
};


```

toioが見つかったら画面下に数が表示されるので左ボタンを押すと接続処理に入ります


```cpp:main.cpp
loop() {
  M5.update();
  if (M5.BtnA.wasPressed()) {
    if (toios.size() == 0) {
      if (advDevices.size() == 0) {
        // advertiseが見つかっていなければ中止
        return;
      }
      // toioが接続されてなければ接続開始
      if (connectToServer()) {
      return;
    }
  }
}

```

connectToServer()の中ではNimBLEのサンプルと同じことをしていますが、
toioのCharctersticsをtoioのメンバ変数に設定します。

```cpp

      // toioのメンバ変数
      if (pChr->getUUID().equals(chrUuidMotor)) {
        // モーター
        Serial.print("Motor chr\n");
        toio.pChrMotor = pChr;
      }
      else if (pChr->getUUID().equals(chrUuidBattery)) {
        // バッテリー
        Serial.print("Battery Chr\n");
        toio.pChrBattery = pChr;
      }
      else if (pChr->getUUID().equals(chrUuidLight)) {
        // ライト
        Serial.print("Light Chr\n");
        toio.pChrLight = pChr;
      }
      else if (pChr->getUUID().equals(chrUuidConfig)) {
        // 設定
        Serial.print("Config Chr\n");
        toio.pChrConfig = pChr;
      }
      else if (pChr->getUUID().equals(chrUuidMotion)) {
        // 6軸センサ
        Serial.print("Motion Chr\n");
        toio.pChrMotion = pChr;
      }
      else if (pChr->getUUID().equals(chrUuidIdInf)) {
        // ポジションID
        Serial.print("PositionId Chr\n");
        toio.pChrIdInf = pChr;
      }
      // else if (pChr->canNotify()) {
      // notifyを使う場合はnotifyCBに登録する
      //   if (!pChr->subscribe(true, notifyCB)) {
      //     /** Disconnect if subscribe failed */
      //     pClient->disconnect();
      //     return false;
      //   }
      //   Serial.print("Notify subscribed ");
      //   Serial.println(pChr->getUUID().toString().c_str());
      // }
    }
    toios.push_back(toio);

    Serial.println("Done with this device!");
  }

  ```

## toioを操作する

connectToServer()を実行したらtoioと通信できるようになります。

std::vector<Toio> toiosに接続したtoioのだけ要素が入っているので取り出して操作を実行します。

Toioクラスのメンバ変数に格納した各機能のCharacteristicに対してwriteValue()することで操作できます。

例:モーターを動かす場合

```cpp:Toio.h

// モーターを動かすメンバ関数
void Toio::commandMotor(bool forwardLeft, uint8_t speedLeft, bool forwardRight, uint8_t speedRight) {
  uint8_t data[7];
  data[0] = 0x01;                                 // モーター制御
  data[1] = 0x01;                                 // 左
  forwardLeft ? data[2] = 0x01 : data[2] = 0x02;  // 回転方向0x01:前
  data[3] = speedLeft;                            // 速度
  data[4] = 0x02;                                 // 右
  forwardRight ? data[5] = 0x01 : data[5] = 0x02; // 回転方向0x01:前
  data[6] = speedRight;                           // 速度

  Serial.printf("%s", pChrMotor->getUUID().toString().c_str());
  // モーターに対応するcharcterisitcsにwriteする
  pChrMotor->writeValue(data, 7, false);
  Serial.println("Motor command sent");
}

```

```cpp

std::vector<Toio> toios;

for (int i = 0; i < toios.size(); i++) {
  // vectorからtoioを取り出す
  // Toioクラスのメンバ関数を実行する
  // 左正回転,スピード0x16,右正回転,スピード0x16
  toios.at(i).commandMotor(true, 0x16, true, 0x16);
}

```

toioから情報を取得するときは機能に対応するCharacteristicsから読み込みます。
NimBLEAttValueで受けてdataとlengthを取得します

PositionIdを取得する

```cpp:Toio.h

// update()を実行するとPositionIDを更新する
void Toio::update() {

  // Position ID を読む
  NimBLEAttValue value = pChrIdInf->readValue();
  const uint8_t *pData = value.data();
  int length = value.length();

  // 0リセット
  x = 0;
  y = 0;
  direction = 0;

      // x,y,directionはuint16_tがuint8_tのリトルエンディアンに分割されている
    x = x | pData[2];
    x = (x << 8) | pData[1];
    y = y | pData[4];
    y = (y << 8) | pData[3];
    direction = direction | pData[6];
    direction = (direction << 8) | pData[5];
    Serial.printf("ID: %d Pos x:%d ", id, x);
    Serial.printf("y:%d ", y);
    Serial.printf("dirc:%d\n", direction);
}

```

```cpp:main.cpp

// toioの情報を更新する
// xTaskCreateUniversalで起動されてバックグラウンドで実行される
void updateToios(void *pvParameters) {
  while (true) {
    Serial.println("update:");
    for (int i = 0; i < toios.size(); i++) {
      toios.at(i).update();
      // Serial.printf("ID: %d Battery %d％\n", i, toios.at(i).battrey());
    }
    for (int i = 0; i < toios.size(); i++) {
      // ディスプレイにpositionIDを表示する
      M5.Display.setCursor(0, i * 20);
      M5.Display.printf("ID: %2d x:%3u y:%3u d:%3u", i, toios[i].x, toios[i].y, toios[i].direction);
    }
    // 秒ごとにPositionIDを更新する
    delay(1000);
  }
}

       // toioの情報を更新するタスクを起動
       // バックグラウンドで実行される
        xTaskCreateUniversal(
            updateToios,
            "updateToios",
            8192,
            NULL,
            1,
            NULL,
            APP_CPU_NUM);

```