Besttechnology - 自動ドアのプログラミング [FDIII-HC Starter Kit Guide/Chapter3.3]

Table of contents

プログラミング

プログラミング

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

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

処理の流れを図にする

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

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

準備

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

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

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

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

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

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

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引数は、指令位置(ゴールポジション)です。

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

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

バーを閉じる

バーを閉じるにはバーを開く時の反対の条件が必要になります。
バーを開く時の条件が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を使用します。

待ち時間を増やす

自動ドアは一旦開くとしばらく閉まりません。そのため、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);
  }
}

トルクリミットの設定

これで図に書いた一通りの処理のプログラムを書きましたので、書き込んで実行したいところですが、プログラムを一通り書いた後、初めて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を指定します。

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

動作確認

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

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

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

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

プログラムの修正

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

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

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