FDIII-HC Starter Kit Guide Chapter8.3 のバックアップソース(No.3) :: Besttechnology

• 差分^[1] を表示
• 現在との差分^[2] を表示
• FDIII-HC Starter Kit Guide Chapter8.3^[3] へ行く。

*プログラミング [#tb098c25]
前進したり、相手を探したり、土俵際に来たらバックしたりする車輪ロボットのプログラムを作成します。

**処理の流れを図にする。 [#q9a32975]

**準備 [#l8e0d6a8]
GCC Developer Liteを起動します。以下基本的な部分を入力します。

 #include <fd.h>
 #define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}

 void main (void) {
   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文のループ回数をカウントする変数countを宣言します。
main関数の最初に
   int count = 0;
と入力します。
更にwhile文の最後に
   count++;
と入力します。while文が一回処理される毎に変数countが1増加します。

while文が100回ループしたら旋回して相手を探すことにします。
while文の中に
    if (count > 100 && CenterData < 31) {
      fd_DXWriteWordData (1, 32, (1 << 10) + 500);
      fd_DXWriteWordData (2, 32, (1 << 10) + 500);
    }
と入力します。A&&BはAかつBを表します。countが100より大きいかつCenterDataが31より小さい場合、if文の中を実行します。
CenterData < 31は、相手を見つけて突進している最中に旋回してしまわないようにするためです。

暫く旋回しながら相手を探します。

この時、fd_Wait関数で暫く旋回させても意味がありません。何故ならfd_Wait関数で待機している間は、プログラムは他の処理を行えないからです。

for文で適当な回数ループさせながら、正面センサの値をチェックします。
旋回の指令の後に
      for (i = 0; i < 500; i++) {
        fd_DXReadByteData (100, 27, &CenterData);
        if (CenterData > 30) i = 500;
      }
と入力します。for文を500回ループさせ、for文内では正面センサ値の取得を繰り返します。もし正面センサの値が30より大きければ、iを500にして即座にfor文を抜けます。

**プログラム完成 [#ba482b2d]

最後にwhile文の最後にKEY_QUIT;を入れて完成です。
 #include <fd.h>
 #define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}

 void main (void) {
   uint8_t LeftData, CenterData, RightData;
   int count = 0, i;

   fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4);
   fd_DXSetEndlessTurn (1, 1);
   fd_DXSetEndlessTurn (2, 1);

   while (1) {
     fd_DXReadByteData (100, 26, &LeftData);
     fd_DXReadByteData (100, 27, &CenterData);
     fd_DXReadByteData (100, 28, &RightData);
     fd_printf ("%3d %3d %3d %6d\r", LeftData, CenterData, RightData,  count);
     if (RightData < 200) {
       fd_DXWriteWordData (1, 32, 500);
       fd_DXWriteWordData (2, 32, (1 << 10) + 500);
       fd_Wait(2000);
       fd_DXWriteWordData (1, 32, (1 << 10) + 500);
       fd_DXWriteWordData (2, 32, (1 << 10) + 500);
       fd_Wait(2000);
     } 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);
       }
     }
     if (count > 100 && CenterData < 31) {
       fd_DXWriteWordData (1, 32, (1 << 10) + 500);
       fd_DXWriteWordData (2, 32, (1 << 10) + 500);
       for (i = 0; i < 500; i++) {
        fd_DXReadByteData (100, 27, &CenterData);
        if (CenterData > 30) i = 500;
       }
       count = 0;
     }
     count++;
     KEY_QUIT;
   }
 }

#html(<object width="480" height="385"><param name="movie" value="http://www.youtube.com/v/LOJX6F4pJOk&amp;hl=ja_JP&amp;fs=1?rel=0"></param><param name="allowFullScreen" value="true"></param><param name="allowscriptaccess" value="always"></param><embed src="http://www.youtube.com/v/LOJX6F4pJOk&amp;hl=ja_JP&amp;fs=1?rel=0" type="application/x-shockwave-flash" allowscriptaccess="always" allowfullscreen="true" width="480" height="385"></embed></object>)

[[次のチャプターへ>FDIII-HC Starter Kit Guide Chapter8.4]]~

[[FDIII-HC Starter Kit Guide]]に戻る

• FDIII-HC Starter Kit Guide Chapter8.3 のバックアップ一覧^[6]
• FDIII-HC Starter Kit Guide Chapter8.3 のバックアップソース(No. All)
  • 1: 2010-07-06 (火) 18:25:27^[7] sho^[8]
  • 2: 2010-07-06 (火) 20:01:22^[9] sho^[8]
  • 3: 2010-07-07 (水) 12:41:54 sho^[8]
  • 4: 2010-07-07 (水) 17:52:25^[10] sho^[8]
  • 5: 2010-10-06 (水) 12:29:37^[11] sho^[8]
  • 現: 2010-10-13 (水) 15:51:42^[12] eid7gud^[13]