*プログラミング [#zd1ae695] #ref(AutomaticDoor_13.png,100%) 「AX-S1の距離センサに人が反応したら、AX-12+でバーを開く」というプログラムを作成します~ Chapter2で「AX-S1の距離センサに泥棒が反応したら、AX-S1のブザーを鳴らして脅かす」というプログラムを作りました。違いはAX-S1のブザーを鳴らすか、AX-12+を動かすかだけです。 **処理の流れを図にする [#t29737cd] 処理の流れを図にしましょう。 #ref(AutomaticDoor_14.png,100%) Chapter2との違いはブザーを鳴らす代わりに、センサ値が閾値以上ならバーを開け、閾値以下ならバーを閉じるという所です。人が来てバーが開い後、直ぐ閉じてしまうと困るのでその後に少し待機します。 **準備 [#o28e1018] 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のソフトウェアリセット>FDIII-HC Starter Kit Guid Chapter2.5#e7171726]]をご覧下さい。 fd_Init (0, BT_CONSOLE, FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP, 7.4); については[[Chapter2.5のFDIII-HCのブザーを鳴らす>FDIII-HC Starter Kit Guid Chapter2.5#vb6f386d]]をご覧下さい。 ---- ファイルを別の名前で保存しましょう。 メニューの「ファイル」 -> 「名前を付けて保存」 -> ファイル名に「AutomaticDoor.c」と入力して適当なフォルダの中へ「保存」 **ブザーを鳴らす部分を削除 [#ma9b92ad] #ref(AutomaticDoor_15.png,100%) fd_DXWriteByteData (100, 40, 10); fd_DXWriteByteData (100, 41, 10); はブザーを鳴らす部分です。今回は必要ありませんので削除します。 **バーを開ける [#icc81839] #ref(AutomaticDoor_16.png,100%) センサ値と閾値を比較するif分の { } の中に fd_DXWriteWordData (1, 30, 300); と入力して下さい。 これはAX-12+に2バイトのデータを書き込むための関数です。 #ref(AutomaticDoor_17.png,100%) fd_DXWriteWordDataは関数名です。 ''第1引数''はDynamixelのIDです。付属のAX-12+のIDは1です。 ''第2引数''はAX-12+のコントロールテーブルのアドレスです。[[AX-12+コントロールテーブル>FDIII-HC Starter Kit Guid Chapter1.1#xa86a787]]を見て下さい。AX-12+のホーンの位置の指令は、アドレス30と31のゴールポジションにデータを書くことで行います。~ ゴールポジションはアドレス2つ分(2バイト)で表されます。2バイトのデータで表されているので、fd_DXWriteByteData関数(1バイドのデータを書き込む)ではなく、fd_DXWriteWordData関数(2バイドのデータを書き込む)を使います。 ''第3引数''は書き込む値(ここではコールポジション)です。 #ref(AutomaticDoor_18.png,100%) AX-12+のホーンは図のように0°から300°まで回転します。ゴールポジションの値は0°なら0、中心の150°なら512、300°なら1023になります。~ #ref(AutomaticDoor_19.png,100%) バーが開いている状態は大凡0°と150°の真ん中として、ゴールポジションの値を300にしました。もし、AX-12+を反対向きに付けた場合は700位にしましょう。 **if~ else~ [#e4c030b4] #ref(AutomaticDoor_20.png,100%) if文後の{}の後に else {} と入力します。(if文について詳しくは[[Chapter1.4のif文>FDIII-HC Starter Kit Guid Chapter1.4#c6144380]]をご覧下さい。)~ if文の () 内の条件以外の場合は、elseの{}内を実行します。ここでは「センサ値が閾値(100)より大きい場合を除く」、つまり「センサ値が閾値以下の場合」else 内を実行するということなります。 **バーを閉じる [#sd8759fe] #ref(AutomaticDoor_21.png,100%) else 後の {} 内に fd_DXWriteWordData (1, 30, 512); と記載します。バーを開ける場合のゴールポジションを512(真ん中)に変えただけです。 **少し待つ [#q859211e] #ref(AutomaticDoor_22.png,100%) else の {} の後に fd_Wait (2000); と入力して下さい。 これは指定した時間待機する関数です。 #ref(AutomaticDoor_23.png,100%) fd_Waitは関数名です。 ''第1引数''は待つ時間(ms)です。ms(ミリ秒)は1000分の1秒です。つまり1000と入力すると1秒待機します。ここでは2秒待機させるために2000と入力しました。 **トルクリミットの設定 [#a3422d73] これで図に書いた一通りの処理のプログラムを書きましたので、書き込んで実行したいところですが、プログラムを一通り書いた後、初めてAX-12+を動かす時に注意する点があります。~ プログラムを間違えると、自分が意図していた角度とは異なる角度にAX-12+のホーンが動いてしまうことがあります。ホーンは0~300°まで動きますが、フレームやケースを取り付けた場合、0~300°の範囲で動かそうとしてもフレームやケースにぶつかってそれより狭い範囲でしか動きません。それにも拘らず、干渉する範囲まで動かそうとするとどうなるでしょうか?~ AX-12+に負担が掛かりギアがかけてしまいます。~ そうならないようにテスト動作の際はAX-12+のトルクリミットを下げます。トルクリミットとはホーンを動かす力の最大値です。ホーンが回りにくい時、AX-12+は頑張ってホーンを回そうと力を入れます。その力を制限することで壊れることを防ぎます。~ ただそれでもAX-12+に負担になりますので予期せぬ動きをした場合は、直ぐに電源を切るようにしましょう。 #ref(AutomaticDoor_24.png,100%) fd_Init関数の後に fd_DXWriteWordData (1, 34, 150); と入力して下さい。~ [[AX-12+コントロールテーブル>FDIII-HC Starter Kit Guid Chapter1.1#xa86a787]]を見て下さい。~ トルクリミットはアドレス34と35の2バイトで表されるので、fd_DXWriteWordData関数を使います。~ IDは1、アドレスは34、トルクリミットは150とします。~ トルクリミットは1~1024まで設定でき、0にすると最大の力で動作します。~ 値が小さ過ぎると力が足りずにホーンが回らなくなります。 **動作確認 [#e4ea86c9] 一通りプログラムが完成しました。FDIII-HCにプログラムを書き込んで動かしてみましょう。~ 書き込み方法は[[前章>FDIII-HC Starter Kit Guid Chapter2.4#ybaa88fa]]などを参考にして下さい。~ 動かして見て気になる点は無いでしょうか? #ref(AutomaticDoor_25.png,100%) 人がセンサの前に立ってから、バーが開くまでにちょっと時間が掛かっているのが気になりませんか? **プログラムの修正 [#wfed4549] #ref(AutomaticDoor_14.png,100%) プログラムの流れを書いた図を見てみましょう。 センサ値取得 -> 閾値と比較 -> バーを開く/閉じる -> 少し待つ(2秒) -> センサ値・・・というようにループしています。~ つまり毎回2秒待っているため、センサの値を2秒に1回しか見ていないということになります(他の処理は殆んど一瞬で終わっています)。よって人がセンサの前に立っても、最悪2秒間はバーが開かないのです。 #ref(AutomaticDoor_26.png,100%) 2秒待ちたいのはバーを開いた後だけですのでバーを開く fd_DXWriteWordData (1, 30, 300); の後に fd_Wait (2000); を移動します。 **再び動作確認 [#h4448457] #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にプログラムを書き込んで動かしてみましょう。~ 書き込み方法は[[前章>FDIII-HC Starter Kit Guid Chapter2.4#ybaa88fa]]などを参考にして下さい。~ 如何でしょうか?バーを開いた後の待機時間が2秒では短く感じませんか?~ もし短かったら、fd_Waitの引数を増やして、再び書き込んで下さい。 このように、ある程度プログラムができたら、動かして見て、気になる所を修正して、また動かして見て・・・を繰り返すことで納得できるプログラムを作って行きます。 [[次のチャプターへ>FDIII-HC Starter Kit Guid Chapter3.4]]~ [[FDIII-HC Starter Kit Guid]]へ戻る
(This host) = https://www.besttechnology.co.jp