2: 2010-07-06 (火) 20:01:22 sho ソース 現: 2010-10-13 (水) 15:51:42 eid7gud ソース
Line 1: Line 1:
-只今編集中です。 
- 
*プログラミング [#tb098c25] *プログラミング [#tb098c25]
-前進したり、相手を探したり、土俵際に来たらバックしたりする車輪ロボットのプログラムを作成します。+前進したり、相手を探したり、土俵際に来たらバックしたりする相撲ロボットのプログラムを作成します。
**処理の流れを図にする。 [#q9a32975] **処理の流れを図にする。 [#q9a32975]
 +
 +#ref("sumou_block.png")
**準備 [#l8e0d6a8] **準備 [#l8e0d6a8]
Line 11: Line 11:
 #include <fd.h>  #include <fd.h>
 #define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}  #define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}
 + 
 void main (void) {  void main (void) {
   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);
Line 18: Line 18:
**AX-12+をエンドレスターンモードに切りかえる [#jc4c05f3] **AX-12+をエンドレスターンモードに切りかえる [#jc4c05f3]
-これまでAX-1+に、0から300度の間のある位置へ動けという命令を出しました。これを位置決めモードといいます。位置決めモードでは、AX-12+のホーンを車輪のようにクルクル回転させることはできません。+これまでは、AX-1+に対し、0から300度間のある位置へ動けという命令を出してきました。これを位置決めモードといいます。位置決めモードでは、AX-12+のホーンを車輪のようにクルクル回転させることはできません。
-車輪の様に扱うためにはエンドレスターンモードに設定する必要があります。+車輪の様に扱うためには、エンドレスターンモードに設定する必要があります。
fd_Initの後ろに fd_Initの後ろに
 fd_DXSetEndlessTurn (1, 1);  fd_DXSetEndlessTurn (1, 1);
 fd_DXSetEndlessTurn (2, 1);  fd_DXSetEndlessTurn (2, 1);
と入力します。 と入力します。
-fd_DXSetEndlessTurnは、エンドレスターンモードに設定する、又は位置決めモードに戻す関数です。+fd_DXSetEndlessTurnは、エンドレスターンモードに設定、又は位置決めモードに設定する関数です。
''第1引数''は、DynamixelのIDです。 ''第1引数''は、DynamixelのIDです。
-''第2引数''は、エンドレスターンと位置決めモードの切り替えです。1でエンドレスターン、0で位置決めモードに設定します。+''第2引数''は、エンドレスターンと位置決めモードの切り替え値です。1でエンドレスターン、0で位置決めモードに設定します。
-ID1と2のAX-12+をエンドレスターンモードに設定しています。+ここでは、IDが1と2のAX-12+をエンドレスターンモードに設定しています。 
 + 
 +''一度エンドレスターンモードに設定すると、電源を切ってもその設定が保存されます。''~ 
 +位置決めモードで使用したい場合は、 
 + fd_DXSetEndlessTurn (1, 0); 
 +を一度実行して位置決めモードに設定し直す必要があります。
**AX-S1の赤外線反射センサの値を取得する [#d3df0f37] **AX-S1の赤外線反射センサの値を取得する [#d3df0f37]
Line 49: Line 54:
と入力します。 と入力します。
3方向のセンサの値を取得し、それを表示します。 3方向のセンサの値を取得し、それを表示します。
-whileループで囲ってループさせます。+while文で囲ってループさせます。
-ここでプログラムを実行して、3方向のセンサの値の変化を見てみましょう。+ここで適当に名前を付けて保存し、コンパイルします。 
 +FDIII-HCに書き込んでプログラムを実行し、3方向のセンサの値の変化を見てみましょう。
-RightDataは地面の方向に向いているので常に255となります。+RightDataは、地面の方向に向いているので常に255となります。
もしAX-S1を反対に取り付けていたらLeftDataです。以下LeftDataとRightDataを読み替えて下さい。 もしAX-S1を反対に取り付けていたらLeftDataです。以下LeftDataとRightDataを読み替えて下さい。
-CenterDataとLeftDataは手を近づけると値が変わります。+CenterDataとLeftDataは、手を近づけると値が変わります。
**前に進む [#dadc53a1] **前に進む [#dadc53a1]
Line 64: Line 70:
| Value | 0 | 回転方向 | 回転速度 | | Value | 0 | 回転方向 | 回転速度 |
-回転速度のデータは2バイト、つまり16ビットです。上表のように、16ビットの中のビット11が1なら右回転、0なら左回転です。ビット1～10は回転速度です。+回転速度のデータは2バイト、つまり16ビットです。上表のように、16ビットの中のビット11が1なら右回転、0なら左回転になります。ビット1～10は回転速度です。
エンドレスターンの設定の後に エンドレスターンの設定の後に
Line 71: Line 77:
と入力します。 と入力します。
-ID=1のAX12+のアドレス32に(1 << 10) + 500を書き込みます。 +ID=1のAX12+のアドレス32,33に(1 << 10) + 500を書き込みます。 
-(1 << 10)は、1を10だけ桁の大きいほうへビットシフトすると言う意味です。 +(1 << 10)は、1を10だけ桁の大きいほうへ、ビットシフトすると言う意味です。 
-つまり2進数で書くと、00000000001の1を左へ10だけ移動して、10000000000となります。これを回転速度の値、ここでは500に足すことで、回転方向を決めるビット11を1に設定します。+つまり2進数で書くと、00000000001の1を左へ10だけ移動して、10000000000となります。これを回転速度の値(ここでは500)に足すことで、回転方向を決めるビット11を1に設定します。
AX-12+は左右対称に設置されているので、回転方向は逆にします。 AX-12+は左右対称に設置されているので、回転方向は逆にします。
Line 81: Line 87:
**土俵際にきたらバックする [#r7679d07] **土俵際にきたらバックする [#r7679d07]
丸いテーブルを土俵と仮定します。 丸いテーブルを土俵と仮定します。
-真っ直ぐ進むといづれテーブルから落ちてしまいます。+真っ直ぐ進むと、いずれテーブルから落ちてしまいます。
そこで地面を向いている赤外線反射センサを使います。 そこで地面を向いている赤外線反射センサを使います。
-センサの値がある値より低くなったらバックします。+センサがテーブルから外れ、センサ値がある閾値より低くなったらバックします。
while文中のセンサ値を取得した後ろに while文中のセンサ値を取得した後ろに
Line 103: Line 109:
プログラムを実行してみましょう。土俵際で前後に行ったりきたりしてしまいます。 プログラムを実行してみましょう。土俵際で前後に行ったりきたりしてしまいます。
-土俵際に着たら暫くバックするようにします。 +土俵際に着たら暫くの間、バックし続けるようにします。 
-バックの回転速度の設定の後に+バックするように回転速度を設定した箇所の後に
     fd_Wait (2000);      fd_Wait (2000);
と入力します。 と入力します。
プログラムはfd_Wait関数で2秒間待機し、その間バックし続けます。 プログラムはfd_Wait関数で2秒間待機し、その間バックし続けます。
-バックした後、直進するとまた土俵際に来てしまうので、旋回します。+バックした後、直進するとまた土俵際に来てしまうので旋回します。
fd_Wait関数の後に fd_Wait関数の後に
     fd_DXWriteWordData (1, 32, (1 << 10) + 500);      fd_DXWriteWordData (1, 32, (1 << 10) + 500);
Line 129: Line 135:
     }      }
に書き換えます。 に書き換えます。
-正面のセンサの値が30より大きければ回転速度を1023、それ以外は500となります。+正面のセンサの値が30より大きければ回転速度を1023、それ以外は500とします。
-プログラムを実行してみましょう+プログラムを実行してみましょう。
**相手を探す [#ha0d0d54] **相手を探す [#ha0d0d54]
-暫く直進しても相手が見つからない場合は、旋回して辺りを探します。 +暫く直進しても相手が見つからない場合は、旋回して辺りを探してみましょう。 
-「暫く」を「while文が100周したら」に置き換えて考えます。 +「暫く直進する」を「while文が100周するまで触診する」に置き換えて考えます。 
-while文のループ回数をカウントする変数coutnを宣言します。+while文のループ回数をカウントする変数countを宣言します。
main関数の最初に main関数の最初に
   int count = 0;    int count = 0;
と入力します。 と入力します。
-更にwhile文の最後に+更にwhile文内の最後に
   count++;    count++;
と入力します。while文が一回処理される毎に変数countが1増加します。 と入力します。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&hl=ja_JP&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&hl=ja_JP&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]]に戻る
トップ   差分 リロード印刷に適した表示   全ページ一覧 単語検索 最新ページの一覧   最新ページのRSS 1.0 最新ページのRSS 2.0 最新ページのRSS Atom