[GPSRCC]ロボットカーの制御について

先日、今回出場する大宮工業高校のはなしを少し聞いたりインタビューの記事を読んだところ、プログラムが難しいかもなぁって思いました。ので、今回はプログラムについて書きます。

こちらは機体がだいたいできあがりました！

【はじめに】
大前提として、使うのは「目的位置」と「現在位置」と「(現在)角度」の状態。今回は現在の状態をGPSと地磁気センサで取得しますが、GPSモジュールから出力されるデータの中に方位のデータがあるのでこれでもいいです。

そして作るのは「目的地にロボットを誘導するプログラム」で、特にルールにしばられるアルゴリズムではありません。巡回するルールでは目的地を順に変更していけばいいです。

【緯度経度の扱い方】
座標空間は緯度経度に定数を掛けて得る直交座標です。y軸正が北、x軸正が東となるようにとります。適当な定数を掛けた値なので単位はmでもmmでもなんでもよく、"単位距離"というのが妥当かもしれません。
ここで注意！世界地図や日本地図を見ると分かりますが方眼が北方向に伸びてますね。距離を正しく表そうとすると緯度には経度の1.2倍くらい掛ける必要があります。（1.2ってのは東京の緯度だとだいたいこれくらいという値です。）

図で見るとこんな感じ。

【目標方位の求め方】
　dx = (目標経度 - 現在経度)×定数
　dy = (目標緯度 - 現在緯度)×定数×1.2
まず目標位置と現在位置の差をこんな感じで定義します。
そしたら角度は逆三角関数arcTanをつかって
　θ = arcTan(dy/dx)
で求められます。（高校生だとここが難しいのかな）
プログラムを書く時はatan関数を使えばいいのですが、atan関数を使える状態であればatan2関数も使えるはずです。atan2はatanと違って値域を考えなくて良いので便利です。

図にして考えるといい。

【アルゴリズム】
上のような準備が済んだら
次の式で左右のモータに与えるゲイン（電圧）を決定します。

　左モータゲイン = 直進ゲイン - 回転ゲイン×(目標方位 - 現在方位)
　右モータゲイン = 直進ゲイン + 回転ゲイン×(目標方位 - 現在方位)

この式の直進ゲインと回転ゲインのパラメータを調整してください。簡単に説明するとこんな感じ。
直進ゲイン：目標にまっすぐ向かう時の速さ
回転ゲイン：目標の角度に向ける速さ

【これができたら】
次は競技に合わせて目標地点に着いたら、目標地点を変更するようにすればいいでしょう。
上で紹介したのは角度しか制御してませんが、距離や他のパラメータを制御して自分流にしてくのがいいと思います。が、GPSロボットカーコンテストについて言えるのはあまり凝った制御は必要ないということです。それよりはハードウェアの調整に力を注いだ方がいいとおもいます。

地磁気が指す北と緯度経度座標の北は違ったり、方位は北から右回り正の値なのに対して数学の角度は東から左回り正のあたいだったり、他にも注意しなきゃいけないことがあるはずです。

[GPSRCC] タイヤとモータドライバのテスト

前々回くらいに設計を載せたタイヤのテストです。
仮組なのでプラプラしてますが接着すれば大丈夫なはず。
ロータリエンコーダは組んでみないと分からんですが分からんですが自作ギアが使えているようでひとまず安堵。



組む時はちゃんとハンダ付けして中空パイプの中を通す予定。
構想段階では、タイヤの中にモータドライバ入れるとか、複数モーターを独立に制御するとか考えてたけど今回は見送り。そこはさくふわ先生の次回作に期待ということで・・。

モータドライバICの基板はこんな感じでまとめてます。
ICはTA8428K(S)という秋月電子で２個300円のものを片側で２つ使用。
本当はTB6643KQなるICを使いたかったのだけど、電源電圧が10V以上だったので断念。
制御する機器によらず使えるようにするために基板はモジュールごとに分割します。

ガンガン作って行かないともう時間がないですね。

[GPSRCC] RaspberryPiでBluetooth QZPOD編

QZPODというGPS/QZSS受信器が届きました。
うれしくてとりあえず動作確認したかったのでRaspberryPiと通信させました。

というわけでそれまでの記録をば。

【Bluetoothドングルの設定から接続】
・使うもの
　　aitendoで安かったBuletoothドングル
　　OSはRaspbianでやりました（wifiの設定が楽！）

・下準備
bluetoothを使うためのプログラムをインストール
pi@raspberrypi ~ $ sudo apt-get install bluetooth bluez-utils blueman
bluemanはGUI用らしいからコマンドラインで使うならなくてもいいって。
だーっとインストールされる２,３分かかったかも。
途中Y/nを２回くらい聞かれた気がする。

・接続はおもったより簡単！
１.ドングル認識されてるか確認
pi@raspberrypi ~ $ lsusb
Bus 001 Device 002: ID 0424:9512 Standard Microsystems Corp. 
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 001 Device 003: ID 0424:ec00 Standard Microsystems Corp. 
Bus 001 Device 004: ID 2019:ab2a PLANEX GW-USNano2 802.11n Wireless Adapter [Realtek RTL8188CUS]
Bus 001 Device 005: ID 0a12:0001 Cambridge Silicon Radio, Ltd Bluetooth Dongle (HCI mode)
４番目はwifiアダプタで、５番目にbluetoothなんちゃらってのが出てきた。これだ。

２.bluetooth受信してるか確認
pi@raspberrypi ~ $ hcitool scan
Scanning ...
00:1B:DC:0F:45:2B QZPOD0040
まぁこんな感じで出てくると。周りにキーボードとか検出される機器があるとここにでてくる。
ちなみに下準備のインストールができてないと、hcitool？そんなコマンドnot foundっておこられる。

３.接続する
さっきのアドレスをつかって接続
pi@raspberrypi ~ $ sudo bluez-simple-agent hci0 00:1B:DC:0F:45:2B
RequestPinCode (/org/bluez/2153/hci0/dev_00_1B_DC_0F_45_2B)
Enter PIN Code: 0000
正しいPINコードを入れる。そんなの聞いてなかったけど初期値でした。
正しいのを入れると
Release
New device (/org/bluez/2153/hci0/dev_00_1B_DC_0F_45_2B)
って出てくる。

ちなみに間違ってると
Creating device failed: org.bluez.Error.AuthenticationFailed: Authentication Failed
っておこられる。

４.確認
pi@raspberrypi ~ $ sudo bluez-test-device list
BC:8D:A6:71:32:21 Bluetooth Keyboard
00:1B:DC:0F:45:2B QZPOD0045
つながっておる。
これで接続は完了。次回以降は勝手に接続されるみたい。

５.補足
接続解除するには・・
pi@raspberrypi ~ $ sudo bluez-test-device remove 00:1B:DC:0F:45:2B
繋ごうとして、もう繋がっていると・・
pi@raspberrypi ~ $ sudo bluez-simple-agent hci0 00:1B:DC:0F:45:2B
Creating device failed: org.bluez.Error.AuthenticationTimeout: Authentication Timeout


【Bluetoothでシリアル通信】
なにしてるかちゃんと理解してないけど、bluetoothの接続先の１つを１つのデバイスとして認識させてるっぽい。
pi@raspberrypi ~ $ rfcomm connect hci0 00:1B:DC:0F:45:2B &
Connected /dev/rfcomm0 to 00:1B:DC:0F:45:2B on channel 1
Press CTRL-C for hangup
とする。

pi@raspberrypi ~ $ ls -la /dev/rfcomm0
crw-rw---T 1 root dialout 216, 0 Sep  8 03:14 /dev/rfcomm0
で/dev/rfcomm0というデバイスとして使えるようになったっぽい。

teraTermみたいなシリアル通信用のプログラムをインストール。
ここではminicomっての使ったけど、何がいいのかは知らない。とりあえず入れてみた。
pi@raspberrypi ~ $ sudo aptitude install minicom

さっき用意したデバイス/dev/rfcomm0に接続するには
pi@raspberrypi ~ $ minicom -D /dev/rfcomm0
を実行。
Welcome to minicom 2.6.1

OPTIONS: I18n 
Compiled on Apr 28 2012, 19:24:31.
Port /dev/rfcomm0

Press CTRL-A Z for help on special keys

と表示され...

$GPGGA,031529.00,,,,,0,00,,,,,,,*44
$GNGSA,A,1,,,,,,,,,,,,,,,*00
$GPGSV,1,1,01,01,,,18,,,,,,,,,,,,*70
$GNRMC,031529.00,V,,,,,,,080914,,,N*6B
$GNZDA,031529.00,08,09,2014,,*72
$GPGGA,031530.00,,,,,0,00,,,,,,,*4C
......
とバンバンきた。

Ctrl-A Zでヘルプって書いてあるので
終了するには、Ctrl-A Z からの Q を打鍵。おわった。

とまぁ、シリアル通信とQZPODの動作の確認はできました。

めでたしめでたし。

次はシリアル通信の結果を用いたプログラムをつくりましょう。