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

プログラミング anchor.png

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

Page Top

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

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

 100%, flow.png

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

Page Top

準備 anchor.png

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

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


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

Page Top

メインループ内を削除し、手を開くを追加 anchor.png

AX-S1の操作は不要になりましたので、関連する関数を削除します。 合わせて、不要になる変数も削除します。

#include <fd.h>
void main (void) {
  uint8_t ReadData;

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

  while (!fd_rx_buff ()) {
    DX_ReadByteData (100, 36, &ReadData, 10, NULL);
    fd_printf ("%3d\r", ReadData);
    fd_DXSetPosition (1, ReadData * 3);
    if (fd_GetPB ()) {
      DX_WriteByteData (100, 36, 0, 10, NULL);
    }
    fd_Wait (10);
  }
}
down_arrow.png
#include <fd.h>
void main (void) {

  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_Wait (10);
  }
}

AX-12Aのホーンを回転させれば手は開きますので、fd_DXSetPositionを使用します。

AX12Home.png

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

 100%, RobotHand_p4.png

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

Page Top

負荷を読み込む anchor.png

先ずAX-12Aから読み込んだ負荷の値を保存するための変数を宣言します。負荷を取得する関数(fd_DXGetLoad)が用意されていますので、引数として必要なint16_t型で宣言しています。

#include <fd.h>
void main (void) {
  int16_t LoadData;

  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, 512);

  while (!fd_rx_buff () && !fd_GetPB ()) {
    fd_DXGetLoad (1, &LoadData);
    fd_printf ("%5d\r", LoadData);
    fd_Wait (10);
  }
}
Page Top

fd_DXGetLoad anchor.png

アクチュエータの現在の負荷の値を取得します。
第1引数は、DynamixelのIDを指定します。AX-12AのIDは1です。
第2引数は、負荷の値を保存する変数のアドレスを指定します。


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


fd_DXGetLoad関数の後に読み込んだ負荷の値を表示します。 負荷の値はコントロールテーブルで11ビット使用することになっていますが、fd_DXGetLoadではビット10を5桁表示としています。10ビットで表現できる最大数は1023ですが、マイナス符号があるため5桁としています。


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

 100%, simpleterm.png
Page Top

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

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

 100%, RobotHand_p10.png

閉じる方向へ力を加えると、負荷の値は50や100といった値になります。開く方向へ力を加えると、-50や-100といった値になります。手を閉じて物をはさむ場合、手が開く方向に負荷はかかります。よって負荷はマイナスの値を使用することになります。

Page Top

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

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

#include <fd.h>
void main (void) {
  int16_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, 512);

  while (!fd_rx_buff () && !fd_GetPB ()) {
    fd_DXGetLoad (1, &LoadData);
    fd_printf ("%5d\r", LoadData);
    fd_DXSetPosition (1, ++GoalPosition);
    fd_Wait (10);
  }
}

先ずゴールポジションを保存する変数を宣言します。 uint16_t GoalPositon = 300は、変数宣言時に予め300という値を入れておくという意味です。
ゴールポジションを1加算して、fd_DXSetPositionでゴールポジションの指定を行います。

Page Top

処理の流れの見直し anchor.png


今回必要な負荷の値はマイナス値となっていますので、閾値もマイナスの値を使用します。マイナス値はプラスの値と異なり、数字が大きくなればなる程、値としては小さくなります。 つまり、マイナスでは-100<-10ですが、プラスでは100>10となります。
処理の流れの図では負荷の値<閾値となっていますので、仮に閾値を-100とした場合、手を閉じる判断は負荷の値<-100となってしまいます。これは、開く方向に大きな負荷がかかった場合のみ手を閉じることになってしまうため、マイナス符号を考慮して、以下のように処理の流れを修正します。

flow_2.png

処理の流れに合わせて、プログラムを修正します。

#include <fd.h>
void main (void) {
  int16_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, 512);

  while (!fd_rx_buff () && !fd_GetPB ()) {
    fd_DXGetLoad (1, &LoadData);
    fd_printf ("%5d\r", LoadData);
    if (LoadData < -100 && LoadData < 0) {
      fd_DXSetPosition (1, ++GoalPosition);
    }
    fd_Wait (10);
  }
}
Page Top

動作確認 anchor.png

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

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

 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: 2013-04-09 (Tue) 23:47:49 (JST) (2728d)