LoginSignup
7
6

More than 3 years have passed since last update.

@hsgucci

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

Last updated at Posted at 2020-03-23

参考にしたページ

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

　参考URL：TelloをM5Stack Grayでコントロールするスケッチを作りました

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

はじめに

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

また，Tello SDKのrcコマンドを使うことで，ラジコン操作のようなスムーズな動きも可能になります．
rcコマンドを利用して，これまで以下の記事を書きました．

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

具体的には，以下の動画の様に動かせるようになります．

概要

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

M5Stackは，USB Type-Cコネクタの隣に，赤いGroveポート(ポートA)があり，I2Cの端子が来ています．
今回は，ポートAにWiiヌンチャクを接続し，Telloを動かします．
具体的なシステム構成は以下の様になります．
tello_m5_nunchuck.png

Wiiヌンチャクの利用

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

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

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

ハードウェア

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

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

切断したWiiリモコンのケーブル色に関しては，以下が参考になります．
　参考URL：　ヌンチャクをマウスにしてみよう（ヌンチャク型コントローラ・ミーツ・Digispark）

また，M5StackのGroveポートAに関しては，以下が参考になります．
　参考URL：　IoT何をいまさら(34) M5StackのGroveコネクタ

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

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

ピンの役割 Wiiヌンチャク Groveケーブル
5V 赤 赤
GND 白 黒
SCL 黄 黄
SDA 緑 白

となります．
実際に配線すると，こんな風になります．
grove_nunchuk.jpg

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

ソフトウェア

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

　WiiChuck

（前述の　変換基板　は，このライブラリを使う前提で命名されています）

WiiChuckのインストールの方法は，
Arduino IDEのメニューバー[スケッチ]-[ライブラリをインクルード]-[ライブラリを管理...]
で出てくるライブラリマネージャの検索バーで wiichuck と打って検索しましょう．
WiiChuckを見つけたら，[インストール]ボタンをクリックするだけです．
wiichuck.png

使い方は，メニューバー[ファイル]-[スケッチ例]-[WiiChuck]にサンプルがあります．
wiichuck_example.png

スケッチ

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

以下のコードをコピー＆ペーストするか，
ここ　を右クリックして[名前を付けて保存]機能でファイル保存してください．

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();で行っています．
その後，以下のメンバ関数(メソッド)を使って，値を取ります．

ヌンチャククラスのメソッド
  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の範囲で正規化していたので，
ここでも同様に正規化を行います．

センサ値の正規化
  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で割る」べきなのですが，実際のセンサ値を見て，除数を調整しました．
ちなみに，変数名は航空用語の
・ail(aileron：エルロン)
・ele(elevator：エレベータ)
・thr(throttle：スロットル)
・rud(rudder：ラダー)
を使っています．ドローンをやる人にはお馴染みの用語ですね．

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

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

三項演算子で
      // 不感帯設定　(微弱な加速度値で上昇や旋回しないように)
      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ライブラリはヌンチャクだけでなく，クラシックコントローラにも対応しています．
次回は，クラシックコントローラで普通に操縦できるようにしたいと思います．
　
　

7
6
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
7
6