ArduinoとKinectでラジコンヘリを飛ばしてみた

KinectとProcessing、Arduinoを使って、ラジコンヘリを飛ばすしかけを作ってみました。

動画:

赤外線ラジコンヘリ

飛ばすのはFS-IRH100 (F103)という室内用のラジコンヘリ。
赤外線方式のコントローラが付いてきます。

2000円台という安さながら、慣れるとかなり安定して飛ばすことができ、大人から子供まで十分楽しめます。ちょっとぶつかったくらいなら壊れることもなく、交換用のブレードも付いてくるので安心です。

4ch赤外線コントロール ラジコンヘリコプター FS-IRH100 (メタリックブルー)

バッテリー交換可能になった改良型も出ており、スペアバッテリー付きでも売られていました。飛行時間は5分弱といったところで、頻繁に充電することになるので、こちらの方が良いかもしれません。( 同型を2台買うと言う手もあります。:P )

4ch赤外線コントロール ラジコンヘリコプター MA-RC079 (メタリックブルー) (スペアバッテリー付)

Arduinoからラジコンヘリに赤外線を発信

赤外線コマンドの送信には、Arduinoと赤外線LEDがあればOKです。
以下のスケッチを書き込んでおきます。

https://gist.github.com/4243846#file_rc_heli.ino

3pinとGNDに赤外線LEDのアノード/カソードをそれぞれ接続します。
(38KHzの信号を出すためにPWM機能を使っているため、pinは変更できません。)

シリアルポート・モニタを起動し、通信速度を 115200 bps にすると、up: などと表示されるので、16進数2桁で上下移動量／左右旋回量／左右微調整／前後移動量と、左右スライド(l/s/nのいずれか) を指定すると、赤外線信号が発信されます*1。

名称	意味	値の範囲
up	上下移動	00(停止) 〜 64
lr	左右旋回	(左旋回)0f〜01, 10(旋回なし), 11〜1f(右旋回)
trim	左右調整	同上
fb	前後移動	07〜01(後退), 08(停止), 09〜0f(前進)
sh	左右スライド	l (左スライド), n (停止), r (右スライド)

左右は0fが最も左に、前後は07が最も後退になります。ちょっと変な感じですが、元々のリモコンコードの仕様に従ってこうなっています。*2
また、trimは旋回なしを指定しても回転してしまう時に補正用として使います。

2-3秒間信号を受信しない場合、ヘリは自動的に停止するようです。
なお、この信号はリモコンのband切り替えスイッチを B にしたときのものを使っています。band Aの信号を受信したヘリはそれ以降A以外のコマンドを無視してしまうので、その場合には電源の再投入が必要です。

KinectとProcessingのセットアップ

ProcessingでKinectを使うためには、SimpleOpenNIと関連ソフトウェアのセットアップが必要です。

Macの場合のインストール手順はこちらを参考にさせていただきました。
Mac+Kinect+Processingで遊んでみる - karaage. [からあげ]

Windowsの場合は、こちらが参考になりそうです。
http://cafe.eyln.com/cgi-bin/wiki/wiki.cgi?page=Processing%A4%C7%CD%B7%A4%DC%A4%A6#p20

Exampleから Libraries → SimpleOpenNI → Userを開き、以下のコードを追加します。

int LR_TRIM = 16;
int last = millis();
void sendCommand(int userId)
{
  if (millis() - last < 200) return;
  last = millis();
  
  PVector left = new PVector(), right = new PVector();
  PVector hip_l = new PVector(), hip_r = new PVector(), neck = new PVector();
  PVector shoulder_l = new PVector(), torso = new PVector();
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HAND, left);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_HAND, right);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_SHOULDER, shoulder_l);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_TORSO, torso);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HIP, hip_l);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_HIP, hip_r);
  context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_NECK,  neck);

  if (neck.y < 0)
    return;

  double ry = (right.y - hip_r.y)  / (neck.y - hip_r.y) * 32;
  double ly = (left.y - hip_l.y) / (neck.y - hip_l.y) * 32;
  double lx = (left.x - shoulder_l.x) / (shoulder_l.x - torso.x) * 100;
  double lz = (left.z - shoulder_l.z) / (shoulder_l.x - torso.x) * 100;
  
  int up = (int)(ry < 0 ? 0 : ry < 50 ? ry+32 : ry < 59 ? 82+(ry-50)*2 : 100);
  int lr = 16;
  int trim = LR_TRIM;
  int fb = 8;
  char slide = 'n';

  if (up > 0) {
    if (ly > 15 && ly < 32) {
      if (lx >  300) lr = 24;
      if (lx < -200) lr = 12;
      if (lz >  250) fb = 14;
      if (lz < -150) fb =  6;
    }
  }  
  serial.write(hex(up,2) + hex(lr,2) + hex(trim,2) + hex(fb,2) + slide + "\n");
  while(serial.available() > 0) {
    print(serial.readString());
  }
}

またsetupとdraw関数に、以下を付け加えます。

void setup()
{
  serial = new Serial(this,"/dev/tty.usbserial-A7006UFY", 115200);   // ←追加。シリアルポート名は自分のArduinoに合わせること
  context = new SimpleOpenNI(this);
...
}
void draw() {
...
  // draw the skeleton if it's available
  int[] userList = context.getUsers();
  for(int i=0;i<userList.length;i++)
  {
    if(context.isTrackingSkeleton(userList[i])) {
      drawSkeleton(userList[i]);
      sendCommand(userList[i]);         // ←追加
    }
  }
...
}

以上を追加した全体のコードはこちらにあります。
https://gist.github.com/4243846#file_rc_heli.pde

先ほどのスケッチを書き込んだArduinoとKinectを接続してこのスケッチを起動すると、深度センサで取得した画像が表示されます。
その中に全身が入るように立って、左手を上げていくと、ヘリを離陸させることができます。
左手の高さを調整して、うまくホバリングさせたら、こんどは右手を前後左右に伸ばすと、前後移動や左右旋回させることができます。*3
しゃがんで首の位置が画面の下半分になると、リモコン信号の送出が止まります。おかしなことになったらまずは左手を下げ、それでもダメならしゃがみましょう。（ヘリを移動させる時もしゃがんでいきましょう）^^;

*1:途中数値以外の文字(改行や空白など)が指定されると、up:に戻ります。

*2:このスケッチは、付属のプロポの信号をリモコン用赤外線センサで受信し、制御コードを適当にリバースエンジニアリングして作ったものです。結構複雑なチェックサムが付与されているようで、いろいろな値の組み合わせで試してはいますが、特定の値では受理されないなんて可能性も残っていると思います。

*3:今のところ、旋回／移動速度の調整はできません。また、左右スライドにも対応していません…せっかくの4chヘリなのに。