2: 2011-07-13 (水) 13:00:57 yoshida ソース 現: 2013-04-09 (火) 23:54:11 takaboo ソース
Line 1: Line 1:
 +TITLE:相撲ロボットのプログラミング
**プログラミング [#zc323799] **プログラミング [#zc323799]
前進したり、相手を探したり、土俵際に来たらバックしたりする相撲ロボットのプログラムを作成します。 前進したり、相手を探したり、土俵際に来たらバックしたりする相撲ロボットのプログラムを作成します。
**処理の流れを図にする。 [#p94607c8] **処理の流れを図にする。 [#p94607c8]
-地面が無くなったら後退して反転します。一定時間相手が見つからなければ、反転します。相手を見つけたら全速で直進します。プッシュボタンでプログラムを終了します。+地面が無くなったら後退して旋回します。相手を見つけたら全速で直進します。プッシュボタンでプログラムを終了します。
#ref("flow.png",100%) #ref("flow.png",100%)
Line 16: Line 17:
}} }}
-**AX-12+をエンドレスターンモードに切替える [#n9164561] +**AX-12Aをエンドレスターンモードに切替える [#n9164561] 
-AX-12+は、通常特定の位置へ移動する位置決めモードになっています。位置決めモードは0~300度の制限があり、車輪のように回転させることはできません。~ +AX-12Aは、通常特定の位置へ移動する位置決めモードになっています。位置決めモードは0~300度の制限があり、車輪のように回転させることはできません。~ 
-AX-12+には車輪と同様に無限に回転させるためのエンドレスターンモードが用意されています。~+AX-12Aには車輪と同様に無限に回転させるためのエンドレスターンモードが用意されています。~
#html{{ #html{{
<pre class="brush:c"> <pre class="brush:c">
Line 27: Line 28:
***[[fd_DXSetEndlessTurn>FDIIILIB#b3ee5c​bf]] [#hf2069fc] ***[[fd_DXSetEndlessTurn>FDIIILIB#b3ee5c​bf]] [#hf2069fc]
指定されたIDのアクチュエータのモードを切り替えます。モード切替時にEEPROM(不揮発性メモリ)内の書換えが発生します。EEPROMの書換え回数は数万回ですが、ループ内でモードの切替が発生しているとすぐに限界を迎えることになります。~ 指定されたIDのアクチュエータのモードを切り替えます。モード切替時にEEPROM(不揮発性メモリ)内の書換えが発生します。EEPROMの書換え回数は数万回ですが、ループ内でモードの切替が発生しているとすぐに限界を迎えることになります。~
-''第1引数''は、DynamixelのIDを指定します。 +''第1引数''は、DynamixelのIDを指定します。~ 
-''第2引数''は、モードの指定を行います。trueはエンドレスターンモード、falseは位置決めモードです。+''第2引数''は、モードの指定を行います。trueはエンドレスターンモード、falseは位置決めモードです。~
''一度エンドレスターンモードに設定すると、電源を切っても設定は保存されます。位置決めモードを使用する際は、必ずfd_DXSetEndlessTurnで位置決めモードに設定して下さい。'' ''一度エンドレスターンモードに設定すると、電源を切っても設定は保存されます。位置決めモードを使用する際は、必ずfd_DXSetEndlessTurnで位置決めモードに設定して下さい。''
Line 35: Line 36:
地面に向けた右側と正面の赤外線反射センサの値を取得します。 地面に向けた右側と正面の赤外線反射センサの値を取得します。
#html{{ #html{{
-<pre class="brush:c"> +<pre class="brush:c;highlight:[3,4]"> 
-int CenterData, RightData; +  uint8_t EnemyData, GroundData; 
-while (!fd_GetPB ()) { +  while (!fd_GetPB ()) { 
-  DX_ReadByteData (100, 27, &CenterData, 10, NULL); +    DX_ReadByteData (100, 27, &EnemyData, 10, NULL); // 正面センサ値取得 
-  DX_ReadByteData (100, 28, &RightData, 10, NULL); +    DX_ReadByteData (100, 28, &GroundData, 10, NULL); // 右側センサ値取得 
-  fd_printf ("center: %3d right: %3d\r", CenterData, RightData); +    fd_printf ("enemy: %3d ground: %3d\r", EnemyData, GroundData); 
-  fd_Wait (10); +    fd_Wait (10); 
-}+  }
</pre> </pre>
}} }}
-AX-S1の右側は地面を向いているので、RightDataは常に255になります。もし、AX-S1を反対に取り付けていた場合は、以下のように左側のデータを取得して下さい。+AX-S1の右側は地面を向いているので、GroundDataは常に255になります。もし、AX-S1を反対に取り付けていた場合は、左側のセンサ値を取得して下さい。 
 + 
 +**AX-12Aを回転させる [#y123b383] 
 +エンドレスターンモードでは位置ではなく、速度を指定することで回転を行います。
#html{{ #html{{
<pre class="brush:c"> <pre class="brush:c">
-int CenterData, LeftData; +// アクチュエータ操作 
-while (!fd_GetPB ()) { +void Direction (short left, short right) { 
-  DX_ReadByteData (100, 26, &LeftData,  10, NULL); +  fd_DXSetSpeed (1, -right); 
-  DX_ReadByteData (100, 27, &CenterData, 10, NULL); +  fd_DXSetSpeed (2, left);
-  fd_printf ("left: %3d center: %3d right: %3d\r", LeftDat, CenterData); +
-  fd_Wait (10);+
} }
</pre> </pre>
}} }}
 +ID1、ID2のAX-12Aは逆向きに取り付けていますので、値を反転させる必要があります。個別に記述しても良いのですが、間違えないように関数として1つにまとめてみましょう。~
 +ここではID1が右車輪を想定しているため、値を-(マイナス)しています。AX-12Aの取り付けが反対であれば、ID2の値を-(マイナス)指定して下さい。
-**AX-12+を回転させる [#y123b383] 
-エンドレスターンモードでは[[Moving Speed>DXTABLE1#i69bf70b]]に値を設定することでAX-12+は回転を始めます。 
-| ビット | 15~11 | 10  || 9~0 | 
-| 値 | 0 | 回転方向 |  1(右方向) | 出力比率 | 
-|^ |^ |^ |  0(左方向) |^ | 
-エンドレスターンモードでは回転速度はなく、最大出力に対する割合の指定となります。 
-#html{{ 
-<pre class="brush:c"> 
-short Speed = 512; 
-fd_DXSetEndlessTurn (1, true); 
-fd_DXSetSpeed (1, (1<<10) & Speed);  // 出力50%の右回転 
-fd_DXSetSpeed (1, Speed);            // 出力50%の左回転 
-fd_DXSetSpeed (1, (1<<10) & (Speed<<1));  // 出力100%の右回転 
-fd_DXSetSpeed (1, Speed<<1);              // 出力100%の左回転 
-</pre> 
-}} 
***[[fd_DXSetSpeed>FDIIILIB#b509ce72]] [#j20842a3] ***[[fd_DXSetSpeed>FDIIILIB#b509ce72]] [#j20842a3]
指定したIDのアクチュエータの速度を指定します。~ 指定したIDのアクチュエータの速度を指定します。~
Line 79: Line 67:
''第2引数''は、位置指定モードの場合は角速度(単位は約0.111rpm)ですが、エンドレスターンモードの場合は最大出力に対する出力の割合になります。 ''第2引数''は、位置指定モードの場合は角速度(単位は約0.111rpm)ですが、エンドレスターンモードの場合は最大出力に対する出力の割合になります。
-**相手を見つける [#qf172cd6]+**センサ値に応じて方向を変える [#qf172cd6] 
 +右側のセンサで土俵際を検知したら2秒間後退し、左に旋回します。~
正面のセンサで相手を見つけたら、100%の出力で直進します。見つからない場合は出力を50%に落として直進します。 正面のセンサで相手を見つけたら、100%の出力で直進します。見つからない場合は出力を50%に落として直進します。
#html{{ #html{{
<pre class="brush:c"> <pre class="brush:c">
-short CenterData; +  uint8_t EnemyData, GroundData; 
-if (CenterData > 30) { + 
-  Speed = 1023; +  fd_DXSetEndlessTurn (1, true); 
-} else { +  fd_DXSetEndlessTurn (2, true); 
-  Speed = 512;+ 
 +  while (!fd_GetPB ()) { 
 +   DX_ReadByteData (100, 27, &EnemyData, 10, NULL); 
 +   DX_ReadByteData (100, 28, &GroundData, 10, NULL); 
 +   fd_printf ("enemy: %3d ground: %3d\r", EnemyData, GroundData); 
 +   if (GroundData < 200) {    // 土俵際検知 
 +     Direction (-512, -512); // 後退 
 +     fd_Wait (2000); 
 +     Direction (-512, 512);  // 旋回 
 +     fd_Wait (2000); 
 +   } else { 
 +     if (EnemyData > 30) { // 正面敵検知 
 +        Direction (1023, 1023); // 全速前進 
 +     } else { 
 +        Direction (512, 512);  // 50%で前進 
 +     } 
 +   } 
 +   fd_Wait (10); 
 +  } 
 +</pre> 
 +}} 
 + 
 +**プログラム完成 [#s3a81913] 
 +ではプログラムをまとめてみましょう。 
 +#html{{ 
 +<pre class="brush:c"> 
 +#include <fd.h> 
 +// アクチュエータ操作 
 +void Direction (short left, short right) { 
 +  fd_DXSetSpeed (1, -right); 
 +  fd_DXSetSpeed (2, left); 
 +} 
 +// メイン関数 
 +void main (void) { 
 +  uint8_t EnemyData, GroundData; 
 + 
 +  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); 
 +  fd_SetUVThreshold (7.4); 
 +  DX_ChangeBaudrate (1000000); 
 + 
 +  fd_DXSetEndlessTurn (1, true); 
 +  fd_DXSetEndlessTurn (2, true); 
 + 
 +  while (!fd_GetPB ()) { 
 +   DX_ReadByteData (100, 27, &EnemyData, 10, NULL); 
 +   DX_ReadByteData (100, 28, &GroundData, 10, NULL); 
 +   fd_printf ("enemy: %3d ground: %3d\r", EnemyData, GroundData); 
 +   if (GroundData < 200) {    // 土俵際検知 
 +     Direction (-512, -512); // 後退 
 +     fd_Wait (2000); 
 +     Direction (-512, 512);  // 旋回 
 +     fd_Wait (2000); 
 +   } else { 
 +     if (EnemyData > 30) {  // 正面敵検知 
 +       Direction (1023, 1023); // 全速前進 
 +     } else { 
 +       Direction (512, 512);  // 50%で前進 
 +     } 
 +   } 
 +   fd_Wait (10); 
 +  } 
 +  Direction (0, 0); // 停止
} }
-fd_DXSetSpeed (1, (1<<10) & Speed); 
-fd_DXSetSpeed (2, Speed); 
</pre> </pre>
}} }}
 +作成したDirection関数でいろいろな動きが可能になります。
 +#html{{
 +<pre class="brush:c">
 +  Direction (100, 512);  // 左へ曲がる
 +  Direction (512, -512);  // その場で右に曲がる(右旋回)
 +  Direction (-512, -100); // 後退しながら右に曲がる
 +</pre>
 +}}
 +
 +[[次のチャプターへ>FDIIICHAPTER8.4]]
 +
 +[[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]に戻る
トップ   差分 リロード印刷に適した表示   全ページ一覧 単語検索 最新ページの一覧   最新ページのRSS 1.0 最新ページのRSS 2.0 最新ページのRSS Atom