FDIII-HC Starter Kit Guide Chapter3.3

プログラミング

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

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

処理の流れを図にする

処理の流れを図にしましょう。

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

準備

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

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

#include <fd.h>
#define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}

void main (void) {
  uint8_t ReadData;
  
  fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4);
  
  while (1) {
    fd_DXReadByteData (100, 27, &ReadData);
    fd_printf ("%3d\r", ReadData);
    if (ReadData > 100) {
      fd_DXWriteByteData (100, 40, 10);
      fd_DXWriteByteData (100, 41, 10);
    }
    KEY_QUIT;
  }
}

プログラムは上記のようになっているでしょうか。
Chapter2.5の補足で追加した箇所があります。

#define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}

と

    KEY_QUIT;

についてはChapter2.5のソフトウェアリセットをご覧下さい。

  fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4);

についてはChapter2.5のFDIII-HCのブザーを鳴らすをご覧下さい。

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

メニューの「ファイル」→「名前を付けて保存」→ファイル名に「AutomaticDoor.c」と入力し、適当なフォルダに「保存」します。

ブザーを鳴らす部分を削除

     fd_DXWriteByteData (100, 40, 10);
     fd_DXWriteByteData (100, 41, 10);

はブザーを鳴らす部分です。今回は必要ありませんので削除します。

バーを開ける

センサ値と閾値を比較するif文の{ }の中に

     fd_DXWriteWordData (1, 30, 300);

と入力して下さい。

これはAX-12+に2バイトのデータを書き込むための関数です。

fd_DXWriteWordDataは関数名です。

第1引数は、DynamixelのIDです。付属のAX-12+のIDは1です。

第2引数は、AX-12+のコントロールテーブルのアドレスです。AX-12+コントロールテーブルを見て下さい。AX-12+のホーンの位置の指令は、アドレス30と31のゴールポジションにデータを書くことで行います。
ゴールポジションはアドレス2つ分(2バイト)で表されます。2バイトのデータで表されているので、fd_DXWriteByteData関数(1バイドのデータを書き込む)ではなく、fd_DXWriteWordData関数(2バイドのデータを書き込む)を使います。

第3引数は、書き込む値(ここではコールポジション)です。

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

バーが開いている状態は大凡0°と150°の真ん中として、ゴールポジションの値を300にしました。もし、AX-12+を反対向きに付けた場合は700位にしましょう。

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

if~ else~

if文後の{}の後に else {} と入力します。

if文の()内の条件以外の場合は、elseの{}内を実行します。ここでは「センサ値が閾値(100)より大きい場合を除く」、つまり「センサ値が閾値以下の場合」else 内を実行するということなります。

バーを閉じる

else 後の {} 内に

     fd_DXWriteWordData (1, 30, 512);

と記載します。バーを開ける場合のゴールポジションを512(真ん中)に変えただけです。

少し待つ

else の{}の後に

   fd_Wait (2000);

と入力して下さい。 これは指定した時間待機する関数です。

fd_Waitは関数名です。

第1引数は、待つ時間(ms)です。ms(ミリ秒)は1000分の1秒です。つまり1000と入力すると1秒待機します。ここでは2秒待機させるために2000と入力しました。

トルクリミットの設定

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

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

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

fd_Init関数の後に

 fd_DXWriteWordData (1, 34, 150);

と入力して下さい。

AX-12+コントロールテーブルを見て下さい。
トルクリミットはアドレス34と35の2バイトで表されるので、fd_DXWriteWordData関数を使います。
IDは1、アドレスは34、トルクリミットは150とします。
トルクリミットは1～1024まで設定でき、0にすると最大の力で動作します。
値が小さ過ぎると力が足りずにホーンが回らなくなります。

動作確認

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

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

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

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

プログラムの修正

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

2秒待ちたいのはバーを開いた後だけですのでバーを開く

     fd_DXWriteWordData (1, 30, 300);

の後に

     fd_Wait (2000);

を移動します。

再び動作確認

#include <fd.h>
#define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}

void main (void) {
  uint8_t ReadData;

  fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4);
  fd_DXWriteWordData (1, 34, 150);

  while (1) {
    fd_DXReadByteData (100, 27, &ReadData);
    fd_printf ("%3d\r", ReadData);
    if (ReadData > 100) {
      fd_DXWriteWordData (1, 30, 300);
      fd_Wait (2000);
    }
    else {
      fd_DXWriteWordData (1, 30, 512);
    }
    KEY_QUIT;
  }
}

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

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

如何でしょうか？
バーを開いた後の待機時間が2秒では短いと感じませんか？
もし短かったら、fd_Waitの引数を増やして再び書き込んで試してみて下さい。

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

次のチャプターへ

FDIII-HC Starter Kit Guideへ戻る