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

プログラミング anchor.png

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

Page Top

処理の流れを図にする。 anchor.png

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

 100%, Robothand_p1.png

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

Page Top

準備 anchor.png

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

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


メニューの「ファイル」→「名前を付けて保存」→ファイル名に「RobotHand.c」と入力し、Chapter1.3のサンプルプログラムのダウンロードで作成したSampleフォルダに「保存」します。

Page Top

不要な部分を削除 anchor.png

 100%, Robothand_p2.png

メイン関数内の最初の

uint8_t ReadData;
bool PB;

を削除します。

while文の中の

fd_DXReadByteData (100, 36, &ReadData);
fd_printf ("%3d\r", ReadData);
fd_DXWriteWordData (1, 30, ReadData * 3);
PB = fd_GetPB();
if(PB == true){
  fd_DXWriteByteData (100, 36, 0);
}

を削除します。

Page Top

手を開く anchor.png

AX-12+のホーンを回転させることで手を開きます。

 100%, Robothand_p3.png

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

 100%, Robothand_p4.png

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

 100%, Robothand_p5.png

AX-12+のホーンを動かすには、アドレス30,31にゴールポジションを書き込みます。2バイトのデータを書き込むにはfd_DXWriteWordData関数を使います。

手を開くのは最初の1回だけですので、while文の前に

fd_DXWriteWordData (1, 30, 300);

と入力します。引数は、DynamixelのID=1、アドレス=30、書き込むデータ=300です。

Page Top

負荷を読み込む anchor.png

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

 100%, Robothand_p6.png

メイン関数の初めに

uint16_t PresentLoad;

と入力します。

uint16_t は、16ビット=2バイトデータの変数の型です。この型はC言語で通常使う型ではありません。通常のC言語で書くとunsigned shortです。

変数名のPresentLoadは、現在の負荷という意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。


AX-12+から負荷の値を読み込みます。 2バイトのデータを読み込むには、fd_DXReadWordData関数を使います。

 100%, Robothand_p7.png

while文の中に

fd_DXReadWordData (1, 40, &PresentLoad);

と入力します。DynamixeのID=1、アドレス=40、読み込んだデータを入れる変数のアドレス=&PresentLoadです。変数のアドレスなのでPresentLoadの前に&を付けます。


 100%, Robothand_p8.png

読み込んだ負荷の値を表示します。 fd_DXReadWordDataの後に

fd_printf("%6d\r",PresentLoad);

と入力します。負荷の値がどのような数値なのかまだ分かりませんので、桁数を多めにとります。


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

 100%, Robothand_p9.png
Page Top

AX-12+の負荷の値 anchor.png

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

 100%, Robothand_p10.png

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

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

ビットBit15~11Bit10Bit9 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となります。

Page Top

手を少しづつ閉じる anchor.png

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

 100%, Robothand_p11.png

先ずゴールポジションを保存する変数を宣言します。 uint16_t PresentLoad;の下に

uint16_t GoalPositon = 300;

と入力します。 GoalPositon = 300は、変数に予め300という値を入れておくという意味です。


 100%, Robothand_p12.png

AX-12+にゴールポジションを書き込みます。 fd_printf文の後に

fd_DXWriteWordData (1, 30, GoalPosition);

と入力します。


 100%, Robothand_p13.png

ゴールぽシジョンの値を1増やします。 fd_printf文の後に

GoalPosition++;

と入力します。GoalPositionに1足すという意味です。

Page Top

負荷の値によって分岐 anchor.png

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

 100%, Robothand_p14.png

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

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

動作確認 anchor.png

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

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

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

 100%, Robothand_p15.png

次のチャプターへ

FDIII-HC Starter Kit Guideに戻る


Front page   Diff ReloadPrint View   Page list Search Recent changes   RSS of recent changes (RSS 1.0) RSS of recent changes (RSS 2.0) RSS of recent changes (RSS Atom)
Last-modified: 2010-10-06 (Wed) 12:11:10 (JST) (3304d)