1: 2010-07-06 (火) 18:25:27 sho[6] [7] | 2: 2010-07-06 (火) 20:01:22 sho[6] [8] | ||
---|---|---|---|
Line 1: | Line 1: | ||
+ | 只今編集中です。 | ||
+ | |||
*プログラミング [#tb098c25] | *プログラミング [#tb098c25] | ||
前進したり、相手を探したり、土俵際に来たらバックしたりする車輪ロボットのプログラムを作成します。 | 前進したり、相手を探したり、土俵際に来たらバックしたりする車輪ロボットのプログラムを作成します。 | ||
Line 13: | Line 15: | ||
fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4); | fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4); | ||
} | } | ||
+ | |||
+ | **AX-12+をエンドレスターンモードに切りかえる [#jc4c05f3] | ||
+ | |||
+ | これまでAX-1+に、0から300度の間のある位置へ動けという命令を出しました。これを位置決めモードといいます。位置決めモードでは、AX-12+のホーンを車輪のようにクルクル回転させることはできません。 | ||
+ | |||
+ | 車輪の様に扱うためにはエンドレスターンモードに設定する必要があります。 | ||
+ | fd_Initの後ろに | ||
+ | fd_DXSetEndlessTurn (1, 1); | ||
+ | fd_DXSetEndlessTurn (2, 1); | ||
+ | と入力します。 | ||
+ | fd_DXSetEndlessTurnは、エンドレスターンモードに設定する、又は位置決めモードに戻す関数です。 | ||
+ | |||
+ | ''第1引数''は、DynamixelのIDです。 | ||
+ | |||
+ | ''第2引数''は、エンドレスターンと位置決めモードの切り替えです。1でエンドレスターン、0で位置決めモードに設定します。 | ||
+ | |||
+ | ID1と2のAX-12+をエンドレスターンモードに設定しています。 | ||
+ | |||
+ | **AX-S1の赤外線反射センサの値を取得する [#d3df0f37] | ||
+ | AX-S1の3方向の赤外線反射センサの値を取得します。 | ||
+ | |||
+ | main関数の最初に | ||
+ | uint8_t LeftData, CenterData, RightData; | ||
+ | と入力し、センサ値を入れる変数を宣言します。 | ||
+ | |||
+ | エンドレスターンの設定の後に | ||
+ | while (1) { | ||
+ | fd_DXReadByteData (100, 26, &LeftData); | ||
+ | fd_DXReadByteData (100, 27, &CenterData); | ||
+ | fd_DXReadByteData (100, 28, &RightData); | ||
+ | fd_printf ("%3d %3d %3d\r", LeftData, CenterData, RightData); | ||
+ | } | ||
+ | と入力します。 | ||
+ | 3方向のセンサの値を取得し、それを表示します。 | ||
+ | whileループで囲ってループさせます。 | ||
+ | |||
+ | ここでプログラムを実行して、3方向のセンサの値の変化を見てみましょう。 | ||
+ | |||
+ | RightDataは地面の方向に向いているので常に255となります。 | ||
+ | もしAX-S1を反対に取り付けていたらLeftDataです。以下LeftDataとRightDataを読み替えて下さい。 | ||
+ | CenterDataとLeftDataは手を近づけると値が変わります。 | ||
+ | |||
+ | **前に進む [#dadc53a1] | ||
+ | |||
+ | エンドレスターンモードでは、アドレス32,33に回転速度を設定すると回転を始めます。 | ||
+ | |||
+ | | ビット | ビット16~12 | ビット11 | ビット10~1 | | ||
+ | | Value | 0 | 回転方向 | 回転速度 | | ||
+ | |||
+ | 回転速度のデータは2バイト、つまり16ビットです。上表のように、16ビットの中のビット11が1なら右回転、0なら左回転です。ビット1~10は回転速度です。 | ||
+ | |||
+ | エンドレスターンの設定の後に | ||
+ | fd_DXWriteWordData (1, 32, (1 << 10) + 500); | ||
+ | fd_DXWriteWordData (2, 32, 500); | ||
+ | と入力します。 | ||
+ | |||
+ | ID=1のAX12+のアドレス32に(1 << 10) + 500を書き込みます。 | ||
+ | (1 << 10)は、1を10だけ桁の大きいほうへビットシフトすると言う意味です。 | ||
+ | つまり2進数で書くと、00000000001の1を左へ10だけ移動して、10000000000となります。これを回転速度の値、ここでは500に足すことで、回転方向を決めるビット11を1に設定します。 | ||
+ | |||
+ | AX-12+は左右対称に設置されているので、回転方向は逆にします。 | ||
+ | |||
+ | プログラムを実行して前に進むことを確認しましょう。 | ||
+ | |||
+ | **土俵際にきたらバックする [#r7679d07] | ||
+ | 丸いテーブルを土俵と仮定します。 | ||
+ | 真っ直ぐ進むといづれテーブルから落ちてしまいます。 | ||
+ | そこで地面を向いている赤外線反射センサを使います。 | ||
+ | センサの値がある値より低くなったらバックします。 | ||
+ | |||
+ | while文中のセンサ値を取得した後ろに | ||
+ | if (RightData < 200) { | ||
+ | fd_DXWriteWordData (1, 32, 500); | ||
+ | fd_DXWriteWordData (2, 32, (1 << 10) + 500); | ||
+ | } else { | ||
+ | fd_DXWriteWordData (1, 32, (1 << 10) + 500); | ||
+ | fd_DXWriteWordData (2, 32, 500); | ||
+ | } | ||
+ | と入力します。 | ||
+ | RightDataが200以下なら後退、200より上なら前進します。 | ||
+ | |||
+ | エンドレスターンの設定の後に書いた | ||
+ | fd_DXWriteWordData (1, 32, (1 << 10) + 500); | ||
+ | fd_DXWriteWordData (2, 32, 500); | ||
+ | は削除します。 | ||
+ | |||
+ | プログラムを実行してみましょう。土俵際で前後に行ったりきたりしてしまいます。 | ||
+ | |||
+ | 土俵際に着たら暫くバックするようにします。 | ||
+ | バックの回転速度の設定の後に | ||
+ | fd_Wait (2000); | ||
+ | と入力します。 | ||
+ | プログラムはfd_Wait関数で2秒間待機し、その間バックし続けます。 | ||
+ | |||
+ | バックした後、直進するとまた土俵際に来てしまうので、旋回します。 | ||
+ | fd_Wait関数の後に | ||
+ | fd_DXWriteWordData (1, 32, (1 << 10) + 500); | ||
+ | fd_DXWriteWordData (2, 32, (1 << 10) + 500); | ||
+ | fd_Wait(2000); | ||
+ | と入力します。右を前進方向、左を後進方向に回転させることで旋回します。旋回も2秒間続けます。 | ||
+ | |||
+ | プログラムを実行してみましょう。テーブルから落ちなくなったでしょうか。 | ||
+ | |||
+ | **相手を見つけて突進 [#v91f1a7a] | ||
+ | 正面のセンサで相手を見つけたら、回転速度を上げて突進します。 | ||
+ | else文中の前進回転速度の設定の部分を | ||
+ | if (CenterData > 30) { | ||
+ | fd_DXWriteWordData (1, 32, (1 << 10) + 1023); | ||
+ | fd_DXWriteWordData (2, 32, 1023); | ||
+ | } else { | ||
+ | fd_DXWriteWordData (1, 32, (1 << 10) + 500); | ||
+ | fd_DXWriteWordData (2, 32, 500); | ||
+ | } | ||
+ | に書き換えます。 | ||
+ | 正面のセンサの値が30より大きければ回転速度を1023、それ以外は500となります。 | ||
+ | |||
+ | プログラムを実行してみましょう | ||
+ | |||
+ | **相手を探す [#ha0d0d54] | ||
+ | |||
+ | 暫く直進しても相手が見つからない場合は、旋回して辺りを探します。 | ||
+ | 「暫く」を「while文が100周したら」に置き換えて考えます。 | ||
+ | while文のループ回数をカウントする変数coutnを宣言します。 | ||
+ | main関数の最初に | ||
+ | int count = 0; | ||
+ | と入力します。 | ||
+ | 更にwhile文の最後に | ||
+ | count++; | ||
+ | と入力します。while文が一回処理される毎に変数countが1増加します。 |
(This host) = http://www.besttechnology.co.jp