M5StickC+UnitVとロボットトイtoioの連携でオートサッカーロボットを作る

[今年のMakerFaireTokyo2020にて展示させていただいた](https://makezine.jp/event/makers-mft2020/m0040/)「M5StickC+UnitV+toioでオートサッカー」について作り方を紹介します。

どんなものかはこちらの動画をご覧ください！
色付きサッカーボールを見せるとカメラで認識し、それをtoioでずっと追いかけます。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a> でオートサッカー⚽️<br>AIカメラ <a href="https://twitter.com/hashtag/UnitV?src=hash&amp;ref_src=twsrc%5Etfw">#UnitV</a> と <a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> そしてスイッチサイエンスさんより発売された「toio™️開発用プレイマット」を使って自動でサッカーするデモを作りました。今日 <a href="https://twitter.com/hashtag/MFTokyo2020?src=hash&amp;ref_src=twsrc%5Etfw">#MFTokyo2020</a> でtoio友の会のブースで展示していただいてます！<a href="https://t.co/WUBoXI1D10">https://t.co/WUBoXI1D10</a> <a href="https://twitter.com/hashtag/toiotomo?src=hash&amp;ref_src=twsrc%5Etfw">#toiotomo</a> <a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/D3Jr1OrZBL">pic.twitter.com/D3Jr1OrZBL</a></p>&mdash; Akichika Tanaka (@akichika) <a href="https://twitter.com/akichika/status/1312585948442492929?ref_src=twsrc%5Etfw">October 4, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

小型のM5StickCとUnitVとtoioを組み合わせてローカルに閉じたシステムで手軽に運用できるので発展性がありそうです。

# 必要なもの

- [toio コア キューブ](https://toio.io/platform/)（以降キューブ）
    - [toio本体セット](https://toio.io/platform/)に付属、もしくは[単体](https://toio.io/platform/cube/)で購入も可能　[※SwichScienceで販売されています](https://www.switch-science.com/catalog/6300/)
- [toio 開発用プレイマット（スイッチサイエンス）](https://www.switch-science.com/catalog/6650/)
- [M5StickC](https://www.switch-science.com/catalog/6350/)
- [M5 UnitV](https://www.switch-science.com/catalog/6481/)
- スマホスタンド、ミニ三脚など上から撮影できる固定器具
- [色付きサッカーボール（蛍光オレンジやピンクなど、テーブルサッカー用が扱いやすい）](https://www.amazon.co.jp/%E3%83%9F%E3%83%8B%E3%82%B5%E3%83%83%E3%82%AB%E3%83%BC-%E3%83%86%E3%83%BC%E3%83%96%E3%83%AB%E3%82%B5%E3%83%83%E3%82%AB%E3%83%BC-%E3%83%95%E3%83%BC%E3%82%BA%E3%83%9C%E3%83%BC%E3%83%AB-%E3%82%B5%E3%83%83%E3%82%AB%E3%83%BC%E3%82%B2%E3%83%BC%E3%83%A0-%E3%83%9F%E3%83%8B%E3%82%B5%E3%83%83%E3%82%AB%E3%83%BC%E3%83%9C%E3%83%BC%E3%83%AB/dp/B07W6GXWSM)
- レゴブロックやGoロボの人形等（サッカーのキャラクター用）
- ボールが飛び出ないようにするための段ボールの壁

# 組み立て方

まずUnitV、M5StickCそれぞれにプログラムを書き込みます。プログラムについては後述します。
次にUnitV付属の4PケーブルでM5StickCと接続します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/33040/97d08711-93d3-acf0-8347-7786c72ef5ce.png)

電気的にはこれで終了です。はんだ付け不要、ほとんど何もしていませんので超簡単です。必要に応じてM5StickCにUSBで充電したり電源を確保します。
マットの上空からUnitVで撮影するために、スマホスタンドを使います。自由に伸縮できるタイプの方が調整が楽です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/33040/729950ef-9982-54d3-fb71-6f0fb98aaf9f.png)

なお、「[toio 開発用プレイマット](https://www.switch-science.com/catalog/6650/)」にはサッカーフィールドの画像（Adobe Stockで素材購入）をCMYのみで印刷しました。印刷にはEPSONのA3プリンターEP-982A3を使いました。この機種には「カラーインクで黒を表現する」モードがあり、toio開発用プレイマットの特殊パターンに影響を与えずに印刷可能でした。
詳しくは[こちらの記事](https://qiita.com/akichika/items/2a41e7e97f261db07ef3)もご参考ください。


# アルゴリズム

プログラムの作成においては[こちらの記事](https://qiita.com/airpocket/items/714aac02b132f486eb67)を大いに参考にさせていただき、プログラムも流用させていただきました。素晴らしい記事をありがとうございました。


##ボール認識
お手軽に実現するため、ボールの認識についてはUnitVのクロマキー（特定の色を認識）を使います。そのためボールにはできるだけ蛍光色などの特徴的なものを利用する方が良いと思います。抽出した色の重心位置をターゲット位置としてM5StickCへ送信します。
コードはUnitVのMaixPyで書きました。ほとんど上で紹介した[こちらの記事](https://qiita.com/airpocket/items/714aac02b132f486eb67)のクロマキーのコードを微修正した程度です。書き込み方法等参考にしてみてください。

ボールの色を認識するための調整もしっかり行う必要があります。上記同様[こちらの記事](https://qiita.com/airpocket/items/714aac02b132f486eb67)が参考になります。

なお、座標空間の合わせこみや・位置合わせは本来は補正やアフィン変換も含めてしっかりキャリブレーションしなければならないのですが、今回は簡単のために撮影位置をマットの真上にしてプログラムに実機を合わせこむことにしました。シンプルですが、とりあえず成立したので良しとします。
長いのでプログラム（ソースコード）は最後に掲載します。

##ボール追尾アルゴリズム
UnitVからM5StickCに送られてきた重心位置にtoioの目標指定移動機能を使ってキューブを動かします。ある範囲を超えたら中央に戻ることで、端の方に来た時に壁に向かってボールを押し続けてスタックしてしまうことを避けるようにしました。
toioとの通信にはいつもお世話になっている@KatsuShun89さんの[こちらの記事](https://qiita.com/KatsuShun89/items/1d3d1ccae226a5b73da8)を参考にしました。本当にありがとうございます。
長いのでこちらもプログラム（ソースコード）は最後に掲載します。

# あそび方
M5StickCの電源を入れると一番近くのキューブに接続します。ボールを見つけると自動で追いかけます。

サッカーと言っても、ゴールの認識や敵キャラまでは準備していないので、とりあえずドリブルし続けるだけとなっていますが、見た目には楽しいです。

マットの四隅を少し高くすると中央に向かってボールが転がってくるようになるので、キューブがボールを端までドリブルして中央に戻る間にボールが戻り、無限にドリブルし続けるような動きをするようになります。

最後にあらためて実機の動きを再掲します。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a> でオートサッカー⚽️<br>AIカメラ <a href="https://twitter.com/hashtag/UnitV?src=hash&amp;ref_src=twsrc%5Etfw">#UnitV</a> と <a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> そしてスイッチサイエンスさんより発売された「toio™️開発用プレイマット」を使って自動でサッカーするデモを作りました。今日 <a href="https://twitter.com/hashtag/MFTokyo2020?src=hash&amp;ref_src=twsrc%5Etfw">#MFTokyo2020</a> でtoio友の会のブースで展示していただいてます！<a href="https://t.co/WUBoXI1D10">https://t.co/WUBoXI1D10</a> <a href="https://twitter.com/hashtag/toiotomo?src=hash&amp;ref_src=twsrc%5Etfw">#toiotomo</a> <a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/D3Jr1OrZBL">pic.twitter.com/D3Jr1OrZBL</a></p>&mdash; Akichika Tanaka (@akichika) <a href="https://twitter.com/akichika/status/1312585948442492929?ref_src=twsrc%5Etfw">October 4, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


# 1人称バージョン

M5StickC+UnitV+toioを1つにまとめた「１人称視点」で追尾ものも作ってみました。
こちらの方がよりロボットらしくまとまってはいますが、M5StickCの電池が10分程度しか持たないのでデモは大変でした。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a> とAIカメラ <a href="https://twitter.com/hashtag/UnitV?src=hash&amp;ref_src=twsrc%5Etfw">#UnitV</a> &amp; <a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> で実現した超小型完全自律サッカーロボット、つくばの <a href="https://twitter.com/hashtag/TMMF2020?src=hash&amp;ref_src=twsrc%5Etfw">#TMMF2020</a> 会場では人間対ロボットで試合ができるようにしてお子さんたちに大人気です。かなり単純なアルゴリズムでもそこそこ動いてくれて何よりです。 <a href="https://t.co/fD1bG6JGVz">pic.twitter.com/fD1bG6JGVz</a></p>&mdash; Akichika Tanaka (@akichika) <a href="https://twitter.com/akichika/status/1228550387617021953?ref_src=twsrc%5Etfw">February 15, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


#おわりに

toioとM5StickC、UnitVの組み合わせはサイズ感や手軽さ共にとてもマッチしているので、自律移動ロボット作りにもってこいです。ぜひみなさんも試してみてください！

#参考：プログラム一覧

##UnitV用のプログラム

[参考記事](https://qiita.com/airpocket/items/714aac02b132f486eb67)
MaixPyで書き込みます。

```python:boot.py
#original> https://qiita.com/airpocket/items/714aac02b132f486eb67
import sensor
import image
import lcd
import time
import utime
from machine import UART
from Maix import GPIO
from fpioa_manager import *

fm.register(34,fm.fpioa.UART1_TX)
fm.register(35,fm.fpioa.UART1_RX)
uart_out = UART(UART.UART1, 115200, 8, None, 1, timeout=1000, read_buf_len=4096)

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_hmirror(1)                       #カメラ左右反転
sensor.set_auto_gain(0)                     #検出安定化のためオートゲインオフ
sensor.set_auto_whitebal(0)                 #検出安定化のためオートホワイトバランスオフ
sensor.run(1)

while False:
    uart_out.write('TEST\n')
    utime.sleep_ms(100)

target_lab_threshold =(49, 100, -2, 73, 23, 103) #ボールに合わせて要調整

while True:
    img=sensor.snapshot()

    blobs = img.find_blobs([target_lab_threshold], x_stride = 2, y_stride = 2, pixels_threshold = 100, merge = True, margin = 20)
    if blobs:
        max_area = 0
        target = blobs[0]
        for b in blobs:
            if b.area() > max_area:
                max_area = b.area()
                target = b
        area = target.area()
        dx = 160 - target[5]                #UnitVとM5stackVではカメラ取付向きが違うためｘセンター位置を320pix/2,ｘ軸パラメータをtarget[6]→[5]へ変更
        dy = 120 - target[6]                #UnitVとM5stackVではカメラ取付向きが違うためｘセンター位置を320pix/2,ｘ軸パラメータをtarget[6]→[5]へ変更
        hexlist = [(dx >> 8) & 0xFF, dx & 0xFF,(dy >> 8) & 0xFF, dy & 0xFF, (area >> 8) & 0xFF, area & 0xFF] #6Byte
        print(hexlist, target[5], target[6], target.area())
        if uart_out.read(4096):
            uart_out.write(bytes(hexlist))
        else:
            pass
        tmp=img.draw_rectangle(target[0:4])
        tmp=img.draw_cross(target[5], target[6])
        c=img.get_pixel(target[5], target[6])

    else:
        if uart_out.read(4096):
            hexlist = [0x80, 0x00, 0x80, 0x00, 0x00, 0x00]
            uart_out.write(bytes(hexlist))
        else:
            pass
```



##M5StickC用のプログラム
[toioとM5Stack連携の参考記事](https://qiita.com/KatsuShun89/items/1d3d1ccae226a5b73da8)
[M5StickC用の設定](https://qiita.com/usashirou/items/3f149727aceb5ffa0f2d)をしたArduino SDKで書き込みます。


```Arduino:toio-soccer.ino
//original> https://qiita.com/KatsuShun89/items/1d3d1ccae226a5b73da8

#include <M5StickC.h>
#include <BLEDevice.h>


// toio service
static BLEUUID service_UUID("10B20100-5B3B-4571-9508-CF3EFCD7BBAE");
// read sensor characteristic
static BLEUUID read_char_UUID("10B20101-5B3B-4571-9508-CF3EFCD7BBAE");
// motor characteristic
static BLEUUID motor_char_UUID("10B20102-5B3B-4571-9508-CF3EFCD7BBAE");
// sound characteristic
static BLEUUID sound_char_UUID("10B20104-5B3B-4571-9508-CF3EFCD7BBAE");

static boolean do_connect = false;
static boolean connected = false;
static boolean do_scan = false;
static BLERemoteCharacteristic* read_characteristic;
static BLERemoteCharacteristic* motor_characteristic;
static BLERemoteCharacteristic* sound_characteristic;
static BLEAdvertisedDevice* my_device;

static bool is_btnA_switch = false;

static uint8_t motor_data_size = 8;
static uint8_t motor_pos_data_size = 13;
static uint8_t sound_data_size = 3;

static uint8_t current_sound = 0;

enum {
  sound_enter = 0,
  sound_selected,
  sound_cancel,
  sound_cursor,
  sound_mat_in,
  sound_mat_out,
  sound_get_1,
  sound_get_2,
  sound_get_3,
  sound_effect_1,
  sound_effect_2,
  sound_max, //threshold
};

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];
  Serial.printf("read id:%d\n", id);
  //need to size check
  readFunctionTable[id - 1](data, length);
}

static void notifyReadCallback(BLERemoteCharacteristic* ble_remote_characteristic,
                               uint8_t* data,
                               size_t length,
                               bool is_notify) {
  Serial.print("Notify callback for characteristic ");
  Serial.print(ble_remote_characteristic->getUUID().toString().c_str());
  Serial.print(" of data length ");
  Serial.println(length);
  Serial.print("data: ");
  for (uint8_t i = 0; i < length; i++) {
    Serial.printf("%x ", data[i]);
  }
  Serial.println("");

  selectReadFunction(data, length);

}

class MyClientCallback : public BLEClientCallbacks {
    void onConnect(BLEClient* client) {
    }

    void onDisconnect(BLEClient* client) {
      connected = false;
      Serial.println("onDisconnect");
    }
};

bool connectToServer() {
  Serial.print("Forming a connection to ");
  Serial.println(my_device->getAddress().toString().c_str());

  BLEClient*  client  = BLEDevice::createClient();
  Serial.println(" - Created client");

  client->setClientCallbacks(new MyClientCallback());

  // Connect to the remove BLE Server.
  client->connect(my_device);  // if you pass BLEadvertised_device instead of address, it will be recognized type of peer device address (public or private)
  Serial.println(" - Connected to server");

  // Obtain a reference to the service we are after in the remote BLE server.
  BLERemoteService* remote_service = client->getService(service_UUID);
  if (remote_service == nullptr) {
    Serial.print("Failed to find our service UUID: ");
    Serial.println(service_UUID.toString().c_str());
    client->disconnect();
    return false;
  }
  Serial.println(" - Found toio service");

  //read
  read_characteristic = remote_service->getCharacteristic(read_char_UUID);
  if (read_characteristic == nullptr) {
    Serial.print("Failed to find read characteristic UUID: ");
    Serial.println(read_char_UUID.toString().c_str());
    client->disconnect();
    return false;
  }
  Serial.println(" - Found read characteristic");

  // Read the value of the characteristic.
  if (read_characteristic->canRead()) {
    std::string value = read_characteristic->readValue();
    Serial.print("read characteristic value was: ");
    Serial.println(value.c_str());
  }

  if (read_characteristic->canNotify())
    read_characteristic->registerForNotify(notifyReadCallback);


  //motor
  motor_characteristic = remote_service->getCharacteristic(motor_char_UUID);
  if (motor_characteristic == nullptr) {
    Serial.print("Failed to find motor characteristic UUID: ");
    Serial.println(motor_char_UUID.toString().c_str());
    client->disconnect();
    return false;
  }
  Serial.println(" - Found motor characteristic");

  //sound
  sound_characteristic = remote_service->getCharacteristic(sound_char_UUID);
  if (sound_characteristic == nullptr) {
    Serial.print("Failed to find sound characteristic UUID: ");
    Serial.println(sound_char_UUID.toString().c_str());
    client->disconnect();
    return false;
  }
  Serial.println(" - Found sound characteristic");


  connected = true;
}
/**
   Scan for BLE servers and find the first one that advertises the service we are looking for.
*/
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    /**
        Called for each advertising BLE server.
    */
    void onResult(BLEAdvertisedDevice advertised_device) {
      Serial.print("BLE Advertised Device found: ");
      Serial.println(advertised_device.toString().c_str());

      // We have found a device, let us now see if it contains the service we are looking for.
      if (advertised_device.haveServiceUUID() && advertised_device.isAdvertisingService(service_UUID)) {

        BLEDevice::getScan()->stop();
        my_device = new BLEAdvertisedDevice(advertised_device);
        do_connect = true;
        do_scan = true;

      } // Found our server
    } // onResult
}; // Myadvertised_deviceCallbacks


//-----Begin UnitV handler-----

HardwareSerial VSerial(1);

typedef struct
{
  int16_t dx;
  int16_t dy;
  uint32_t area;
} v_response_t;


uint8_t I2CWrite1Byte(uint8_t Addr, uint8_t Data)
{
  Wire.beginTransmission(0x38);
  Wire.write(Addr);
  Wire.write(Data);
  return Wire.endTransmission();
}

uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length)
{
  Wire.beginTransmission(0x38);
  Wire.write(Addr);
  for (int i = 0; i < Length; i++)
  {
    Wire.write(Data[i]);
  }
  return Wire.endTransmission();
}

uint8_t Setspeed(int16_t Vtx, int16_t Vty, int16_t Wt)
{
  int16_t speed_buff[4] = {0};
  int8_t speed_sendbuff[4] = {0};

  Wt = (Wt > 100) ? 100 : Wt;
  Wt = (Wt < -100) ? -100 : Wt;

  Vtx = (Vtx > 100) ? 100 : Vtx;
  Vtx = (Vtx < -100) ? -100 : Vtx;
  Vty = (Vty > 100) ? 100 : Vty;
  Vty = (Vty < -100) ? -100 : Vty;

  Vtx = (Wt != 0) ? Vtx * (100 - abs(Wt)) / 100 : Vtx;
  Vty = (Wt != 0) ? Vty * (100 - abs(Wt)) / 100 : Vty;

  speed_buff[0] = Vty - Vtx - Wt;
  speed_buff[1] = Vty + Vtx + Wt;
  speed_buff[3] = Vty - Vtx + Wt;
  speed_buff[2] = Vty + Vtx - Wt;

  for (int i = 0; i < 4; i++)
  {
    speed_buff[i] = (speed_buff[i] > 100) ? 100 : speed_buff[i];
    speed_buff[i] = (speed_buff[i] < -100) ? -100 : speed_buff[i];
    speed_sendbuff[i] = speed_buff[i];
  }
  return I2CWritebuff(0x00, (uint8_t *)speed_sendbuff, 4); //NEED TO FIX FOR toio
}

enum
{
  kNoTarget = 0,
  kLeft,
  kRight,
  kStraight,
  kTooClose
};

const uint16_t kThreshold = 30; // If target is in range ±kThreshold, the car will go straight
v_response_t v_data;    // Data read back from V
uint8_t state = 0;  // Car's movement status

//----End UnitV handler----///

void setupBLE() {
  BLEDevice::init("");

  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.
  BLEScan* ble_scan = BLEDevice::getScan();
  ble_scan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  ble_scan->setInterval(1349);
  ble_scan->setWindow(449);
  ble_scan->setActiveScan(true);
  ble_scan->start(5, false);
}

void setup() {
  M5.begin();
  M5.Lcd.fillScreen(WHITE);
  delay(100);
  M5.Lcd.fillScreen(0x0000);
  M5.Lcd.setTextFont(1);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setTextColor(0xFFFF, 0x0000);
  M5.Lcd.drawString("toio soccer", 5, 5);

  //for UnitV
  VSerial.begin(115200, SERIAL_8N1, 33, 32);
  Wire.begin(0, 26);

  //for toio
  Serial.begin(115200);
  Serial.println("Starting Arduino BLE Client application...");
  setupBLE();
  
} // End of setup.

static String getSoundStr(void)
{
  String sound_str;
  switch (current_sound) {
    case sound_enter:
      sound_str = "Enter   ";
      break;
    case sound_selected:
      sound_str = "Selected";
      break;
    case sound_cancel:
      sound_str = "Cancel  ";
      break;
    case sound_cursor:
      sound_str = "Cursor  ";
      break;
    case sound_mat_in:
      sound_str = "Mat in  ";
      break;
    case sound_mat_out:
      sound_str = "Mat out ";
      break;
    case sound_get_1:
      sound_str = "Get 1   ";
      break;
    case sound_get_2:
      sound_str = "Get 2   ";
      break;
    case sound_get_3:
      sound_str = "Get 3   ";
      break;
    case sound_effect_1:
      sound_str = "Effect 1";
      break;
    case sound_effect_2:
      sound_str = "Effect 2";
      break;
  }
  return sound_str;
}

int btnAhold = 0;
static void checkPushButton(void)
{
  if (M5.BtnA.wasPressed()) {
    setupBLE();
    esp_restart();
  }

  if (M5.BtnB.wasPressed()) {
    sendSoundControl();
    current_sound++;
    if (current_sound >= sound_max) current_sound = sound_enter;
  }
  M5.Lcd.drawString(getSoundStr(), 160, 180);
}

static void setMotorVelocity(uint8_t vel_l, uint8_t dir_l, uint8_t vel_r, uint8_t dir_r)
{
  uint8_t data[motor_data_size] = {0};

  if (is_btnA_switch == true)
  {
    vel_l = 0;
    vel_r = 0;
  }
  else if (is_btnA_switch == false && (position_id.x_cube_center < 80 || position_id.x_cube_center > 410 || position_id.y_cube_center > 410 || position_id.y_cube_center < 80))
  {
    vel_l = 0;
    vel_r = 0;
  }

  data[0] = 0x02; //motor control with specifiled time
  data[1] = 0x01; //motor id 1 : left
  data[2] = dir_l; //direction
  data[3] = vel_l; //speed
  data[4] = 0x02; //motor id 2 : right
  data[5] = dir_r; //direction
  data[6] = vel_r; //speed
  data[7] = 0x0A; //control time
  //Serial.println("write value");

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

static void setPosition(uint16_t pos_x, uint16_t pos_y, uint16_t pos_dir, uint8_t vel)
{
  uint8_t data[motor_pos_data_size] = {0};
  data[0] = 0x03; //motor control with specifiled position
  data[1] = 0x00; //
  data[2] = 0x05; //time out
  data[3] = 0x00; //control type
  data[4] = vel; //velocity
  data[5] = 0x00; //constant velocity
  data[6] = 0x00; //RESERVED
  data[7] = pos_x & 0xFF; //target position on X axis Low
  data[8] = (pos_x >> 8) & 0xFF; //target position on X axis
  data[9] = pos_y & 0xFF; //target position on Y axis LOW
  data[10] = (pos_y >> 8) & 0xFF; //target position on Y axis
  //  data[11] = pos_dir & 0xFF; //target direction LOW
  data[11] = 0x00;
  //  data[12] = (pos_dir >> 8) & 0xFF; //target direction
  data[12] = 0xA0;
  motor_characteristic->writeValue(data, sizeof(data));
}


static void sendSoundControl(void)
{
  uint8_t data[sound_data_size] = {0};
  data[0] = 0x02; //0x02:sound effect
  data[1] = current_sound; //sound effects
  data[2] = 0x10; //volume
  //Serial.println("write value");

  sound_characteristic->writeValue(data, sizeof(data));
}

static void drawReadSensor(void)
{
  M5.Lcd.setTextSize(1);
  String x_cube_str = String("X:") + String(position_id.x_cube_center) + String(" ");
  M5.Lcd.drawString(x_cube_str, 10, 20);
  String y_cube_str = String("Y:") + String(position_id.y_cube_center) + String(" ");
  M5.Lcd.drawString(y_cube_str, 10, 30);
  String angle_cube_str = String("Ang:") + String(position_id.angle_cube_center) + String(" ");
  M5.Lcd.drawString(angle_cube_str, 10, 40);
  String standard_id_str = String("StdID:") + String(standard_id.standard_id) + String(" ");
  M5.Lcd.drawString(standard_id_str, 10, 50);
  String standard_id_angle_str = String("StdAng:") + String(standard_id.angle_cube) + String(" ");
  M5.Lcd.drawString(standard_id_angle_str, 10, 60);
  if (is_btnA_switch == true)  M5.Lcd.drawString("STOP", 10, 70);
  else  M5.Lcd.drawString("MOVE", 10, 70);
}


int counter = 0;
int flip = 0;
int wdt_counter = 0;

// This is the Arduino main loop function.
void loop() {

  M5.update();

  checkPushButton();
  if (wdt_counter++ > 100) {
    setupBLE();
    esp_restart();
  }



  // 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) {
    drawReadSensor();
    wdt_counter = 0;
  } 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
  }



  //----Begin UnitV toio relation----


  VSerial.write(0xAF);
  if (VSerial.available())
  {
    uint8_t buffer[6]; //7
    VSerial.readBytes(buffer, 6); //7
    v_data.dx = (buffer[0] << 8) | buffer[1];
    v_data.dy = (buffer[2] << 8) | buffer[3];
    v_data.area = (buffer[4] << 8) | buffer[5];

    Serial.printf("Buffer:%d %d %d %d %d %d ", buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
    M5.Lcd.fillRect(0, 0, TFT_WIDTH, 79, BLACK);
    M5.Lcd.fillRect(0, 80, TFT_WIDTH, TFT_HEIGHT, GREEN);
    M5.Lcd.drawRect(10, 80, 60, 80, WHITE);

    M5.Lcd.setTextSize(1);
    String cx_str = String("cx:") + String(v_data.dx + 250);
    String cy_str = String("cy:") + String(v_data.dy + 250);
    M5.Lcd.drawString(cx_str, 40, 20);
    M5.Lcd.drawString(cy_str, 40, 30);

    if (position_id.x_cube_center > 90 && position_id.x_cube_center < 400 && position_id.y_cube_center > 140 && position_id.y_cube_center < 360) {
      M5.Lcd.fillRect(40 - ((position_id.y_cube_center - 250) / 4), ((position_id.x_cube_center - 250) / 4) + 120, 8, 8, WHITE);
    }


    if (v_data.dx > -160 && v_data.dx < 160 && v_data.dy > -120 && v_data.dy < 120 && v_data.area > 100)
    {
      M5.Lcd.fillRect(((v_data.dy + 120) / 4) - 4 + 10, ((v_data.dx + 160) / 4) - 4 + 80, 8, 8, RED);
      if (v_data.area > 200)
      {
        M5.Lcd.drawString("SHOOT", 10, 10);

        if (counter == 0) {
          if (v_data.dx > -140 && v_data.dx < 140 && v_data.dy > -100 && v_data.dy < 100)
            setPosition((uint16_t)(v_data.dx + 250), (uint16_t)(-v_data.dy + 250), 0x00, 0x30);
          else
            setPosition((uint16_t)(250), (uint16_t)(250), 0x00, 0x30);
        }

        counter++;
        if (counter > 50) {
          counter = 0;

        }

      }
      else
      {
        M5.Lcd.drawString("SEEK", 10, 10);
        //setMotorVelocity(0x10,0x02,0x10,0x01);
        counter = 0;
      }
      String area_str = String("a:") + String(v_data.area);
      M5.Lcd.drawString(area_str, 35, 70);

    }
    else
    {
      M5.Lcd.drawString("LOST", 10, 10);

      counter++;
      if (counter > 20) {
        counter = 0;
        flip = (flip == 0) ? 1 : 0;
      }
      if (flip == 0)setMotorVelocity(0x8, 0x02, 0x08, 0x01);
      else setMotorVelocity(0x08, 0x01, 0x08, 0x02);

      M5.Lcd.drawString(String(flip), 40, 10);
      M5.Lcd.drawString(String(counter), 60, 10);
    }

    M5.Lcd.setTextSize(1);
    Serial.printf("%d, %d, %d, %d\n", v_data.dx, v_data.dy, v_data.area, state);
  }
  else
    counter = 0;


  //---- End UnitV toio relation ----


  delay(20); // Delay a second between loops.
} // End of loop
```