FDIII-HC Starter Kit Guide​/Chapter5.3 のバックアップソース(No.2) :: Besttechnology

• 差分^[1] を表示
• 現在との差分^[2] を表示
• FDIII-HC Starter Kit Guide/Chapter5.3^[3] へ行く。

|CENTER:BGCOLOR(red):||c
|  :idea:|AX-12+に過度な負荷がかかったり、長時間負荷がかかった状態でいたりすると、ロボットハンドで挟んだ物が壊れるだけでなく、AX-12+のギアが欠けたり、モーターが焼けたりする場合があります。プログラミングの際は十分ご注意下さい。|

*プログラミング [#m4a200aa]
「掴む力を自動的にコントロールするロボットハンド」のプログラムを作成します。
少しづつ手を閉じて行き、物を挟むことで負荷が増加したら手を閉じるのを止めるという方法で、掴む力をコントロールします。
**処理の流れを図にする。 [#i00714fa]
処理の流れを図にしましょう。

#ref(flow.png, 100%)

先ず手を大きく開きます。~
負荷を測定し、負荷の値が閾値より小さければ少し手を閉じ、閾値以上ならば何もせず負荷の測定へ戻るを繰り返します。

**準備 [#i7d866da]

GCC Developer Liteを起動して、Chapter4で作成したプログラムを開いて下さい。

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

----

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

**メインループ内を削除し、手を開くを追加 [#la580a9f]
AX-S1の操作は不要になりましたので、関連する関数を削除します。
合わせて、不要になる変数も削除します。~
#ref(gcc_edit_1.png,100%)
AX-12+のホーンを回転させれば手は開きますので、fd_DXSetPositionを使用します。~
#ref(AX12Hone.png,100%)

AX-12+のホーンは0～300°の範囲で動き、それに対するポジション値はそれぞれ0～1023です。

#ref(RobotHand_p4.png, 100%)

手が開いた状態は、AX-12+のポジションが300の時とします。もし手を反対に付けてしまった場合は、700の時となります(以下300として説明します)。

**負荷を読み込む [#n3c87f70]
先ずAX-12+から読み込んだ負荷の値を保存するための変数を宣言します。負荷は、AX-12+コントロールテーブルのアドレス40と41の値で表されることから、読み込んだ負荷の値を入れる変数も、16ビットのデータサイズである必要があります。

#ref(RobotHand_p6.png, 100%)

変数名のLoadDataは、負荷データという意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。
AX-12+から負荷の値を読込むには、[[fd_DXGetLoad>FDIIILIB#e1ca8211]]関数を使用します。

''第1引数''は、DynamixelのIDです。AX-12+のIDは1です。~
''第2引数''は、現在の負荷の値をいれるための変数のアドレスです。

----

fd_DXGetLoad関数の後に読み込んだ負荷の値を表示します。
負荷の値は[[コントロールテーブル>DXTABLE1#k921f4f9]]で10ビットまで使用することになっていますので、4桁表示としています。(10ビットで表現できる最大数は2047)

----

負荷の値がどのようなものなのか確認するために、コンパイルしてFDIII-HCに書き込み実行してみましょう。

#ref(RobotHand_p9.png, 100%)

**AX-12+の負荷の値 [#a916853d]
負荷の値を確認するために、手でロボットハンドを開く方向と閉じる方向へ力を加えてみましょう。

#ref(RobotHand_p10.png, 100%)

閉じる方向へ力を加えると、負荷の値は50や100といった値になります。開く方向へ力を加えると、少し力を加えただけで1050や1100といった値になります。開く方法へ力を加えた場合、何故急に大きな値になるのでしょうか。

AX-12+の負荷の値は下表のように2バイト=16ビットで表されます。

| ビット | Bit15~11 | Bit10 | Bit9 Bit8 Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 |
| 値 | 0 | 負荷の方向 | 負荷の値 |

ビット0～9は負荷の値、ビット10は負荷の方向を表しています。つまり負荷の方向によって、ビット10が0又は1になるということです。

ビット10は、2進数の11桁目です。2進数の11桁目は、10進数で表すと1024です。

よって手が開く方向に力を加えた場合、10進数で表すと「負荷の値＋1024」となり、例えば負荷の値が50なら、50+1024で1074となります。

**手を少しづつ閉じる [#mdb3661c]
ゴールポジションの値を少しづつ増やしながら、AX-12+に逐次書き込むことで、手を少しづつ閉じる動作を実現します。

#ref(RobotHand_p11.png, 100%)

先ずゴールポジションを保存する変数を宣言します。
uint16_t PresentLoad;の下に
 uint16_t GoalPositon = 300;
と入力します。
GoalPositon = 300は、変数に予め300という値を入れておくという意味です。

----

#ref(RobotHand_p12.png, 100%)

AX-12+にゴールポジションを書き込みます。
fd_printf文の後に
 fd_DXWriteWordData (1, 30, GoalPosition);
と入力します。

----

#ref(RobotHand_p13.png, 100%)

ゴールぽシジョンの値を1増やします。
fd_printf文の後に
 GoalPosition++;
と入力します。GoalPositionに1足すという意味です。

*負荷の値によって分岐 [#m50c25ac]

負荷の値が、ある閾値より小さい場合だけ、ゴールポジションの値に1足し、そのゴールポジションをAX-12+へ書き込みます。

#ref(RobotHand_p14.png, 100%)

ゴールポジションに1足す行とゴールポジションを書き込む行をif文で囲みます。if文の条件は、「負荷の値が1124より下だったら」とします。手を閉じて物を挟んだ時、負荷は手が開く方向にかかります。よって、負荷の方向1024と負荷の値100を足した値の1124としました。

 if(PresentLoad < 1124) {
    GoalPosition++;
    fd_DXWriteWordData (1, 30, GoalPosition);
 }

**動作確認 [#x3a5a029]
 #include <fd.h>
 #define KEY_QUIT { if(fd_rx_buff()) fd_SoftReset();}

 void main (void) {
   uint16_t PresentLoad;
   uint16_t GoalPosition = 300;

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

   while (1) {
     fd_DXReadWordData (1, 40, &PresentLoad);
     fd_printf("%6d\r",PresentLoad);
     if(PresentLoad < 1124) {
       GoalPosition++;
       fd_DXWriteWordData (1, 30, GoalPosition);
     }
     KEY_QUIT;
   }
 }

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

優しく物を挟んでいるか確認しましょう。
万が一プログラムの誤入力で挟む力の制御がされなかった時のために、直ぐに電源を切れるよう準備しましょう。

#ref(RobotHand_p15.png, 100%)

#html(<object width="480" height="385"><param name="movie" value="http://www.youtube.com/v/F8ZTZNc_tpk?fs=1&amp;hl=ja_JP&amp;rel=0"></param><param name="allowFullScreen" value="true"></param><param name="allowscriptaccess" value="always"></param><embed src="http://www.youtube.com/v/F8ZTZNc_tpk?fs=1&amp;hl=ja_JP&amp;rel=0" type="application/x-shockwave-flash" allowscriptaccess="always" allowfullscreen="true" width="480" height="385"></embed></object>)

[[次のチャプターへ>FDIIICHAPTER5.4]]~

[[FDIII-HC Learning Guide]]に戻る

• FDIII-HC Starter Kit Guide/Chapter5.3 のバックアップ一覧^[6]
• FDIII-HC Starter Kit Guide/Chapter5.3 のバックアップソース(No. All)
  • 1: 2011-07-05 (火) 18:25:09^[7] yoshida^[8]
  • 2: 2011-07-05 (火) 19:17:42 yoshida^[8]
  • 3: 2011-07-06 (水) 12:54:27^[9] yoshida^[8]
  • 4: 2011-07-06 (水) 17:01:36^[10] yoshida^[8]
  • 5: 2011-07-06 (水) 17:59:29^[11] yoshida^[8]
  • 6: 2011-07-11 (月) 12:30:43^[12] yoshida^[8]
  • 7: 2011-07-12 (火) 13:18:25^[13] yoshida^[8]
  • 8: 2011-07-12 (火) 18:41:39^[14] yoshida^[8]
  • 9: 2011-07-22 (金) 11:53:17^[15] yoshida^[8]
  • 現: 2013-04-09 (火) 23:47:49^[16] takaboo^[17]