M5Stackでペンプロッタを作ってみた

M5Stackとステッピングモーター、サーボでペンプロッタを作成しました
自分は手始めとして手に入れやすい部材を使いましたが応用すればもっと高精度なものも作れると思います

こんな感じで動作します

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">うまくいきました🤗<br><br>ただタイムラプスしないといけないくらい時間がかかるんで実用性は微妙かも🤣🤣🤣<a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/MCSxDfefOw">pic.twitter.com/MCSxDfefOw</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1436387816808665089?ref_src=twsrc%5Etfw">September 10, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

## 使用するデバイス
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/9380540d-f7cf-a080-59db-7edfcdff3970.png)

M5Stack Basic - スイッチサイエンス
https://www.switch-science.com/catalog/3647/

Arduino互換なESP32がケースに入っててボタンとディスプレイとバッテリーがついててIOも豊富でWifiとBluetoothも使えるニクい奴

バリエーションによって9軸センサがついてたりするけどbasicは一番基本的な奴

----

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/e7d85b17-f140-0baf-4e77-c45ea9226b5a.png)

M5Stack用GRBLモジュール - スイッチサイエンス
https://www.switch-science.com/catalog/6994/

ステッピングモーターを3個まで制御できるモジュール
スタックすると最大6個まで使えます
Gcodeが使えるので割とお手軽にステッピングモーターを動かせます

----

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/85a150f5-a2c1-9498-02ad-b72e987f35df.png)
M5Stack用12チャンネル サーボドライバモジュール - スイッチサイエンス
https://www.switch-science.com/catalog/6060/

サーボを12個まで制御できるモジュール
現在は型落ちみたいなのでServo2モジュールを使用してください

M5Stack用Servo2モジュール - 16チャンネル サーボドライバ - スイッチサイエンス
https://www.switch-science.com/catalog/6737/

----

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/3d87e2b4-6311-1fd2-c035-192763cb3d3d.png)


Amazon | 3DプリンタガイドレールセットT8リードスクリュー ピッチ1mmリード1mm +リニアシャフト8 * 100mm + KP08 SK8 SC8UU +ナットハウジング+カップリング+ステップモータ (200mm) | リニアガイド | 産業・研究開発用品 通販
https://www.amazon.co.jp/gp/product/B07D3P1YST/

自分はステッピングモータに関しては素人だったのでステッピングモーターとレールがセットになってるものを購入しました

----

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/7e35d01c-2784-04dc-2171-fbc9a65065d7.png)

Amazon.co.jp: マイクロスライディングテーブルステッピングモーターリニアモータースクリュー2相4線リニアステッピングモータースクリュースライドテーブル4‑9V駆動電圧18°ステップ角 : 産業・研究開発用品
https://www.amazon.co.jp/gp/product/B08LQ36RZM/


Y軸には簡易型のリニアレールを使いました
Stepperモジュールが9V~なので対応した電圧の物を用意しましょう
ただこのモーターは4~9Vなんですが短時間で過熱するので実用時は
上記の大きいステッピングモーターを使った方がいいと思います

## ハードを構築する
ホームセンターでアクリル板を買ってきてドリルで穴を開けてX軸のレールをネジ止めしました
左側のステッピングモーターは高さが合わなかったのでアクリル板の外に出して高さを逃がしています

![unnamed.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/3d3c4de3-4c26-37af-d1e8-374cad7a2f58.jpeg)

メモ
赤A+青A-黒B+緑B-
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/36965af0-eb55-1c4d-33a8-6f47332ae54c.png)

Y軸はX軸のアルミブロックのネジ穴の幅がちょうどよかったので
直接ネジ止めしました
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/b7143433-f0d1-4a82-4ddc-9915908ee5d8.png)

リミットスイッチは工具箱にあったタクトスイッチを使いました
グルーガンでアルミブロックに当たるところに固定します
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/0139e594-6a27-607a-3821-20a645375e8a.png)

リミットスイッチはGNDがXYZ共用なので接続するための基板を作りました

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/9b12ef13-991d-98a8-a549-54a402d0ae83.png)

サーボはServoモジュールから制御します

Y軸のステップモーターにサーボを設置します
台座はthingiverseで見つけたアクチュエーターを
Fusion360で切ったり貼ったりして3Dプリンタで出力しました

Linear Servo Actuators by potentprintables - Thingiverse
https://www.thingiverse.com/thing:3170748

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/2012a0cd-d4c8-95c4-1bdf-ba862178fae1.png)

これにペンを固定することでサーボを回して上下することができます

## スケッチを書いていく

Githubのサンプルコードを見ながら書いていきます

以下の.inoと.cppと.hを同じフォルダに入れればコンパイルできると思います

M5Stack/xyz_control.ino at master · m5stack/M5Stack · GitHub
https://github.com/m5stack/MODULE_GRBL13.2/tree/master/examples/xyz_control

MODULE_GRBL13.2/src at master · m5stack/MODULE_GRBL13.2 · GitHub
https://github.com/m5stack/MODULE_GRBL13.2/tree/master/src

基本的にはG1の直線だけ使っています

```C++
_GRBL.sendGcode("G1 X5Y5Z5 F200");
```

ちなみに動作が遅かったので
初期化の引数とかCcodeのFRAMEを変えてみたんですが

```cpp
  _GRBL.Init(400, 400, 400, 30);
  _GRBL.sendGcode("G1 X2 F100");
  _GRBL.sendGcode("G1 X2 F200");
  _GRBL.sendGcode("G1 X3 F300");
  _GRBL.sendGcode("G1 X4 F400");
  _GRBL.sendGcode("G1 X5 F500");
```
脱調するんですよね・・・(´・ω・`)

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">X,YそれぞれF100,F200,F300,F400,F500とスピード変えてテストしてみた<br><br>F100は脱調してるっぽいけどF200からはスムーズでF500でもまだ余裕ありそう<br><br>Y軸はアッチッチになるから遅くしようと思ってたけど素早く動かして放熱時間を取った方がいいかもね <a href="https://t.co/fTuKPPgigJ">pic.twitter.com/fTuKPPgigJ</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1415323063575400448?ref_src=twsrc%5Etfw">July 14, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

うまい設定方法があったら教えてください(´・ω・`)

ステップモーターを動かせるようになったのでこんな関数を作りました

```cpp
void moveTo(float toX, float toY) {
  String command = "G1 X";
  command += String(toX);
  command += " F200";
  Serial.println(command.c_str());
  _GRBL.Gcode((char *)(command.c_str()));
  command = "G1 Y";
  command += String(toY);
  command += " F200";
  Serial.println(command.c_str());
  _GRBL.Gcode((char *)(command.c_str()));
  _GRBL.WaitIdle();
}
```

C言語が分からないのでJavaライクな書き方です(´・ω・`)

## 原点戻しを実装する

リセットするごとに原点が変わっても困るので原点に戻す動作を実装します

まず1だけ＋方向に動かしてからリミットスイッチが押されるまで-0.3ずつ動かし続けます

リミットスイッチが押されると_GRBL.InLock()がtrueになるので
Unlock()してから+に戻します

この時タクトスイッチが押されたままUnlock()するので即lock()されることがあり、
その対策のために動かしてから少し待ってリミットスイッチが押されたままだと再度動かすようにしています

```cpp

void resetOrigin(char *axis) {
  _GRBL.SetMode("distance");  //　相対移動モード
  std::string str = "G1 1 F200";

  str.insert(3, axis);

  // USE Gcode
  _GRBL.Gcode(const_cast<char *>(str.c_str()));  // +1移動

  str = "G1 -0.3 F200";
  str.insert(3, axis);

  while (!_GRBL.InLock()) {
    _GRBL.Gcode(
        const_cast<char *>(str.c_str()));  // -0.2ずつロックされるまで移動
    delay(100);
  }

  Serial.println("Locked");
  Serial.println("Back to Origin");

  str = "G1 0.2 F200";
  str.insert(3, axis);

  while (_GRBL.InLock()) {
    _GRBL.UnLock();  // リミットスイッチのロックを解除する
    _GRBL.SetMode("distance");  // absoluteに戻るので再設定
    _GRBL.Gcode(const_cast<char *>(str.c_str()));
    delay(300);
    // スイッチが押しっぱなしだと即座にロックに戻るので移動してから少し待つ
  }

  _GRBL.SetMode("absolute");
  Serial.println("Moved to Origin");
}

```

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">M5Stackペンプロッタの原点戻し動画(´・ω・`) <a href="https://t.co/M3jWKmBsTY">pic.twitter.com/M3jWKmBsTY</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1438090344822820866?ref_src=twsrc%5Etfw">September 15, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

## 画面をタップする

サーボを回すとペンを上下できるのでペンの上げ下げとタップで関数を作ります

定数は現物合わせで調整してください

```cpp
int PEN_UP = 130;
int PEN_DOWN = 100;

void penUp() {
  // servo(PEN_UP);
  Servo_write_angle(0, PEN_UP);
  delay(100);
}
void penDown() {
  Servo_write_angle(0, PEN_DOWN);
  delay(100);
}
void tap() {
  penDown();
  penUp();
}

void Servo_write_angle(uint8_t number, uint8_t angle) {
  Wire.beginTransmission(SERVO_ADDR);
  Wire.write(0x10 | number);
  Wire.write(angle);
  Wire.endTransmission();
}
```

そうしたら画面の上を動かしてタップする場所を定数にしていきます
今回はデモのために某FG●を操作することにしました

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/e660c394-484c-aeda-d562-b8690089b66d.png)

自分が動かした感じでは横の1マスが10,縦が1くらいになるのでタップする場所を列挙していきます

<details><summary>長くなるので折りたたみ</summary><div>

```cpp
float SKILL_1_1_X = 0;
float SKILL_1_1_Y = 0.7;
float SKILL_1_2_X = 4;
float SKILL_1_2_Y = 0.7;
float SKILL_1_3_X = 7.5;
float SKILL_1_3_Y = 0.7;

float SKILL_2_1_X = 13.5;
float SKILL_2_1_Y = 0.7;
float SKILL_2_2_X = 17.5;
float SKILL_2_2_Y = 0.7;
float SKILL_2_3_X = 21;
float SKILL_2_3_Y = 0.7;

float SKILL_3_1_X = 27.5;
float SKILL_3_1_Y = 0.7;
float SKILL_3_2_X = 31;
float SKILL_3_2_Y = 0.7;
float SKILL_3_3_X = 34.5;
float SKILL_3_3_Y = 0.7;

float OPPONENT_1_X = 17;
float OPPONENT_1_Y = 1.5;
float OPPONENT_2_X = 29;
float OPPONENT_2_Y = 1.5;
float OPPONENT_3_X = 39;
float OPPONENT_3_Y = 1.5;

float ATTACK_X = 52;
float ATTACK_Y = 0.5;

float CARD_1_X = 8;
float CARD_1_Y = 0.7;
float CARD_2_X = 17;
float CARD_2_Y = 0.7;
float CARD_3_X = 29;
float CARD_3_Y = 0.7;
float CARD_4_X = 39;
float CARD_4_Y = 0.7;
float CARD_5_X = 50;
float CARD_5_Y = 0.7;

float HOUGU_1_X = 17;
float HOUGU_1_Y = 3.9;
float HOUGU_2_X = 29;
float HOUGU_2_Y = 3.9;
float HOUGU_3_X = 38;
float HOUGU_3_Y = 3.9;

float MASTER_SKILL_X = 54;
float MASTER_SKILL_Y = 2.6;
float MASTER_SKILL_1_X = 42;
float MASTER_SKILL_1_Y = 2.6;
float MASTER_SKILL_2_X = 46;
float MASTER_SKILL_2_Y = 2.6;
float MASTER_SKILL_3_X = 50;
float MASTER_SKILL_3_Y = 2.6;

float ORDER_CHANGE_1_X = 7;
float ORDER_CHANGE_1_Y = 2.6;
float ORDER_CHANGE_2_X = 15;
float ORDER_CHANGE_2_Y = 2.6;
float ORDER_CHANGE_3_X = 24;
float ORDER_CHANGE_3_Y = 2.6;
float ORDER_CHANGE_4_X = 33;
float ORDER_CHANGE_4_Y = 2.6;
float ORDER_CHANGE_5_X = 41;
float ORDER_CHANGE_5_Y = 2.6;
float ORDER_CHANGE_6_X = 50;
float ORDER_CHANGE_6_Y = 2.6;

float ORDER_EXEC_X = 28;
float ORDER_EXEC_Y = 0.0;
```

</div></details>

タップする座標が分かったらそれぞれを関数にまとめておきます

<details><summary>長くなるので折りたたみ</summary><div>

```cpp
void skill1(int num) {
  Serial.printf("exec Skill1-%d\n", num);

  switch (num) {
    case 1:
      moveTo(SKILL_1_1_X, SKILL_1_1_Y);
      break;
    case 2:
      moveTo(SKILL_1_2_X, SKILL_1_2_Y);
      break;
    case 3:
      moveTo(SKILL_1_3_X, SKILL_1_3_Y);
      break;

    default:
      break;
  }
  tap();
}
void skill2(int num) {
  Serial.printf("exec Skill2-%d\n", num);
  switch (num) {
    case 1:
      moveTo(SKILL_2_1_X, SKILL_2_1_Y);
      break;
    case 2:
      moveTo(SKILL_2_2_X, SKILL_2_2_Y);
      break;
    case 3:
      moveTo(SKILL_2_3_X, SKILL_2_3_Y);
      break;

    default:
      break;
  }
  tap();
}
void skill3(int num) {
  Serial.printf("exec Skill3-%d\n", num);
  switch (num) {
    case 1:
      moveTo(SKILL_3_1_X, SKILL_3_1_Y);
      break;
    case 2:
      moveTo(SKILL_3_2_X, SKILL_3_2_Y);
      break;
    case 3:
      moveTo(SKILL_3_3_X, SKILL_3_3_Y);
      break;

    default:
      break;
  }
  tap();
}

void skillTo(int num) {
  Serial.printf("exec Skill to %d\n", num);
  switch (num) {
    case 1:
      moveTo(OPPONENT_1_X, OPPONENT_1_Y);
      break;
    case 2:
      moveTo(OPPONENT_2_X, OPPONENT_2_Y);
      break;
    case 3:
      moveTo(OPPONENT_3_X, OPPONENT_3_Y);
      break;

    default:
      break;
  }
  tap();
}

void masterSkill(int num) {
  moveTo(MASTER_SKILL_X, MASTER_SKILL_Y);
  tap();
  switch (num) {
    case 1:
      moveTo(MASTER_SKILL_1_X, MASTER_SKILL_1_Y);
      break;
    case 2:
      moveTo(MASTER_SKILL_2_X, MASTER_SKILL_2_Y);
      break;
    case 3:
      moveTo(MASTER_SKILL_3_X, MASTER_SKILL_3_Y);
      break;

    default:
      break;
  }
  tap();
}

void orderChange(int num) {
  switch (num) {
    case 1:
      moveTo(ORDER_CHANGE_1_X, ORDER_CHANGE_1_Y);
      break;
    case 2:
      moveTo(ORDER_CHANGE_2_X, ORDER_CHANGE_2_Y);
      break;
    case 3:
      moveTo(ORDER_CHANGE_3_X, ORDER_CHANGE_3_Y);
      break;
    case 4:
      moveTo(ORDER_CHANGE_4_X, ORDER_CHANGE_4_Y);
      break;
    case 5:
      moveTo(ORDER_CHANGE_5_X, ORDER_CHANGE_5_Y);
      break;
    case 6:
      moveTo(ORDER_CHANGE_6_X, ORDER_CHANGE_6_Y);
      break;
    case 7:
      moveTo(ORDER_EXEC_X, ORDER_EXEC_Y);
      break;

    default:
      break;
  }
  tap();
}

void hougu(int num) {
  switch (num) {
    case 1:
      moveTo(HOUGU_1_X, HOUGU_1_Y);
      break;
    case 2:
      moveTo(HOUGU_2_X, HOUGU_2_Y);
      break;
    case 3:
      moveTo(HOUGU_3_X, HOUGU_3_Y);
      break;

    default:
      break;
  }
  tap();
}

void card(int num) {
  switch (num) {
    case 1:
      moveTo(CARD_1_X, CARD_1_Y);
      break;
    case 2:
      moveTo(CARD_2_X, CARD_2_Y);
      break;
    case 3:
      moveTo(CARD_3_X, CARD_3_Y);
      break;
    case 4:
      moveTo(CARD_4_X, CARD_4_Y);
      break;
    case 5:
      moveTo(CARD_5_X, CARD_5_Y);
      break;

    default:
      break;
  }
  tap();
}

void atack() {
  moveTo(ATTACK_X, ATTACK_Y);
  tap();
}
```

</div></details>

## コマンドを指定できるようにする
図形を書くにしろ画面をタップするにしろタップする場所を連続して指定しないといけないので
タップするコマンドをSDカード上のファイルに保存して選択できるようにします

ここはESP-NOWなりUARTなりで通信してもいいと思いますので都合のいい方法を選んでください

今回はこのようなJSONを用意しました

```json

{
    "data": [
        {
            "name": "rans",
            "command": {
                "wave1": "SK11,ST3,SK21,ST3,MS3,ST3,AT,HG3,CD4,CD5",
                "wave2": "SK23,ST3,SK33,AT,HG3,CD4,CD5",
                "wave3": "SK12,SK22,SK13,ST3,AT,HG3,CD4,CD5"
            }
        }
    ]
}

```

コマンドの意味は
SKxy→x人目のスキルyを実行
STx→スキルのターゲットをx人目とする
MSx→マスタースキルを使う
AT→Attackボタンを押す
HGx→x人目の宝具カードを選択
CDx→x番目のカードを選択
となっています

JSONをSDカード上に置いたら読み込んでjsonArrayに格納します
ArduinoJsonを使用しました

ArduinoJson: Efficient JSON serialization for embedded C++
https://arduinojson.org/

```cpp
  File f = SD.open("/fgo.json");
  DeserializationError error = deserializeJson(jsonObject, f);
  JsonArray jsonArry = jsonObject["data"].as<JsonArray>();
```

JSONの内容が取得出来たらM5Stack TreeViewに渡して選択できるようにします

GitHub - lovyan03/M5Stack_TreeView: M5Stack TreeView menu UI library.
https://github.com/lovyan03/M5Stack_TreeView

```cpp
  tv.itemHeight = 25;
  tv.itemWidth = 100;
  tv.setTextFont(2);

  for (int i = 0; i < jsonArry.size(); i++) {
    Serial.printf("sd:jsonArry[0].name=%s\n",
                  (const char *)(jsonArry[i]["name"]));
  }
  for (int i = 0; i < jsonArry.size(); i++) {
    tv.addItems(std::vector<MenuItem *>{
        new MenuItem((const char *)(jsonArry[i]["name"]), i,
                     std::vector<MenuItem *>{new MenuItem("Wave1", 1, func),
                                             new MenuItem("Wave2", 2, func),
                                             new MenuItem("Wave3", 3, func

                                                          )})});
  }
  tv.begin();

```

TreeViewでコマンドを選択するとtag番号が取得できるのでJsonArrayから該当のコマンドを取得します

```cpp
void func(MenuItem *mi) {
  Serial.print(mi->parentItem()->tag);
  Serial.print(":");
  Serial.println(mi->tag);

  JsonArray jsonArry = jsonObject["data"].as<JsonArray>();
  Serial.printf("sd:jsonArry.size=%d\n", jsonArry.size());

  String str = String(mi->tag);

  str = "wave" + str;
  Serial.printf("name:%s\n",
                (const char *)jsonArry[mi->parentItem()->tag]["name"]);
  Serial.printf(
      "%s:%s\n", str,
      (const char *)(jsonArry[mi->parentItem()->tag]["command"][str]));
  decodeCommand(jsonArry[mi->parentItem()->tag]["command"][str]);
}

```

該当のコマンド行が取得出来たら行をデコードして先ほど作った該当の位置をタップする関数に渡します

```cpp

void decodeCommand(String str) {
  Serial.printf("command:%s\n", str.c_str());

  int index = 0;

  while (true) {
    String com = str.substring(index, str.indexOf(",", index));
    Serial.printf("%s\n", com);

    index = str.indexOf(",", index) + 1;

    if (com.startsWith("SK")) {
      // Serial.println("Skill");
      // Serial.println(atoi(com.substring(2, 3).c_str()));
      switch (atoi(com.substring(2, 3).c_str())) {
        case 1:
          skill1(atoi(com.substring(3).c_str()));
          break;
        case 2:
          skill2(atoi(com.substring(3).c_str()));
          break;
        case 3:
          skill3(atoi(com.substring(3).c_str()));
          break;
        default:
          break;
      }
    } else if (com.startsWith("AT")) {
      atack();
    } else if (com.startsWith("MS")) {
      masterSkill(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("ST")) {
      skillTo(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("OC")) {
      orderChange(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("HG")) {
      hougu(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("CD")) {
      card(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("RS")) {
      resetOrigin("X");
      resetOrigin("Y");
    }

    if (index == 0) {
      break;
    }
  }
}
```

これでJSONに列挙されたコマンドが順次実行されて自動操縦が可能になります

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">うまくいきました🤗<br><br>ただタイムラプスしないといけないくらい時間がかかるんで実用性は微妙かも🤣🤣🤣<a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/MCSxDfefOw">pic.twitter.com/MCSxDfefOw</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1436387816808665089?ref_src=twsrc%5Etfw">September 10, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


最後にメッチャ長くなりましたがソースを置いておきます


<details><summary>長くなるので折りたたみ</summary><div>

```cpp
#include <ArduinoJson.h>
#include <M5Stack.h>
#include <M5TreeView.h>
#include <string.h>

#include "GrblControl.h"

#define STEPMOTOR_I2C_ADDR 0x70
#define SERVO_ADDR 0x53
GRBL _GRBL = GRBL(STEPMOTOR_I2C_ADDR);

int degree = 0;

int PEN_UP = 130;
int PEN_DOWN = 100;

float SKILL_1_1_X = 0;
float SKILL_1_1_Y = 0.7;
float SKILL_1_2_X = 4;
float SKILL_1_2_Y = 0.7;
float SKILL_1_3_X = 7.5;
float SKILL_1_3_Y = 0.7;

float SKILL_2_1_X = 13.5;
float SKILL_2_1_Y = 0.7;
float SKILL_2_2_X = 17.5;
float SKILL_2_2_Y = 0.7;
float SKILL_2_3_X = 21;
float SKILL_2_3_Y = 0.7;

float SKILL_3_1_X = 27.5;
float SKILL_3_1_Y = 0.7;
float SKILL_3_2_X = 31;
float SKILL_3_2_Y = 0.7;
float SKILL_3_3_X = 34.5;
float SKILL_3_3_Y = 0.7;

float OPPONENT_1_X = 17;
float OPPONENT_1_Y = 1.5;
float OPPONENT_2_X = 29;
float OPPONENT_2_Y = 1.5;
float OPPONENT_3_X = 39;
float OPPONENT_3_Y = 1.5;

float ATTACK_X = 52;
float ATTACK_Y = 0.5;

float CARD_1_X = 8;
float CARD_1_Y = 0.7;
float CARD_2_X = 17;
float CARD_2_Y = 0.7;
float CARD_3_X = 29;
float CARD_3_Y = 0.7;
float CARD_4_X = 39;
float CARD_4_Y = 0.7;
float CARD_5_X = 50;
float CARD_5_Y = 0.7;

float HOUGU_1_X = 17;
float HOUGU_1_Y = 3.9;
float HOUGU_2_X = 29;
float HOUGU_2_Y = 3.9;
float HOUGU_3_X = 38;
float HOUGU_3_Y = 3.9;

float MASTER_SKILL_X = 54;
float MASTER_SKILL_Y = 2.6;
float MASTER_SKILL_1_X = 42;
float MASTER_SKILL_1_Y = 2.6;
float MASTER_SKILL_2_X = 46;
float MASTER_SKILL_2_Y = 2.6;
float MASTER_SKILL_3_X = 50;
float MASTER_SKILL_3_Y = 2.6;

float ORDER_CHANGE_1_X = 7;
float ORDER_CHANGE_1_Y = 2.6;
float ORDER_CHANGE_2_X = 15;
float ORDER_CHANGE_2_Y = 2.6;
float ORDER_CHANGE_3_X = 24;
float ORDER_CHANGE_3_Y = 2.6;
float ORDER_CHANGE_4_X = 33;
float ORDER_CHANGE_4_Y = 2.6;
float ORDER_CHANGE_5_X = 41;
float ORDER_CHANGE_5_Y = 2.6;
float ORDER_CHANGE_6_X = 50;
float ORDER_CHANGE_6_Y = 2.6;

float ORDER_EXEC_X = 28;
float ORDER_EXEC_Y = 0.0;

float x = 0.0;
float y = 0.0;

M5TreeView tv;

StaticJsonDocument<2048> jsonObject;

// addr 0x11 means "control the number 1 servo by angle"
void Servo_write_angle(uint8_t number, uint8_t angle) {
  Wire.beginTransmission(SERVO_ADDR);
  Wire.write(0x10 | number);
  Wire.write(angle);
  Wire.endTransmission();
}

void penUp() {
  // servo(PEN_UP);
  Servo_write_angle(0, PEN_UP);
  delay(100);
  // myservo.detach();
}
void penDown() {
  Servo_write_angle(0, PEN_DOWN);
  delay(100);
  // myservo.detach();
}

void resetOrigin(char *axis) {
  _GRBL.SetMode("distance");  //　相対移動モード
  std::string str = "G1 1 F200";

  str.insert(3, axis);

  // USE Gcode
  _GRBL.Gcode(const_cast<char *>(str.c_str()));  // +1移動

  str = "G1 -0.3 F200";
  str.insert(3, axis);

  while (!_GRBL.InLock()) {
    _GRBL.Gcode(
        const_cast<char *>(str.c_str()));  // -0.2ずつロックされるまで移動
    delay(100);
  }

  Serial.println("Locked");
  Serial.println("Back to Origin");

  str = "G1 0.2 F200";
  str.insert(3, axis);

  while (_GRBL.InLock()) {
    _GRBL.UnLock();  // リミットスイッチのロックを解除する
    _GRBL.SetMode("distance");  // absoluteに戻るので再設定
    _GRBL.Gcode(const_cast<char *>(str.c_str()));
    delay(300);
    // スイッチが押しっぱなしだと即座にロックに戻るので移動してから少し待つ
  }

  _GRBL.SetMode("absolute");
  Serial.println("Moved to Origin");
}

void moveTo(float toX, float toY) {
  String command = "G1 X";
  command += String(toX);
  command += " F200";
  Serial.println(command.c_str());
  _GRBL.Gcode((char *)(command.c_str()));
  command = "G1 Y";
  command += String(toY);
  command += " F200";
  Serial.println(command.c_str());
  _GRBL.Gcode((char *)(command.c_str()));
  _GRBL.WaitIdle();
}

void tap() {
  penDown();
  penUp();
}

void skill1(int num) {
  Serial.printf("exec Skill1-%d\n", num);

  switch (num) {
    case 1:
      moveTo(SKILL_1_1_X, SKILL_1_1_Y);
      break;
    case 2:
      moveTo(SKILL_1_2_X, SKILL_1_2_Y);
      break;
    case 3:
      moveTo(SKILL_1_3_X, SKILL_1_3_Y);
      break;

    default:
      break;
  }
  tap();
}
void skill2(int num) {
  Serial.printf("exec Skill2-%d\n", num);
  switch (num) {
    case 1:
      moveTo(SKILL_2_1_X, SKILL_2_1_Y);
      break;
    case 2:
      moveTo(SKILL_2_2_X, SKILL_2_2_Y);
      break;
    case 3:
      moveTo(SKILL_2_3_X, SKILL_2_3_Y);
      break;

    default:
      break;
  }
  tap();
}
void skill3(int num) {
  Serial.printf("exec Skill3-%d\n", num);
  switch (num) {
    case 1:
      moveTo(SKILL_3_1_X, SKILL_3_1_Y);
      break;
    case 2:
      moveTo(SKILL_3_2_X, SKILL_3_2_Y);
      break;
    case 3:
      moveTo(SKILL_3_3_X, SKILL_3_3_Y);
      break;

    default:
      break;
  }
  tap();
}

void skillTo(int num) {
  Serial.printf("exec Skill to %d\n", num);
  switch (num) {
    case 1:
      moveTo(OPPONENT_1_X, OPPONENT_1_Y);
      break;
    case 2:
      moveTo(OPPONENT_2_X, OPPONENT_2_Y);
      break;
    case 3:
      moveTo(OPPONENT_3_X, OPPONENT_3_Y);
      break;

    default:
      break;
  }
  tap();
}

void masterSkill(int num) {
  moveTo(MASTER_SKILL_X, MASTER_SKILL_Y);
  tap();
  switch (num) {
    case 1:
      moveTo(MASTER_SKILL_1_X, MASTER_SKILL_1_Y);
      break;
    case 2:
      moveTo(MASTER_SKILL_2_X, MASTER_SKILL_2_Y);
      break;
    case 3:
      moveTo(MASTER_SKILL_3_X, MASTER_SKILL_3_Y);
      break;

    default:
      break;
  }
  tap();
}

void orderChange(int num) {
  switch (num) {
    case 1:
      moveTo(ORDER_CHANGE_1_X, ORDER_CHANGE_1_Y);
      break;
    case 2:
      moveTo(ORDER_CHANGE_2_X, ORDER_CHANGE_2_Y);
      break;
    case 3:
      moveTo(ORDER_CHANGE_3_X, ORDER_CHANGE_3_Y);
      break;
    case 4:
      moveTo(ORDER_CHANGE_4_X, ORDER_CHANGE_4_Y);
      break;
    case 5:
      moveTo(ORDER_CHANGE_5_X, ORDER_CHANGE_5_Y);
      break;
    case 6:
      moveTo(ORDER_CHANGE_6_X, ORDER_CHANGE_6_Y);
      break;
    case 7:
      moveTo(ORDER_EXEC_X, ORDER_EXEC_Y);
      break;

    default:
      break;
  }
  tap();
}

void hougu(int num) {
  switch (num) {
    case 1:
      moveTo(HOUGU_1_X, HOUGU_1_Y);
      break;
    case 2:
      moveTo(HOUGU_2_X, HOUGU_2_Y);
      break;
    case 3:
      moveTo(HOUGU_3_X, HOUGU_3_Y);
      break;

    default:
      break;
  }
  tap();
}

void card(int num) {
  switch (num) {
    case 1:
      moveTo(CARD_1_X, CARD_1_Y);
      break;
    case 2:
      moveTo(CARD_2_X, CARD_2_Y);
      break;
    case 3:
      moveTo(CARD_3_X, CARD_3_Y);
      break;
    case 4:
      moveTo(CARD_4_X, CARD_4_Y);
      break;
    case 5:
      moveTo(CARD_5_X, CARD_5_Y);
      break;

    default:
      break;
  }
  tap();
}

void atack() {
  moveTo(ATTACK_X, ATTACK_Y);
  tap();
}

void decodeCommand(String str) {
  Serial.printf("command:%s\n", str.c_str());

  int index = 0;

  while (true) {
    String com = str.substring(index, str.indexOf(",", index));
    Serial.printf("%s\n", com);

    index = str.indexOf(",", index) + 1;

    if (com.startsWith("SK")) {
      // Serial.println("Skill");
      // Serial.println(atoi(com.substring(2, 3).c_str()));
      switch (atoi(com.substring(2, 3).c_str())) {
        case 1:
          skill1(atoi(com.substring(3).c_str()));
          break;
        case 2:
          skill2(atoi(com.substring(3).c_str()));
          break;
        case 3:
          skill3(atoi(com.substring(3).c_str()));
          break;
        default:
          break;
      }
    } else if (com.startsWith("AT")) {
      atack();
    } else if (com.startsWith("MS")) {
      masterSkill(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("ST")) {
      skillTo(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("OC")) {
      orderChange(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("HG")) {
      hougu(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("CD")) {
      card(atoi(com.substring(2).c_str()));
    } else if (com.startsWith("RS")) {
      resetOrigin("X");
      resetOrigin("Y");
    } else if (com.startsWith("AC")) {
      if (clockStarted) {
        clockStarted = false;
      } else {
        clockStarted = true;
        xTaskCreatePinnedToCore(clockTask, "clockTask", 4096, NULL, 1, NULL, 0);
      }

    }

    if (index == 0) {
      break;
    }
  }
}

void func(MenuItem *mi) {
  Serial.print(mi->parentItem()->tag);
  Serial.print(":");
  Serial.println(mi->tag);

  JsonArray jsonArry = jsonObject["data"].as<JsonArray>();
  Serial.printf("sd:jsonArry.size=%d\n", jsonArry.size());

  String str = String(mi->tag);

  str = "wave" + str;
  Serial.printf("name:%s\n",
                (const char *)jsonArry[mi->parentItem()->tag]["name"]);
  Serial.printf(
      "%s:%s\n", str,
      (const char *)(jsonArry[mi->parentItem()->tag]["command"][str]));
  decodeCommand(jsonArry[mi->parentItem()->tag]["command"][str]);
}

void setup() {
  M5.begin(true, true, true, true);

  _GRBL.Init(400, 400, 400, 30);
  _GRBL.SetMode("absolute");
  dacWrite(25, 0);  //　ノイズ対策

  tv.itemHeight = 25;
  tv.itemWidth = 100;
  tv.setTextFont(2);

  File f = SD.open("/fgo.json");

  DeserializationError error = deserializeJson(jsonObject, f);

  if (error) {
    Serial.print(F("deserializeJson() failed: "));
    Serial.println(error.f_str());
    return;
  }
  JsonArray jsonArry = jsonObject["data"].as<JsonArray>();
  Serial.printf("sd:jsonArry.size=%d\n", jsonArry.size());

  for (int i = 0; i < jsonArry.size(); i++) {
    Serial.printf("sd:jsonArry[0].name=%s\n",
                  (const char *)(jsonArry[i]["name"]));
  }
  for (int i = 0; i < jsonArry.size(); i++) {
    tv.addItems(std::vector<MenuItem *>{
        new MenuItem((const char *)(jsonArry[i]["name"]), i,
                     std::vector<MenuItem *>{new MenuItem("Wave1", 1, func),
                                             new MenuItem("Wave2", 2, func),
                                             new MenuItem("Wave3", 3, func

                                                          )})});
  }
  tv.begin();
  // penUp();
  delay(1000);
  Servo_write_angle(0, PEN_UP);
}

void loop() {
  M5.update();
  tv.update();

```

</div></details>