FDIII-HC Starter Kit Guide Chapter5.3 :: Besttechnology

プログラミング

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

処理の流れを図にする。

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

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

準備

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

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

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

不要な部分を削除

メイン関数内の最初の

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

を削除します。

手を開く

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

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

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

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

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

fd_DXWriteWordData (1, 30, 300);

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

負荷を読み込む

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

メイン関数の初めに

uint16_t PresentLoad;

と入力します。

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

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

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

while文の中に

fd_DXReadWordData (1, 40, &PresentLoad);

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

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

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

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

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

AX-12+の負荷の値

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

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

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

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

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

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

手を少しづつ閉じる

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

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

uint16_t GoalPositon = 300;

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

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

fd_DXWriteWordData (1, 30, GoalPosition);

と入力します。

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

GoalPosition++;

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

負荷の値によって分岐

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

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

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

動作確認

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

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

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

次のチャプターへ

FDIII-HC Starter Kit Guideに戻る