2008年7月14日月曜日

Bluetooth接続でLEGO MINDSTORM NXTを動かすRubyスクリプト

ヨドバシでBluetoothドングルを購入し、そのまま大学へ。
買ったのはPCIのBT-MicroEDR2
買った後に、先生から「MINDSTORMのBluetoothは相性キツいよ」と言われたが、問題なく繋がった。

んで、色々参考にしながら書いたのが下のRubyスクリプト。実行環境はUbuntu 8.04。
require 'sdl'
require 'rubygems'
require 'nxt_comm'

# control motors methods
# com is boolean. true/false = go/stop
def motor_right(com, pow)
if com
@nxt.set_output_state(
@motor_right,
pow,
NXTComm::MOTORON,
NXTComm::REGULATION_MODE_IDLE,
0,
NXTComm::MOTOR_RUN_STATE_RUNNING,
0
)
else
@nxt.set_output_state(@motor_right, 0, 0, 0, 0, 0, 0)
end
end

def motor_left(com, pow)
if com
@nxt.set_output_state(
@motor_left,
pow,
NXTComm::MOTORON,
NXTComm::REGULATION_MODE_IDLE,
0,
NXTComm::MOTOR_RUN_STATE_RUNNING,
0
)
else
@nxt.set_output_state(@motor_left, 0, 0, 0, 0, 0, 0)
end
end

#MoveMethods
def goAhead(com)
if com
puts "goAhead"
motor_right(true, -100)
motor_left(true, -100)
else
puts "goAhead stop"
motor_right(false, 0)
motor_left(false, 0)
end
end
def goBack(com)
if com
puts "goBack"
motor_right(true, 100)
motor_left(true, 100)
else
puts "goBack stop"
motor_right(false, 0)
motor_left(false, 0)
end
end
def rotateRight(com)
if com
puts "rotateRight"
motor_right(true, 85)
motor_left(true, -85)
else
puts "rotateRight stop"
motor_right(false, 0)
motor_left(false, 0)
end
end
def rotateLeft(com)
if com
puts "rotateLeft"
motor_right(true, -85)
motor_left(true, 85)
else
puts "rotateLeft stop"
motor_right(false, 0)
motor_left(false, 0)
end
end

#main

begin
@nxt = NXTComm.new("/dev/rfcomm0")
@motor_right = NXTComm::MOTOR_A
@motor_left = NXTComm::MOTOR_B
rescue
puts "Bluetooth Connection failed"
exit
end

puts "Connected to NXT"

SDL.init(SDL::INIT_VIDEO)
screen = SDL.setVideoMode(320, 240, 16, SDL::SWSURFACE)

puts "press ESC to exit"

while true
begin
case event = SDL::Event2.poll
when SDL::Event2::Quit
exit
when SDL::Event2::KeyDown
if event.sym == SDL::Key::UP
goAhead(true)
end
if event.sym == SDL::Key::DOWN
goBack(true)
end
if event.sym == SDL::Key::RIGHT
rotateRight(true)
end
if event.sym == SDL::Key::LEFT
rotateLeft(true)
end
when SDL::Event2::KeyUp
if event.sym == SDL::Key::UP
goAhead(false)
end
if event.sym == SDL::Key::DOWN
goBack(false)
end
if event.sym == SDL::Key::RIGHT
rotateRight(false)
end
if event.sym == SDL::Key::LEFT
rotateLeft(false)
end
if event.sym == SDL::Key::ESCAPE
puts "bye"
exit
end
end
rescue
puts "Error"
exit
end
end

exit
余計な部分もあるが、まだ少し弄くろうと思ってるので分かり易さ重視。

作ったのは戦車のような機体。
モータの回転力を与える部分で、前進させるのに-100と指定しているのは、機体のモータの向きを逆に付けてしまっているため。
超信地旋回では、少し回転力を弱くして(85)回りすぎないようにしている。初め、50に設定してみたが、弱すぎて動かず、ビープ音が(汗


事前にインストールしておくもの
  • libserialport-ruby
    Rubyで使うシリアルポートライブラリ。
  • rubygems
    ruby-nxt-0.8.1.gemをインストールするのに必要。gemはRubyでパッケージ/ライブラリを管理するコマンド。
  • ruby-nxt-0.8.1.gem
    RubyからNXTを使うためのライブラリ。aptではなく、直接DLしてきてgemでインストールする。sudo gem install ruby-nxt-0.8.1.gem。http://rubyforge.org/projects/ruby-nxt/
  • libsdl-ruby
    RubyからSDLを使うためのライブラリ。キー入力を取得するのに使った。
また、実行前にNXTとPCをBluetooth接続して、デバイスファイルを作っておく必要がある。そして、デバイスファイルのパスを、スクリプト中のNXTComm.newの引数として渡してやらなければならない。


デバイスファイルの作成方法は次の手順。

NXTのBluetoothをONにして、PCにBluetoothドングルを差した状態で
$ hcitool scan
を実行してNXTのMACアドレスを表示させる。
/etc/bluetooth/rfcomm.confを編集し、次の様にする。
rfcomm0 {
bind yes;
device MACアドレス;
channel 1;
comment "NXT";
}
Bluetoothデーモンを再起動。
/etc/init.d/bluetooth restart
PC側から、もしくはNXT側からBluetooth接続要求を出して接続をペアリングする。ペアリングは上のサ行の前にやっても良かったかもしれない。
これで、Bluetooth接続したNXTへのデバイスファイルが/dev/rfcomm0というパスで出来ている筈。


設定ファイルをいじらずにデバイスファイルを作成する方法が合ったので追記。
$ hcitool scan
で機器を検索し、MACアドレスを表示させる。

$ sudo rfcomm bind 1 MACアドレス チャンネル番号
で、MACアドレスで指定した機器への/dev/rfcomm1というデバイスファイルが出来る。チャンネル番号は省略して良い。省略すると自動で1が割り当てられる。

$ rfcomm
何もオプションを指定せずにrfcommコマンドを叩くと、現在登録している機器の情報が表示される。
rfcomm1: 01:23:45:67:89:10 channel 1 clean
という感じ。

登録機器を開放する場合は、
$ sudo rfcomm releace rfcomm1



これで、取り合えずは動いたのだが、キーを連打すると実際の動作が追いつかなかったり、左右のモータを同時に動かそうとしても少し時間差が出てしまったり、、、

<追記>
ってか、Rubyの標準ライブラリにCursesあるんじゃん。SDLいらんかった orz

0 件のコメント: