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

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

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

**処理の流れを図にする。 [#p94607c8]
地面が無くなったら後退して反転します。一定時間相手が見つからなければ、反転します。相手を見つけたら全速で直進します。プッシュボタンでプログラムを終了します。
#ref("flow.png",100%)

**初期化 [#la3d8bac]
GCC Developer Liteを起動し、初期化部分を記述します。
#html{{
<pre class="brush:c">
  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);
</pre>
}}

**AX-12+をエンドレスターンモードに切替える [#n9164561]
AX-12+は、通常特定の位置へ移動する位置決めモードになっています。位置決めモードは0~300度の制限があり、車輪のように回転させることはできません。~
AX-12+には車輪と同様に無限に回転させるためのエンドレスターンモードが用意されています。~
#html{{
<pre class="brush:c">
fd_DXSetEndlessTurn (1, true);
fd_DXSetEndlessTurn (2, true);
</pre>
}}
***[[fd_DXSetEndlessTurn>FDIIILIB#b3ee5cbf]] [#hf2069fc]
指定されたIDのアクチュエータのモードを切り替えます。モード切替時にEEPROM(不揮発性メモリ)内の書換えが発生します。EEPROMの書換え回数は数万回ですが、ループ内でモードの切替が発生しているとすぐに限界を迎えることになります。~
''第1引数''は、DynamixelのIDを指定します。
''第2引数''は、モードの指定を行います。trueはエンドレスターンモード、falseは位置決めモードです。

''一度エンドレスターンモードに設定すると、電源を切っても設定は保存されます。位置決めモードを使用する際は、必ずfd_DXSetEndlessTurnで位置決めモードに設定して下さい。''

**AS-S1の赤外線反射センサの値を取得する [#of2fa733]
地面に向けた右側と正面の赤外線反射センサの値を取得します。
#html{{
<pre class="brush:c;highlight:[3,4]">
uint8_t CenterData, RightData;
while (!fd_GetPB ()) {
  DX_ReadByteData (100, 27, &CenterData, 10, NULL);
  DX_ReadByteData (100, 28, &RightData,  10, NULL);
  fd_printf ("center: %3d right: %3d\r", CenterData, RightData);
  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>
}}

**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]
指定したIDのアクチュエータの速度を指定します。~
''第1引数''は、DynamixelのIDです。~
''第2引数''は、位置指定モードの場合は角速度(単位は約0.111rpm)ですが、エンドレスターンモードの場合は最大出力に対する出力の割合になります。

**相手を見つける [#qf172cd6]
正面のセンサで相手を見つけたら、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{{
<pre class="brush:c">
int cnt, i;
short Speed = 512;
uint8_t CenterData;
while (!fd_GetPB ()) {
  cnt = 0;
  while (cnt++ > 100 && Speed != 1023) {
    fd_DXSetSpeed (1, (1<<10) & Speed);
    fd_DXSetSpeed (2, (1<<10) & Speed);
    for (i = 0; i < 500; i++) {
      DX_ReadByteData (100, 27, &CenterData, 10, NULL);
      if (CenterData > 30) break;
    }
  }
  fd_Wait (10);
}
</pre>
}}

**プログラム完成 [#s3a81913]
ではプログラムをまとめてみましょう。
#html{{
<pre class="brush:c">
#include &lt;fd.h&gt;
// 旋回ルーチン
int SearchTurn (int lmax) {
  int result = 0, i;
  uint8_t CenterData;
  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) {
  int cnt = 0;
  short Speed;
  uint8_t CenterData, RightData;

  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, &CenterData, 10, NULL);
    DX_ReadByteData (100, 28, &RightData, 10, NULL);
    Speed = 512;
    if (RightData < 200) {
      fd_DXSetSpeed (1, Speed);
      fd_DXSetSpeed (1, (1<<10) & Speed);
      fd_Wait (2000);
      SearchTurn (2000);
    } else {
      if (CenterData > 30) Speed = 1023;
      fd_DXSetSpeed (1, (1 << 10) & Speed);  // 右回転
      fd_DXSetSpeed (2, Speed);               // 左回転(直進)
    }
    if (cnt++ > 100 && CenterData <= 30) {
      SearchTurn (500);
      cnt = 0;
    }
  }
}
</pre>
}}

• FDIII-HC Starter Kit Guide/Chapter8.3 のバックアップ一覧^[6]
• FDIII-HC Starter Kit Guide/Chapter8.3 のバックアップソース(No. All)
  • 1: 2011-07-12 (火) 19:46:40^[7] yoshida^[8]
  • 2: 2011-07-13 (水) 13:00:57^[9] yoshida^[8]
  • 3: 2011-07-14 (木) 12:26:06 yoshida^[8]
  • 4: 2011-07-14 (木) 13:35:39^[10] yoshida^[8]
  • 現: 2013-04-09 (火) 23:54:11^[11] takaboo^[12]