3: 2011-07-14 (木) 12:26:06 yoshida ソース 4: 2011-07-14 (木) 13:35:39 yoshida ソース
Line 1: Line 1:
 +TITLE:相撲ロボットのプログラミング
**プログラミング [#zc323799] **プログラミング [#zc323799]
前進したり、相手を探したり、土俵際に来たらバックしたりする相撲ロボットのプログラムを作成します。 前進したり、相手を探したり、土俵際に来たらバックしたりする相撲ロボットのプログラムを作成します。
**処理の流れを図にする。 [#p94607c8] **処理の流れを図にする。 [#p94607c8]
-地面が無くなったら後退して反転します。一定時間相手が見つからなければ、反転します。相手を見つけたら全速で直進します。プッシュボタンでプログラムを終了します。+地面が無くなったら後退して旋回します。相手を見つけたら全速で直進します。プッシュボタンでプログラムを終了します。
#ref("flow.png",100%) #ref("flow.png",100%)
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 36: Line 37:
#html{{ #html{{
<pre class="brush:c;highlight:[3,4]"> <pre class="brush:c;highlight:[3,4]">
-uint8_t 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> +
-}} +
-AX-S1の右側は地面を向いているので、RightDataは常に255になります。もし、AX-S1を反対に取り付けていた場合は、以下のように左側のデータを取得して下さい。 +
-#html{{ +
-<pre class="brush:c; highlight:[3,4]"> +
-uint8_t CenterData, LeftData; +
-while (!fd_GetPB ()) { +
-  DX_ReadByteData (100, 26, &LeftData,  10, NULL); +
-  DX_ReadByteData (100, 27, &CenterData, 10, NULL); +
-  fd_printf ("left: %3d center: %3d right: %3d\r", LeftDat, CenterData); +
-  fd_Wait (10); +
-}+
</pre> </pre>
}} }}
 +AX-S1の右側は地面を向いているので、GroundDataは常に255になります。もし、AX-S1を反対に取り付けていた場合は、左側のセンサ値を取得して下さい。
**AX-12+を回転させる [#y123b383] **AX-12+を回転させる [#y123b383]
-エンドレスターンモードでは[[Moving Speed>DXTABLE1#i69bf70b]]に値を設定することでAX-12+は回転を始めます。 +エンドレスターンモードでは位置ではなく、速度を指定することで回転を行います。
-| ビット | 15~11 | 10  || 9~0 | +
-| 値 | 0 | 回転方向 |  1(右方向) | 出力比率 | +
-|^ |^ |^ |  0(左方向) |^ | +
-エンドレスターンモードでは回転速度はなく、最大出力に対する割合の指定となります。+
#html{{ #html{{
<pre class="brush:c"> <pre class="brush:c">
-short Speed = 512; +// アクチュエータ操作 
-fd_DXSetEndlessTurn (1, true); +void Direction (short left, short right) { 
-fd_DXSetSpeed (1, (1&lt;<10) & Speed);  // 出力50%の右回転 +&nbsp; fd_DXSetSpeed (1, -right); 
-fd_DXSetSpeed (1, Speed);           // 出力50%の左回転 +&nbsp; fd_DXSetSpeed (2, left); 
-fd_DXSetSpeed (1, (1&lt;<10) & (Speed<<1));  // 出力100%の右回転 +}
-fd_DXSetSpeed (1, Speed<<1);             // 出力100%の左回転+
</pre> </pre>
}} }}
 +ID1、ID2のAX-12+は逆向きに取り付けていますので、値を反転させる必要があります。個別に記述しても良いのですが、間違えないように関数として1つにまとめてみましょう。~
 +ここではID1が右車輪を想定しているため、値を-(マイナス)しています。AX-12+の取り付けが反対であれば、ID2の値を-(マイナス)指定して下さい。
 +
***[[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{{ 
-<pre class="brush:c; highlight:[4, 6]"> 
-short Speed; 
-uint8_t CenterData; 
-DX_ReadByteData (100, 27, &CenterData, 10, NULL); 
-if (CenterData > 30) { 
-  Speed = 1023; 
-} else { 
-  Speed = 512; 
-} 
-fd_DXSetSpeed (1, (1<<10) & Speed); 
-fd_DXSetSpeed (2, Speed); 
-</pre> 
-}} 
- 
-**相手を探す [#k9fd5152] 
-暫くしても相手が見つからない場合は、旋回して辺りを探してみましょう。 
#html{{ #html{{
<pre class="brush:c"> <pre class="brush:c">
-int cnt, i; +  uint8_t EnemyData, GroundData; 
-short Speed = 512; + 
-uint8_t CenterData; +  fd_DXSetEndlessTurn (1, true); 
-while (!fd_GetPB ()) { +  fd_DXSetEndlessTurn (2, true); 
-  cnt = 0; + 
-  while (cnt++ > 100 && Speed != 1023) { +  while (!fd_GetPB ()) { 
-   fd_DXSetSpeed (1, (1<&lt;10) & Speed); +    DX_ReadByteData (100, 27, &EnemyData, 10, NULL); 
-    fd_DXSetSpeed (2, (1<;&lt;10) & Speed); +    DX_ReadByteData (100, 28, &GroundData, 10, NULL); 
-   for (i = 0; i &lt; 500; i++) { +   fd_printf ("enemy: %3d ground: %3d\r", EnemyData, GroundData); 
-      DX_ReadByteData (100, 27, &CenterData, 10, NULL); +   if (GroundData < 200) {    // 土俵際検知 
-     if (CenterData &gt; 30) break;+&nbsp;     Direction (-512, -512); // 後退 
 +&nbsp;    fd_Wait (2000); 
 +      Direction (-512, 512); // 旋回 
 +&nbsp;     fd_Wait (2000); 
 +   } else { 
 + ;     if (EnemyData &gt; 30) { // 正面敵検知 
 +        Direction (1023, 1023); // 全速前進 
 +     } else { 
 +&nbsp;       Direction (512, 512);   // 50%で前進 
 +     }
   }    }
 +   fd_Wait (10);
  }   }
-  fd_Wait (10); 
-} 
</pre> </pre>
}} }}
Line 123: Line 103:
<pre class="brush:c"> <pre class="brush:c">
#include <fd.h> #include <fd.h>
-// 旋回ルーチン +// アクチュエータ操作 
-int SearchTurn (int lmax) { +void Direction (short left, short right) { 
-  int result = 0, i; +  fd_DXSetSpeed (1, -right); 
-  uint8_t CenterData; +  fd_DXSetSpeed (2, left);
-  short Speed = (1<<10) & 512; +
-  fd_DXSetSpeed (1, Speed); // ID1は右回り +
-  fd_DXSetSpeed (2, Speed);  // ID2も右回り(その場で回転) +
-  for (i = 0; i < lmax; i++) { +
-   DX_ReadByteData (100, 27, &CenterData, 10, NULL); +
-   if (CenterData > 30) {    // 正面に敵発見 +
-     result = 1; +
-     break; +
-   } +
-   fd_Wait (1); +
-  } +
-  return result;+
} }
 +// メイン関数
void main (void) { void main (void) {
-  int cnt = 0; +  uint8_t EnemyData, GroundData;
-  short Speed; +
-  uint8_t CenterData, RightData;+
  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);   fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
Line 153: Line 120:
  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); 
-   Speed = 512; +   fd_printf ("enemy: %3d ground: %3d\r", EnemyData, GroundData); 
-   if (RightData < 200) { +   if (GroundData < 200) {   // 土俵際検知 
-     fd_DXSetSpeed (1, Speed); +     Direction (-512, -512); // 後退 
-     fd_DXSetSpeed (1, (1<;&lt;10) & Speed);+     fd_Wait (2000); 
 +&nbsp;     Direction (-512, 512); // 旋回
     fd_Wait (2000);      fd_Wait (2000);
-     SearchTurn (2000); 
   } else {    } else {
-     if (CenterData > 30) Speed = 1023; +     if (EnemyData > 30) { // 正面敵検知 
-     fd_DXSetSpeed (1, (1 << 10) & Speed); // 右回転 +        Direction (1023, 1023); // 全速前進 
-      fd_DXSetSpeed (2, Speed);               // 左回転(直進) +      } else { 
-    } +        Direction (512, 512);   // 50%で前進 
-   if (cnt++ > 100 && CenterData <= 30) { +     }
-      SearchTurn (500); +
-     cnt = 0;+
   }    }
 +   fd_Wait (10);
  }   }
 +  Direction (0, 0); // 停止
} }
</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