Page Top

処理の流れを図にする。 anchor.png

地面が無くなったら後退して旋回します。相手を見つけたら全速で直進します。プッシュボタンでプログラムを終了します。

flow.png
Page Top

初期化 anchor.png

GCC Developer Liteを起動し、初期化部分を記述します。

  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);
Page Top

AX-12Aをエンドレスターンモードに切替える anchor.png

AX-12Aは、通常特定の位置へ移動する位置決めモードになっています。位置決めモードは0~300度の制限があり、車輪のように回転させることはできません。
AX-12Aには車輪と同様に無限に回転させるためのエンドレスターンモードが用意されています。

fd_DXSetEndlessTurn (1, true);
fd_DXSetEndlessTurn (2, true);
Page Top

fd_DXSetEndlessTurn anchor.png

指定されたIDのアクチュエータのモードを切り替えます。モード切替時にEEPROM(不揮発性メモリ)内の書換えが発生します。EEPROMの書換え回数は数万回ですが、ループ内でモードの切替が発生しているとすぐに限界を迎えることになります。
第1引数は、DynamixelのIDを指定します。
第2引数は、モードの指定を行います。trueはエンドレスターンモード、falseは位置決めモードです。

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

Page Top

AS-S1の赤外線反射センサの値を取得する anchor.png

地面に向けた右側と正面の赤外線反射センサの値を取得します。

  uint8_t EnemyData, GroundData;
  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);
    fd_Wait (10);
  }

AX-S1の右側は地面を向いているので、GroundDataは常に255になります。もし、AX-S1を反対に取り付けていた場合は、左側のセンサ値を取得して下さい。

Page Top

AX-12Aを回転させる anchor.png

エンドレスターンモードでは位置ではなく、速度を指定することで回転を行います。

// アクチュエータ操作
void Direction (short left, short right) {
  fd_DXSetSpeed (1, -right); 
  fd_DXSetSpeed (2, left);
}

ID1、ID2のAX-12Aは逆向きに取り付けていますので、値を反転させる必要があります。個別に記述しても良いのですが、間違えないように関数として1つにまとめてみましょう。
ここではID1が右車輪を想定しているため、値を-(マイナス)しています。AX-12Aの取り付けが反対であれば、ID2の値を-(マイナス)指定して下さい。

Page Top

fd_DXSetSpeed anchor.png

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

Page Top

センサ値に応じて方向を変える anchor.png

右側のセンサで土俵際を検知したら2秒間後退し、左に旋回します。
正面のセンサで相手を見つけたら、100%の出力で直進します。見つからない場合は出力を50%に落として直進します。

  uint8_t EnemyData, GroundData;

  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);
  }
Page Top

プログラム完成 anchor.png

ではプログラムをまとめてみましょう。

#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); // 停止
}

作成したDirection関数でいろいろな動きが可能になります。

  Direction (100, 512);   // 左へ曲がる
  Direction (512, -512);  // その場で右に曲がる(右旋回)
  Direction (-512, -100); // 後退しながら右に曲がる

次のチャプターへ

FDIII-HC Starter Kit Guideに戻る


トップ   差分 リロード印刷に適した表示   全ページ一覧 単語検索 最新ページの一覧   最新ページのRSS 1.0 最新ページのRSS 2.0 最新ページのRSS Atom
最終更新: 2013-04-09 (火) 23:54:11 (JST) (4005d)