FDIII-HC Starter Kit Guide Chapter8.3 のソース :: Besttechnology

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

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

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

#ref("sumou_block.png")

**準備 [#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で位置決めモードに設定します。

ここでは、IDが1と2のAX-12+をエンドレスターンモードに設定しています。

''一度エンドレスターンモードに設定すると、電源を切ってもその設定が保存されます。''~
位置決めモードで使用したい場合は、
 fd_DXSetEndlessTurn (1, 0);
を一度実行して位置決めモードに設定し直す必要があります。

**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文で囲ってループさせます。

ここで適当に名前を付けて保存し、コンパイルします。
FDIII-HCに書き込んでプログラムを実行し、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,33に(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);
    } 
と入力します。if文の条件にある''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回ループさせ、正面センサ値の取得を繰り返します。もし正面センサの値が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/N81fTXo0WFU?fs=1&amp;hl=ja_JP&amp;rel=0"></param><param name="allowFullScreen" value="true"></param><param name="allowscriptaccess" value="always"></param><embed src="http://www.youtube.com/v/N81fTXo0WFU?fs=1&amp;hl=ja_JP&amp;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 のバックアップ一覧^[4]
• FDIII-HC Starter Kit Guide Chapter8.3 のバックアップソース(No. All)
  • 1: 2010-07-06 (火) 18:25:27^[5] sho^[6]
  • 2: 2010-07-06 (火) 20:01:22^[7] sho^[6]
  • 3: 2010-07-07 (水) 12:41:54^[8] sho^[6]
  • 4: 2010-07-07 (水) 17:52:25^[9] sho^[6]
  • 5: 2010-10-06 (水) 12:29:37^[10] sho^[6]
  • 現: 2010-10-13 (水) 15:51:42 eid7gud^[11]