M5stackにWiiヌンチャクを接続してTelloをコントロールする

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

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

当該ページのプログラムを応用することで，
M5stack(あるいはESP32)を使って，Telloを様々に動かすことが可能になります．

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

また，[Tello SDKのrcコマンド](https://qiita.com/hsgucci/items/a5926dd396b319e6ccaf#tello-sdk%E3%81%AE%E3%83%A9%E3%82%B8%E3%82%B3%E3%83%B3%E6%93%8D%E4%BD%9C%E3%82%B3%E3%83%9E%E3%83%B3%E3%83%89)を使うことで，ラジコン操作のようなスムーズな動きも可能になります．
rcコマンドを利用して，これまで以下の記事を書きました．

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

本ページでは，M5StackのGloveコネクタに「Wiiヌンチャク」を接続して，
　**<font color=blue>M5stackとWiiヌンチャクで動的に操縦できるように</font>**
します．

具体的には，以下の動画の様に動かせるようになります．
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">M5stackのgrove i2cポートにWiiヌンチャクを接続し、Telloをコントロールしています。<a href="https://t.co/FqkguKACl2">pic.twitter.com/FqkguKACl2</a></p>&mdash; hsgucci404 (@hsgucci404) <a href="https://twitter.com/hsgucci404/status/1230425442009509888?ref_src=twsrc%5Etfw">February 20, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

#概要
Wiiヌンチャクやクラシックコントローラ等のアクセサリは，ケーブルを介してI2Cで「Wiiリモコン」本体へと繋がっています．
したがって，ArduinoのI2Cプログラム(Wire.h関連)を書くことで，スティックやボタンのデータを取り出すことができます．

M5Stackは，USB Type-Cコネクタの隣に，赤いGroveポート(ポートA)があり，I2Cの端子が来ています．
今回は，ポートAにWiiヌンチャクを接続し，Telloを動かします．
具体的なシステム構成は以下の様になります．
![tello_m5_nunchuck.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/811b832b-209c-0318-b360-13de4ffedffb.png)

#Wiiヌンチャクの利用

WiiヌンチャクをM5Stackで使用するにあたって，ハード・ソフトの両面で以下の問題を解決しなければなりません．

- ハードウェア：Groveコネクタに変更する
- ソフトウェア：ArduinoでWiiヌンチャクとI2C通信する

以下，それぞれ解説していきます．

##ハードウェア

Wiiリモコンへと接続するためのコネクタは，任天堂の特別製です．
Arduinoで工作をする人達は『[WiiChuckアダプタ](https://www.aitendo.com/product/16045)』を使うことが多いのですが，
プリント基板を差し込むだけだと，コネクタが抜けてしまうのがちょっと心配です．

**<font color=red>今回は，コネクタを切り離して，直接ハンダ付けしました．</font>**
(というか，元々Arduino Nanoやmbedで使うために切ってあったので，それを流用しました)

切断したWiiリモコンのケーブル色に関しては，以下が参考になります．
　参考URL：　[ヌンチャクをマウスにしてみよう（ヌンチャク型コントローラ・ミーツ・Digispark）](https://qiita.com/TwDaiki/items/875642572aa27a6d42c0)

また，M5StackのGroveポートAに関しては，以下が参考になります．
　参考URL：　[IoT何をいまさら(34) M5StackのGroveコネクタ](https://jhalfmoon.com/dbc/2019/08/07/iot%E4%BD%95%E3%82%92%E3%81%84%E3%81%BE%E3%81%95%E3%82%8934-m5stack%E3%81%AEgrove%E3%82%B3%E3%83%8D%E3%82%AF%E3%82%BF/)

Seeed Studioの提唱する「Groveシステム」では，特別なコネクタと配色の決まったケーブルを利用します．
以下の製品などを購入して切断すると良いでしょう．（筆者は余っていたGroveケーブルを使いました）
　[M5Stack用GROVE互換ケーブル 10 cm（5個入り）](https://www.switch-science.com/catalog/5213/)

上記のページを参考にした結果をまとめると，

| ピンの役割 | Wiiヌンチャク | Groveケーブル |
|:---:|:---:|:---:|
| 5V  | <font color=red>赤</font> | <font color=red>赤</font> |
| GND | <font color=gray>白</font> | 黒 |
| SCL | <font color=orange>黄</font> | <font color=orange>黄</font> |
| SDA | <font color=green>緑</font> | <font color=gray>白</font> |
となります．
実際に配線すると，こんな風になります．
![grove_nunchuk.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/d86574ff-58ba-8720-370f-046d003a72e9.jpeg)

配線の処理はわずかこれだけです．
本来はSCL,SDAのラインにプルアップ抵抗を入れるべきですが，それも省略してしまいました(^^;;

##ソフトウェア
Wiiヌンチャクのボタンデータを取得するにはI2C通信を使う必要があるのですが，フルスクラッチで書いたら大変です．
Arduinoで簡単に使える有名なライブラリがあります．

　[WiiChuck](https://github.com/madhephaestus/WiiChuck)

（前述の　[変換基板](https://www.aitendo.com/product/16045)　は，このライブラリを使う前提で命名されています）

WiiChuckのインストールの方法は，
Arduino IDEのメニューバー[スケッチ]-[ライブラリをインクルード]-[ライブラリを管理...]
で出てくるライブラリマネージャの検索バーで `wiichuck` と打って検索しましょう．
WiiChuckを見つけたら，[インストール]ボタンをクリックするだけです．
![wiichuck.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/936cb052-1529-753d-9045-87dc8dcfc355.png)

使い方は，メニューバー[ファイル]-[スケッチ例]-[WiiChuck]にサンプルがあります．
![wiichuck_example.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/d6931f2f-3c54-0842-ab2e-3fe59ae1fe54.png)


#スケッチ
M5stackに書き込むスケッチは，以下の様になります．

以下のコードをコピー＆ペーストするか，
[ここ](https://raw.githubusercontent.com/hsgucci404/m5stack_tello/master/TelloControl_Nunchuck.ino)　を右クリックして[名前を付けて保存]機能でファイル保存してください．

```cpp:TelloControll_Nunchuck.ino
#define LOAD_FONT2
#define LOAD_FONT4
#include <M5Stack.h>
#include <WiiChuck.h>
#include <WiFi.h>
#include <WiFiUdp.h>
// TELLOのSSID
const char* TELLO_SSID = "TELLO-XXXXXX";  // 自分のTelloのWi-Fi SSIDを入力
// TELLOのIP
const char* TELLO_IP = "192.168.10.1";
// TELLO_PORT
const int PORT = 8889;
// UDPまわり
WiFiUDP Udp;
char packetBuffer[255];
String message = "";


float ail,ele,thr,rud;
char msg_jx[6];
char msg_jy[6];
char msg_ax[6];
char msg_ay[6];
String status_msg;
char command_str[20];

Accessory nunchuck1;

void setup() {
  //M5Stackの初期設定
  M5.begin();
  //画面表示
  //---タイトル
  M5.Lcd.fillRect(0,0,320,30,TFT_BLUE);
  M5.Lcd.drawCentreString("Tello Controller",160,2,4);
  //---ジョイスティックXの表示
  M5.Lcd.setTextColor(TFT_YELLOW,TFT_BLACK);
  M5.Lcd.drawCentreString(" Joy-X : ",20,30,2);
  sprintf(msg_jx,"%-2.2f",ail);
  M5.Lcd.drawCentreString(msg_jx,88,30,2);
  //---ジョイスティックYの表示
  M5.Lcd.drawCentreString(" Joy-Y : ",240,30,2);
  sprintf(msg_jy,"%-2.2f",ele);
  M5.Lcd.drawCentreString(msg_jy,294,30,2);
  //---加速度Xの表示
  M5.Lcd.drawCentreString(" Accel-X : ",20,50,2);
  sprintf(msg_ax,"%-2.2f",rud);
  M5.Lcd.drawCentreString(msg_ax,88,50,2);
  //---加速度Yの表示
  M5.Lcd.drawCentreString(" Accel-Y : ",240,50,2);
  sprintf(msg_ay,"%-2.2f",thr);
  M5.Lcd.drawCentreString(msg_ay,294,50,2);
  
  //---ボタンエリア
  M5.Lcd.fillRect(0,217,320,20,TFT_LIGHTGREY);
  //---ボタン文字
  M5.Lcd.setTextColor(TFT_BLACK,TFT_YELLOW);
  M5.Lcd.drawCentreString(" TAKE OFF ",64,220,2);
  M5.Lcd.setTextColor(TFT_BLACK,TFT_RED);
  M5.Lcd.drawCentreString(" LANDING  ",250,220,2);
  M5.Lcd.setTextColor(TFT_BLACK,TFT_CYAN);
  M5.Lcd.drawCentreString(" STOP ",160,220,2);
  //---方向矢印
  M5.Lcd.fillTriangle(159,40,189,60,129,60,TFT_GREEN);
  M5.Lcd.fillTriangle(159,160,189,140,129,140,TFT_GREEN);
  M5.Lcd.fillTriangle(269,100,220,80,220,120,TFT_GREEN);
  M5.Lcd.fillTriangle(98,80,98,120,49,100,TFT_GREEN);
  //---方向の文字
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawCentreString("FORWARD",160,64,2);
  M5.Lcd.drawCentreString("BACK",160,120,2);
  M5.Lcd.drawCentreString("LEFT",120,92,2);
  M5.Lcd.drawCentreString("RIGHT",200,92,2);
  //---メッセージ領域
  M5.Lcd.drawRoundRect(0,180,319,30,4,TFT_WHITE);
  //---メッセージのタイトル文字
  M5.Lcd.setTextColor(TFT_WHITE,TFT_DARKGREEN);
  M5.Lcd.drawCentreString("<Message>",38,170,1);
  //---メッセージの文字
  //M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  //M5.Lcd.drawString(msg,4,190,1);
  //初期設定

  //Wireライブラリを初期化
  Wire.begin();

  //Wiiヌンチャクを初期化
  nunchuck1.begin();
  if (nunchuck1.type == Unknown) {
    nunchuck1.type = NUNCHUCK;
  }
    
  //WiFi通信の開始
  WiFi.begin(TELLO_SSID, "");
  //WiFi接続　接続するまでループ
  while (WiFi.status() != WL_CONNECTED) {
        print_msg("Now, WiFi Connecting......");
        delay(500);
  }
  print_msg("WiFi Connected.");
  // UDP
  Udp.begin(PORT);
  //Telloへ”command”送信
  print_msg("sendMessage commend");
  tello_command_exec("command");  

  delay(500);
  print_msg("Operation Start!"); 
}

void loop() {
  // ヌンチャク値の取得
  nunchuck1.readData();

  ail = (nunchuck1.getJoyX()-128) / 100.0;
  ele = (nunchuck1.getJoyY()-128) / 100.0;
  thr = (nunchuck1.getAccelY()-512) / 256.0;
  rud = (nunchuck1.getAccelX()-512) / 256.0;

  // 画面表示
  sprintf(msg_jx,"%-2.2f",ail);
  M5.Lcd.drawCentreString("      ",88,30,2);
  M5.Lcd.drawCentreString(msg_jx,88,30,2);
  sprintf(msg_jy,"%-2.2f",ele);
  M5.Lcd.drawCentreString("      ",294,30,2);
  M5.Lcd.drawCentreString(msg_jy,294,30,2);
  sprintf(msg_ax,"%-2.2f",rud);
  M5.Lcd.drawCentreString("      ",88,50,2);
  M5.Lcd.drawCentreString(msg_ax,88,50,2);
  sprintf(msg_ay,"%-2.2f",thr);
  M5.Lcd.drawCentreString("      ",294,50,2);
  M5.Lcd.drawCentreString(msg_ay,294,50,2);

  //離陸
  if(M5.BtnA.wasPressed()) {  // ボタンA
    print_msg("TAKE OFF"); 
    tello_command_exec("takeoff");
  }
  if(nunchuck1.getButtonC() ) {   // ヌンチャクCボタン
    print_msg("TAKE OFF"); 
    tello_command_exec("takeoff");
  }

  //停止
  if(M5.BtnB.wasPressed()) {    //ボタンB処理
      print_msg("STOP");  // 通信不良などの暴走時にはボタンBで止める
      tello_command_exec("rc 0 0 0 0");
  }

  //着陸
  if(M5.BtnC.wasPressed()) {    //ボタンC処理    
    print_msg("LAND");
    tello_command_exec("land");
  }
  if(nunchuck1.getButtonZ() ) {   // ヌンチャクZボタン
    print_msg("LAND");
    tello_command_exec("land");
  }

  //スティックの0.3，加速度の0.5は実測値から閾値を設定した
  if (fabs(ail)> 0.3 || fabs(ele)> 0.3 || fabs(thr)> 0.5 || fabs(rud)> 0.5){
      // 不感帯設定　(微弱な加速度値で上昇や旋回しないように)
      ail = (fabs(ail)>0.3)? ail : 0.0;
      ele = (fabs(ele)>0.3)? ele : 0.0;
      thr = (fabs(thr)>0.5)? thr : 0.0;
      rud = (fabs(rud)>0.5)? rud : 0.0;

      sprintf(command_str,"rc %d %d %d %d",int(ail*100), int(ele*100), int(-thr*100), int(rud*100) );
      tello_command_exec(command_str);
  } else {
      tello_command_exec("rc 0 0 0 0");
  }    
  
  delay(50);
  M5.update();
}

/////////////////////////////
//      ユーザ関数定義       //
/////////////////////////////

// 画面メッセージエリアへ状況メッセージ表示
void print_msg(String status_msg){
  M5.Lcd.setTextColor(TFT_WHITE,TFT_BLACK);
  M5.Lcd.drawString("                          ",4,190,1);
  M5.Lcd.drawString(status_msg,4,190,1);
  status_msg="";
}

// Telloへメッセージ送信＆コマンド実行
void tello_command_exec(char* tello_command){
  Udp.beginPacket(TELLO_IP, PORT);
  Udp.printf(tello_command);
  Udp.endPacket();
//  message = listenMessage();
  delay(10);
}

// Telloからのメッセージ受信
String listenMessage() {
  int packetSize = Udp.parsePacket();
  if (packetSize) {
    IPAddress remoteIp = Udp.remoteIP();
    int len = Udp.read(packetBuffer, 255);
    if (len > 0) {
      packetBuffer[len] = 0;
    }
  }
  delay(10);
  return (char*) packetBuffer;
}
```
#スケッチの解説

Wiiヌンチャクのクラスをグローバル変数で`Accessory nunchuck1;`と作っています.

ヌンチャクの初期化は，`setup()`関数内で`nunchuck1.begin();`と書きます．

ヌンチャクの値の取得は，`loop()`関数内で`nunchuck1.readData();`で行っています．
その後，以下のメンバ関数(メソッド)を使って，値を取ります．

```cpp:ヌンチャククラスのメソッド
  nunchuck1.getJoyX()      // ジョイスティックX　0〜128〜256
  nunchuck1.getJoyY()      // ジョイスティックY　0〜128〜256
  nunchuck1.getAccelY()    // 加速度X　0〜512〜1024?
  nunchuck1.getAccelX()    // 加速度Y　0〜512〜1024?

  nunchuck1.getButtonC()   // ヌンチャクCボタン オンでtrue
  nunchuck1.getButtonZ()   // ヌンチャクZボタン オフでfalse
```

本家のプログラムでは，加速度の値を±1.0の範囲で正規化していたので，
ここでも同様に正規化を行います．

```cpp:センサ値の正規化
  ail = (nunchuck1.getJoyX()-128) / 100.0;
  ele = (nunchuck1.getJoyY()-128) / 100.0;
  thr = (nunchuck1.getAccelY()-512) / 256.0;
  rud = (nunchuck1.getAccelX()-512) / 256.0;
```
ただし，本来の正規化ならば「128を引いた後に128で割る」「512を引いた後に512で割る」べきなのですが，**<font color=red>実際のセンサ値を見て，除数を調整しました．</font>**
ちなみに，変数名は航空用語の
・ail(aileron：エルロン)
・ele(elevator：エレベータ)
・thr(throttle：スロットル)
・rud(rudder：ラダー)
を使っています．ドローンをやる人にはお馴染みの用語ですね．

本プログラムでは，ヌンチャクを「左右に倒すと旋回」「前に倒すと下降」「手前に引くと上昇」する仕様になっています．
**<font color=blue>スティックや加速度の割り当てを変更することで，自分の好みの操縦方法に変更することができます</font>**ので，試してみてください．

rcコマンドの送信直前には，三項演算子(ハテナ文)を使って不感帯を設定しました．

```cpp:三項演算子で
      // 不感帯設定　(微弱な加速度値で上昇や旋回しないように)
      ail = (fabs(ail)>0.3)? ail : 0.0;
      ele = (fabs(ele)>0.3)? ele : 0.0;
      thr = (fabs(thr)>0.5)? thr : 0.0;
      rud = (fabs(rud)>0.5)? rud : 0.0;
```
スティックを倒すと，手が微妙に倒れてしまって，変に上下したり旋回したりするので，しきい値よりも小さい時は0.0に落としています．


#おわりに
今回は，WiiChuckライブラリとM5StackのGrove I2Cポートを使ってTelloを操縦してみました．

FACESジョイスティックも操縦しやすかったのですが，前後左右移動と旋回上下移動は，同時に行うことができませんでした．
今回のWiiヌンチャクだと同時入力が可能なので，ノーズ イン サークルや８の字飛行なども頑張ればやれます(^^

しかし，筆者が中古で買ったWiiヌンチャクは，左右に倒した時の加速度の値が異なっていて，左旋回だけが非常に敏感でした(T_T
加速度の最大最小値を記録し，左右で異なる係数を掛けるという「キャリブレーション処理」を書くべきですね．

WiiChuckライブラリはヌンチャクだけでなく，クラシックコントローラにも対応しています．
次回は，クラシックコントローラで普通に操縦できるようにしたいと思います．