Besttechnology - ロボットハンドのプログラミング [FDIII-HC Starter Kit Guide/Chapter5.3]

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

Table of contents

プログラミング

プログラミング

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

処理の流れを図にする。

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

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

準備

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

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

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

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

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

#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を使用します。

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

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

負荷を読み込む

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

fd_DXGetLoad

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

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

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

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

AX-12Aの負荷の値

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

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

手を少しづつ閉じる

負荷の値によりゴールポジションの値を少しづつ増やしながら、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でゴールポジションの指定を行います。

処理の流れの見直し

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

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

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