FDIII-HC Starter Kit Guide​/Chapter5.3 のバックアップソース(No.3) :: 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>FDIIILIB#nbe84864]]を使用します。~
#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(gcc_edit_2.png, 100%)

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

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

----

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

----

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

#ref(RobotHand_p9.png, 100%)

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

#ref(RobotHand_p10.png, 100%)

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

AX-12+の負荷の値は下表のように16ビットで表されますが、上位5ビットは使用していません。

| ビット | 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 GoalPositon = 300は、変数宣言時に予め300という値を入れておくという意味です。

----

次に負荷の閾値を決めます。~
手を閉じて物を挟んだ時、負荷は手が開く方向にかかるので、負荷の方向1024と負荷の値を足した値が閾値となります。~
ここでは負荷の値を100として、閾値を1124とします。

----

閾値を超えたらfd_DXSetPositionで1加算したゴールポジションを指定します。~

**動作確認 [#x3a5a029]
#html{{
<pre class="brush: c">
#include <fd.h>
void main (void) {
  uint16_t LoadData;
  uint16_t GoalPosition = 300;

  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);
  fd_DXSetPosition (1, 300);

  while (!fd_rx_buff () && !fd_GetPB ()) {
    fd_DXGetLoad (1, &LoadData);
    fd_printf ("%4d\r", LoadData);
    if (LoadData < 1124) {
      fd_DXSetPosition (1, ++GoalPosition);
    }
    fd_Wait (10);
  }
}
</pre>
}}

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

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

#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^[9] yoshida^[8]
  • 3: 2011-07-06 (水) 12:54:27 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]