M5 ATOM MatrixでTelloをコントロールする

#参考にしたページ
Ryze Tech. の Tello を M5 ATOMで動かす方法のベースになっているのは，以下のページです．

　参考URL：[TelloをM5StickCでコントロールするスケッチを作りました](https://qiita.com/Mitsu-Murakita/items/c078752a570bf1295782)

M5Atom Matrixに搭載されているIMUはMPU6886なので，M5StickCと同じです．
当該ページのプログラムを応用することで，
M5Atom MatrixでもTelloを様々に動かすことが可能になります．
ただし，M5Atom LiteはIMUが無いので使えません(>_<

#はじめに
M5StickCの加速度センサーを使ってTelloを操縦する方法は，上記のサイトを利用すれば可能です．

これまで，Tello SDKのrcコマンドを利用して，以下の記事を書きました．

- [M5stackからrcコマンドでTelloをコントロールする](https://qiita.com/hsgucci/items/5069da43ffcd7c6375f9)
- [M5stackのFACESジョイスティックでTelloをコントロールする](https://qiita.com/hsgucci/items/4571c060ece376f80db3)
- [M5stackにWiiヌンチャクを接続してTelloをコントロールする](https://qiita.com/hsgucci/items/8837434f2d5d3e266095)

本ページでは，新たに発売された
　　**<font color=blue>M5Atom Matrixを使ってTelloを操縦</font>**
できるようにします．

具体的には，以下の動画のように動かせるようになります．
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">M5 Atom MatrixでTelloを操縦。 <a href="https://t.co/0UENbOzD7N">pic.twitter.com/0UENbOzD7N</a></p>&mdash; hsgucci404 (@hsgucci404) <a href="https://twitter.com/hsgucci404/status/1284055355475898369?ref_src=twsrc%5Etfw">July 17, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

M5AtomはStackやStickCと違って，バッテリーを内蔵していないので，モバイルバッテリーから電力を供給しています(^^

#概要
M5StickCと異なり，M5Atom MatrixやLiteはI/Oとして使えるボタンが１つしかありません．
なので，これに離陸/着陸に割り当ててしまうと，
[これまで](https://qiita.com/hsgucci/items/5069da43ffcd7c6375f9) のように余ったボタンで左右旋回や上昇/下降モードに切り替える事ができなくなってしまいます．．．

そこで，実はM5StackやM5StickCでこっそり実験していた「**<font color=red>裏返しモード</font>**」をM5Atomに実装する事にしました．
「裏返しモード」とは，これです．
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">M5StackのZ軸加速度を有効利用して、片手で全ての操作を〜 <a href="https://t.co/hUxgLSLEg6">pic.twitter.com/hUxgLSLEg6</a></p>&mdash; hsgucci404 (@hsgucci404) <a href="https://twitter.com/hsgucci404/status/1272702155661533184?ref_src=twsrc%5Etfw">June 16, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

マイコンを裏返すことで，Z軸方向の加速度センサーには負の値が入ります．
if文でZ軸加速度の正負を見て，送信するrcコマンドを切り替えれば良いのです．

システム構成図としては，こんな風になります．
![m5atom_tello.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/7581f25f-026a-2656-e20c-110a04302e85.png)

あ，M5StackやStickCでの裏返しモードの記事も，今後書く予定です(^^;;

#スケッチ
M5Atom Matrixに書き込むスケッチです．

[ここ](https://raw.githubusercontent.com/hsgucci404/m5stack_tello/master/TelloControl-M5AtomMat.ino)　を右クリックして[名前を付けて保存]機能でファイル保存してください．
日本語バージョンは以下に書いてあります．

```cpp:TelloControl_Atom.ino
#include <M5Atom.h>
#include <WiFi.h>
#include <WiFiUdp.h>

// TELLOの設定
const char* TELLO_SSID = "TELLO-xxxxxx";  // 自分のTelloのSSIDをここに書く
const char* TELLO_IP = "192.168.10.1";    // TELLOのIPアドレス
const int PORT = 8889;                    // TELLOのPORT

// WiFi関連の変数
WiFiUDP Udp;
char packetBuffer[255];
String message = "";
char command_str[20];

// MPU6886関連の変数
bool IMU6886Flag = false;

// 加速度を格納する変数
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float x, y, z;

// setup関数はリセット時１度だけ実行される:
void setup() {
  M5.begin();
  // ATOMのMPU6886のサンプルと同じ処理
  if (M5.IMU.Init() != 0)
    IMU6886Flag = false;
  else
    IMU6886Flag = true;

  // 初期化
  Wire.begin();
  WiFi.begin(TELLO_SSID, "");   // default Tello has no password

  // WiFiでTelloに接続するまで待ち続ける
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }

  // UDPクライアント開始
  Udp.begin(PORT);

  // "command"をTelloに送信すると，SDKモードに変わる
  tello_command_exec("command");

  delay(500); // 安定のための待ち時間
}

// loop関数は繰り返し呼び出される．永久ループ:
void loop() {
  // センサーから加速度を取得する
  M5.IMU.getAccelData(&accX, &accY, &accZ);
  x = accX;
  y = accY;
  z = accZ;

  // ボタンBの処理
  // 着陸
  if (M5.Btn.wasPressed()) {  // ボタンBを押してすぐ離すと，着陸
    tello_command_exec("land");
    delay(500);
  }
  // 離陸
  if (M5.Btn.pressedFor(300)) { // ボタンBを300msec押し続けると，離陸
    tello_command_exec("takeoff");
    delay(500);
  }

  // ATOM Matrixは，LEDが上のときz < 0になる．
  if ( z < 0 ) {
    // 0.3以上傾いたときだけ，前後左右移動を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc %d %d 0 0", int(x * 100), int(-y * 100) );   // a:left/right, b:forward/backward
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  } else if ( z > 0 ) {   // ATOM Matrix は裏返しのとき，z > 0
    // 0.3以上傾いたときだけ，上下と旋回を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc 0 0 %d %d", int(y * 100), int(-x * 100) );   // c:up/down d:yaw
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  }

  M5.update();
}

// UDP send command
void tello_command_exec(char* tello_command) {
  Udp.beginPacket(TELLO_IP, PORT);
  Udp.printf(tello_command);
  Udp.endPacket();
  delay(100);
}
```

#スケッチ解説

スケッチは，
　[M5StickCを使う元祖の記事](https://qiita.com/Mitsu-Murakita/items/c078752a570bf1295782)
と
　[M5ATOMのMPU6886を利用するサンプル](https://github.com/m5stack/M5Atom/blob/master/examples/Basics/MPU6886/MPU6886.ino)
を融合させる形で作成しました．

ATOMは，Arduinoのボードとしては`M5Stick-C`をセットします．
マイコン自体はM5StickCと同じなので．

##include
includeで気をつけたいのは，ATOMは`M5Atom.h`を使う点です．
M5StickCのときは`M5StickC.h`でした．変更し忘れないよう注意しましょう．

```cpp:include
#include <M5Atom.h>
#include <WiFi.h>
#include <WiFiUdp.h>
```

##グローバル変数
TelloのSSIDの記述や，WiFiに関する部分は，[M5StickCを使う元祖の記事](https://qiita.com/Mitsu-Murakita/items/c078752a570bf1295782)のままです．

```cpp:グローバル変数
// TELLOの設定
const char* TELLO_SSID = "TELLO-xxxxxx";  // 自分のTelloのSSIDをここに書く
const char* TELLO_IP = "192.168.10.1";    // TELLOのIPアドレス
const int PORT = 8889;                    // TELLOのPORT

// WiFi関連の変数
WiFiUDP Udp;
char packetBuffer[255];
String message = "";
char command_str[20];

// MPU6886関連の変数
bool IMU6886Flag = false;

// 加速度を格納する変数
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float x, y, z;
```
元祖の記事では，電源投入後に加速度センサの値を何回か取得してオフセット量を設定する処理がありました．
しかし，小さなATOMはUSBケーブルのテンションでも傾いてしまうので，水平ではない姿勢でリセットした場合には，逆に変なバイアスがかかってしまい，水平にしてるのにTelloが動く，という現象が多発しました．
なのでオフセット計算の部分は，ざっくりオミットしました．

また，裏返しモードを使うために，Z軸の加速度を取るfloat型変数も用意しました．

##setup関数
setup関数は，ほぼ元祖の記事と同じですが，StickCと異なる部分を削除しました．

- ATOMは液晶画面が無いので，LCD関連の関数を削除
- 加速度のキャリブレーション(オフセット値計算）を削除

おかげで，ずいぶんスッキリしました．

```cpp:setup関数
// setup関数はリセット時１度だけ実行される:
void setup() {
  M5.begin();
  // ATOMのMPU6886のサンプルと同じ処理
  if (M5.IMU.Init() != 0)
    IMU6886Flag = false;
  else
    IMU6886Flag = true;

  // 初期化
  Wire.begin();
  WiFi.begin(TELLO_SSID, "");   // default Tello has no password

  // WiFiでTelloに接続するまで待ち続ける
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }

  // UDPクライアント開始
  Udp.begin(PORT);

  // "command"をTelloに送信すると，SDKモードに変わる
  tello_command_exec("command");

  delay(500); // 安定のための待ち時間
}
```

##loop関数
loop関数も，LCD関連の命令がなくなったので，読みやすくなっています．

```cpp:loop関数
void loop() {
  // センサーから加速度を取得する
  M5.IMU.getAccelData(&accX, &accY, &accZ);
  x = accX;
  y = accY;
  z = accZ;

  // ボタンBの処理
  // 着陸
  if (M5.Btn.wasPressed()) {  // ボタンBを押してすぐ離すと，着陸
    tello_command_exec("land");
    delay(500);
  }
  // 離陸
  if (M5.Btn.pressedFor(300)) { // ボタンBを300msec押し続けると，離陸
    tello_command_exec("takeoff");
    delay(500);
  }

  // ATOM Matrixは，NeoPixelが上のときz < 0になる．
  if ( z < 0 ) {
    // 0.3以上傾いたときだけ，前後左右移動を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc %d %d 0 0", int(x * 100), int(-y * 100) );   // a:left/right, b:forward/backward
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  } else if ( z > 0 ) {   // ATOM Matrix は裏返しのとき，z > 0
    // 0.3以上傾いたときだけ，上下と旋回を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc 0 0 %d %d", int(y * 100), int(-x * 100) );   // c:up/down d:yaw
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  }

  M5.update();
}
```
ポイントは，ボタンBの処理に使われている２つの命令です．

- M5.Btn.pressed()：ボタンを押してすぐ離した場合
- M5.Btn.pressedFor(300)：ボタンを引数で与えたミリ秒間押し続けた場合

離陸する際には，ボタンBを長押しします．
一方，ボタンをポンッと短押しすれば着陸です．
予想外の挙動をしたり，危ないときなどにはパニックになるので，長押しで着陸にはしませんでした．

##裏返しモード
ATOMは，Z加速度の値が，我々の一般的感覚と逆です．
![m5atom_accZ.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/dd16088f-8921-67c2-6924-55db3ca9b176.png)
おそらく回路基板に上下逆に搭載されているんでしょう．
っと思っていたら，ATOMを分解した猛者の方がいました．
[M5Atom(M5Atom MatrixとM5Atom Lite)を手に入れたので調べてみた](http://kako.com/blog/?p=47273)
この写真を見ると，
![m5atom_open.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/0383b2ec-9ea2-459c-326a-854ab22b199e.png)
基板の上面にNeoPixelが，裏側にIMUがあるのがわかります．

と，言う訳で，プログラムの際には正・負を逆にして書かなければなりません．

```cpp:ATOMを裏返したときに処理を変える
  // ATOM Matrixは，NeoPixel側が上のときz < 0になる．
  if ( z < 0 ) {
    // 0.3以上傾いたときだけ，前後左右移動を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc %d %d 0 0", int(x * 100), int(-y * 100) );   // a:left/right, b:forward/backward
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  } else if ( z > 0 ) {   // ATOM Matrix は裏返しのとき，z > 0
    // 0.3以上傾いたときだけ，上下と旋回を有効にする
    if (fabs(x) > 0.3 || fabs(y) > 0.3) {
      sprintf(command_str, "rc 0 0 %d %d", int(y * 100), int(-x * 100) );   // c:up/down d:yaw
    } else {
      sprintf(command_str, "rc 0 0 0 0" );
    }
    tello_command_exec(command_str);  // rcコマンドを送信
  }

```
それ以外の処理は，これまでのrcコマンドと同じです．
意外と簡単にかけることがわかったかと思います．


#おわりに

M5Atomの様な超小型のマイコンでTelloが動かせるとなると，手袋に仕込んだり，魔法の杖や剣っぽい物に仕込んだりして，格好良く作りたくなってきます．