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に戻る


Front page   Diff ReloadPrint View   Page list Search Recent changes   RSS of recent changes (RSS 1.0) RSS of recent changes (RSS 2.0) RSS of recent changes (RSS Atom)
Last-modified: 2013-04-09 (Tue) 23:54:11 (JST) (2728d)