ページ内コンテンツ

プログラミング anchor.png

AutomaticDoor_1.png

「AX-S1の赤外線反射センサに人が反応したら、AX-12Aでバーを開く」というプログラムを作成します

Chapter2で「AX-S1の赤外線反射センサに泥棒が反応したら、AX-S1のブザーを鳴らして脅かす」というプログラムを作りました。違いはAX-S1のブザーを鳴らすか、AX-12Aを動かすかだけです。

Page Top

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

Chapter2と同様に処理の流れを図にしましょう。

flow_1.png

Chapter2との違いはブザーを鳴らす代わりに、センサ値が閾値以上ならバーを開け、閾値以下ならバーを閉じるという所です。人が来てバーが開いた後、直ぐ閉じてしまうと困るので少しの間待機させます。

Page Top

準備 anchor.png

GCC Developer Liteを起動して、Chapter2で作成した警報器のプログラムを開いて下さい。

メニューの「ファイル」→「開く」→ ファイル(AlarmUnit.c)を選択し、「開く」をクリックします。

ファイルを別の名前で保存しましょう。

メニューの「ファイル」→「名前を付けて保存」→ファイル名に「AutomaticBar .c」と入力して「保存」します。

gcc_save.png
Page Top

ブザーを鳴らすからバーを開けるに変更 anchor.png

DX_WriteByteDataを削除し、fd_DXSetPositionに変更します。

fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
fd_SetUVThreshold (7.4);
DX_ChangeBaudrate (1000000);
 
while (true) {
  DX_ReadByteData (100, 27, &ReadData, 10, NULL);
  fd_printf ("%3d\r", ReadData);
  if (ReadData > 100) {
    DX_WriteByteData (100, 40, 10, 10, NULL);
    DX_WriteByteData (100, 41, 10, 10, NULL);
  }
  fd_Wait (10);
}
down_arrow.png
fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
fd_SetUVThreshold (7.4);
DX_ChangeBaudrate (1000000);

while (!fd_rx_buff () ) {
  DX_ReadByteData (100, 27, &ReadData, 10, NULL);
  fd_printf ("%3d\r", ReadData);
  if (ReadData > 100) {
    fd_DXSetPosition (1, 300);
  }
  fd_Wait (10);
}

fd_DXSetPositionは指定されたアクチュエータの位置を変更する関数です。

第1引数は、DynamixelのIDです。AX-12AのIDは1です。
第2引数は、指令位置(ゴールポジション)です。

AX12_position.png

AX-12Aのホーンは図のように0°から300°まで回転します。ゴールポジションの値は0°なら0、中心の150°なら512、300°なら1023になります。

  :idea:AX-12Aのホーンの角度によっては、フレームとAX-12A、又はフレームと自作ケースが干渉します。干渉が起こるようなゴールポジションを指定した場合、機器の破損の原因となります。必ず設定するゴールポジションが干渉が起こらない範囲内にあることを十分確認して下さい。

Page Top

バーを閉じる anchor.png

バーを閉じるにはバーを開く時の反対の条件が必要になります。
バーを開く時の条件がif (ReadData > 100)となっていますので、反対の条件はelseとなります。

#include <fd.h>
void main (void) {
  uint8_t ReadData;

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

  fd_DXSetTorqueLimit (1, 154);

  while (!fd_rx_buff ()) {
    DX_ReadByteData (100, 27, &ReadData, 10, NULL);
    fd_printf ("%3d\r", ReadData);
    if (ReadData > 100) {
      fd_DXSetPosition (1, 300);
    } else {
      fd_DXSetPosition (1, 512);
    }
    fd_Wait (10);
  }
}

バーを開く/閉じるはゴールポジションを変更するだけで、同じfd_DXSetPositionを使用します。

Page Top

待ち時間を増やす anchor.png

自動ドアは一旦開くとしばらく閉まりません。そのため、10msの停止を2000ms(2秒)の停止へ変更します。

#include <fd.h>
void main (void) {
  uint8_t ReadData;

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

  while (!fd_rx_buff ()) {
    DX_ReadByteData (100, 27, &ReadData, 10, NULL);
    fd_printf ("%3d\r", ReadData);
    if (ReadData > 100) {
      fd_DXSetPosition (1, 300);
    } else {
      fd_DXSetPosition (1, 512);
    }
    fd_Wait (2000);
  }
}
Page Top

トルクリミットの設定 anchor.png

これで図に書いた一通りの処理のプログラムを書きましたので、書き込んで実行したいところですが、プログラムを一通り書いた後、初めてAX-12Aを動かす時に注意する点があります。

プログラムを間違えると、自分が意図していた角度とは異なる角度にAX-12Aのホーンが動いてしまうことがあります。ホーンは0～300°まで動きますが、フレームやケースを取り付けた場合、0～300°の範囲で動かそうとしてもフレームやケースにぶつかってそれより狭い範囲でしか動きません。それにも拘らず、干渉する範囲まで動かそうとするとどうなるでしょうか？
AX-12Aに負担が掛かりギアがかけてしまいます。

そうならないようにテスト動作の際はAX-12Aのトルクリミットを下げます。トルクリミットとはホーンを動かす力の最大値です。ホーンが回りにくい時、AX-12Aは頑張ってホーンを回そうと力を入れます。その力を制限することで壊れることを防ぎます。それでもAX-12Aの負担になりますので予期せぬ動きをした場合は、直ぐに電源を切るようにしましょう。

#include <fd.h>
void main (void) {
  uint8_t ReadData;

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

  fd_DXSetTorqueLimit (1, 154);

  while (!fd_rx_buff ()) {
    DX_ReadByteData (100, 27, &ReadData, 10, NULL);
    fd_printf ("%3d\r", ReadData);
    if (ReadData > 100) {
      fd_DXSetPosition (1, 300);
    } else {
      fd_DXSetPosition (1, 512);
    }
    fd_Wait (2000);
  }
}

fd_DXSetTorqueLimitは指定されたアクチュエータのトルクを変更します。

第1引数は、DynamixelのIDです。AX-12AのIDは1です。
第2引数は、トルクリミットを指定します。トルクリミットは0～1023まで設定することができます。ここでは154を指定していますので、約15%でトルクが制限されます。100%のトルクを得るには1023を指定します。

トルク制限を行ってしまうと、必要なトルクが得られないこともあります。その場合は、一旦トルク制限を行い、徐々に制限値を上げていきましょう。

Page Top

動作確認 anchor.png

一通りプログラムが完成しました。FDIII-HCにプログラムを書き込んで動かしてみましょう。

書き込み方法はChapter2.4などを参考にして下さい。

動かして見て気になる点は無いでしょうか？

AutomaticDoor_25.png

人がセンサの前に立ってから、バーが開くまでにちょっと時間が掛かっているのが気になりませんか？

Page Top

プログラムの修正 anchor.png

プログラムの流れを書いた図を見てみましょう。 センサ値取得 → 閾値と比較 → バーを開く/閉じる→少し待機(2秒) → センサ値取得・・・というようにループしています。
つまり毎回2秒待っているため、センサの値を2秒に1回しか見ていないということになります（他の処理は殆んど一瞬で終わっています)。よって人がセンサの前に立っても、最悪2秒間はバーが開かないのです。 2秒待ちたいのはバーを開いた後だけなので処理の流れは以下のようになります。

flow_2.png

プログラムに反映すると以下のようになります。

#include 
void main (void) {
  uint8_t ReadData;

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

  fd_DXSetTorqueLimit (1, 154);

  while (!fd_rx_buff ()) {
    DX_ReadByteData (100, 27, &ReadData, 10, NULL);
    fd_printf ("%3d\r", ReadData);
    if (ReadData > 100) {
      fd_DXSetPosition (1, 300);
      fd_Wait (2000);
    } else {
      fd_DXSetPosition (1, 512);
    }
    fd_Wait (10);
  }
}
Page Top

再び動作確認 anchor.png

修正が完成しました。再びFDIII-HCにプログラムを書き込んで動かしてみましょう。
如何でしょうか？
バーを開いた後の待機時間が2秒では短いと感じませんか？
もし短かったら、fd_Waitの引数を増やして再び書き込んで試してみて下さい。

このように、ある程度プログラムが作成できたら動作確認と修正を繰り返します。そうすることで納得できるプログラムの完成となるわけです。

次のチャプターへ

FDIII-HC Starter Kit Guideへ戻る

添付ファイル: filedown_arrow.png 302件 [詳細] fileflow_2.png 419件 [詳細] fileflow_1.png 478件 [詳細] fileAutomaticDoor_25.png 313件 [詳細] fileAX12_position.png 311件 [詳細] filegcc_save.png 359件 [詳細] fileAutomaticDoor_1.png 329件 [詳細]
トップ   差分 リロード印刷に適した表示   全ページ一覧 単語検索 最新ページの一覧   最新ページのRSS 1.0 最新ページのRSS 2.0 最新ページのRSS Atom
最終更新: 2013-04-09 (火) 23:44:33 (JST) (4026d)
リンクページ: FDIII-HC Starter Kit Guide Chapter4.3 Chapter3.4 Chapter3.2 Chapter1.1 FREEDOM III library Chapter2.4