1: 2011-07-05 (火) 18:25:09 yoshida ソース 現: 2013-04-09 (火) 23:47:49 takaboo ソース
Line 1: Line 1:
 +TITLE:ロボットハンドのプログラミング
|CENTER:BGCOLOR(red):||c |CENTER:BGCOLOR(red):||c
-|  :idea:|AX-12+に過度な負荷がかかったり、長時間負荷がかかった状態でいたりすると、ロボットハンドで挟んだ物が壊れるだけでなく、AX-12+のギアが欠けたり、モーターが焼けたりする場合があります。プログラミングの際は十分ご注意下さい。|+|  :idea:|AX-12Aに過度な負荷がかかったり、長時間負荷がかかった状態でいたりすると、ロボットハンドで挟んだ物が壊れるだけでなく、AX-12Aのギアが欠けたり、モーターが焼けたりする場合があります。プログラミングの際は十分ご注意下さい。|
*プログラミング [#m4a200aa] *プログラミング [#m4a200aa]
「掴む力を自動的にコントロールするロボットハンド」のプログラムを作成します。 「掴む力を自動的にコントロールするロボットハンド」のプログラムを作成します。
少しづつ手を閉じて行き、物を挟むことで負荷が増加したら手を閉じるのを止めるという方法で、掴む力をコントロールします。 少しづつ手を閉じて行き、物を挟むことで負荷が増加したら手を閉じるのを止めるという方法で、掴む力をコントロールします。
 +
**処理の流れを図にする。 [#i00714fa] **処理の流れを図にする。 [#i00714fa]
処理の流れを図にしましょう。 処理の流れを図にしましょう。
Line 11: Line 13:
先ず手を大きく開きます。~ 先ず手を大きく開きます。~
-負荷を測定し、負荷の値が閾値より小さければ少し手を閉じ、閾値以上ならば何もせず負荷の測定へ戻るを繰り返します。+負荷の値を取得し、負荷の値が閾値より小さければ少し手を閉じ、閾値以上ならば何もせず負荷の取得へ戻ることを繰り返します。
**準備 [#i7d866da] **準備 [#i7d866da]
Line 23: Line 25:
メニューの「ファイル」→「名前を付けて保存」→ファイル名に「RobotHand.c」と入力して「保存」します。 メニューの「ファイル」→「名前を付けて保存」→ファイル名に「RobotHand.c」と入力して「保存」します。
-**メインループ内を手を開くに変更 [#la580a9f] +**メインループ内を削除し、手を開くを追加 [#la580a9f]
- +
-#ref(gcc_edit_1.png, 100%) +
AX-S1の操作は不要になりましたので、関連する関数を削除します。 AX-S1の操作は不要になりましたので、関連する関数を削除します。
-合わせて、不要になる変数も削除します。+合わせて、不要になる変数も削除します。~ 
 +#html{{ 
 +<pre class="brush:c"> 
 +#include <fd.h> 
 +void main (void) { 
 +  uint8_t ReadData;
-AX-12+のホーンを回転させれば手は開きますので、fd_DXSetPositionを使用します。 +  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); 
-#ref(AX12Hone.png,100%)+  fd_SetUVThreshold (7.4); 
 +  DX_ChangeBaudrate (1000000);
-AX-12+のホーンは0～300°の範囲で動き、それに対するポジション値はそれぞれ0～1023です。+  fd_DXSetTorqueLimit (1, 154);
-#ref(RobotHand_p4.png, 100%)+  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); 
 +  } 
 +} 
 +</pre> 
 +}} 
 +#ref(down_arrow.png) 
 +#html{{ 
 +<pre class="brush:c"> 
 +#include <fd.h> 
 +void main (void) {
-手が開いた状態は、AX-12+のポジションが300の時とします。もし手を反対に付けてしまった場合は、700の時となります(以下300として説明します)。+  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); 
 +  fd_SetUVThreshold (7.4); 
 +  DX_ChangeBaudrate (1000000);
-#ref(gcc_edit_2.png, 100%)+  fd_DXSetTorqueLimit (1, 154); 
 +  fd_DXSetPosition (1, 300);
-**負荷を読み込む [#n3c87f70] +  while (!fd_rx_buff () && !fd_GetPB ()) { 
-先ずAX-12+から読み込んだ負荷の値を保存するための変数を宣言します。負荷は、AX-12+コントロールテーブルのアドレス40と41の値で表されることから、読み込んだ負荷の値を入れる変数も、16ビットのデータサイズである必要があります。+   fd_Wait (10); 
 +  } 
 +} 
 +</pre> 
 +}} 
 +AX-12Aのホーンを回転させれば手は開きますので、[[fd_DXSetPosition>FDIIILIB#nbe84864]]を使用します。~ 
 +#ref(AX12Hone.png,100%)
-#ref(RobotHand_p6.png, 100%)+AX-12Aのホーンは0～300°の範囲で動き、それに対するポジション値はそれぞれ0～1023です。
-メイン関数の初めに +#ref(RobotHand_p4.png, 100%)
- uint16_t PresentLoad; +
-と入力します。+
-uint16_t は、16ビット=2バイトデータの変数の型です。この型はC言語で通常使う型ではありません。通常のC言語で書くとunsigned shortです。+手が開いた状態は、AX-12Aのポジションが300の時とします。もし手を反対に付けてしまった場合は、700の時となります(以下300として説明します)。
-変数名のPresentLoadは、現在の負荷という意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。+**負荷を読み込む [#n3c87f70] 
 +先ずAX-12Aから読み込んだ負荷の値を保存するための変数を宣言します。負荷を取得する関数([[fd_DXGetLoad>FDIIILIB#e1ca8211]])が用意されていますので、引数として必要なint16_t型で宣言しています。 
 +#html{{ 
 +<pre class="brush:c"> 
 +#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);
-AX-12+から負荷の値を読み込みます。 +  fd_DXSetTorqueLimit (1, 154); 
-2バイトのデータを読み込むには、fd_DXReadWordData関数を使います。 +  fd_DXSetPosition (1, 512);
- +
-#ref(RobotHand_p7.png, 100%) +
- +
-while文の中に +
- fd_DXReadWordData (1, 40, &PresentLoad); +
-と入力します。DynamixeのID=1、アドレス=40、読み込んだデータを入れる変数のアドレス=&PresentLoadです。変数のアドレスなのでPresentLoadの前に&を付けます。+
 +  while (!fd_rx_buff () && !fd_GetPB ()) {
 +   fd_DXGetLoad (1, &LoadData);
 +   fd_printf ("%5d\r", LoadData);
 +   fd_Wait (10);
 +  }
 +}
 +</pre>
 +}}
 +***[[fd_DXGetLoad>FDIIILIB#e1ca8211]] [#cebc7456]
 +アクチュエータの現在の負荷の値を取得します。~
 +''第1引数''は、DynamixelのIDを指定します。AX-12AのIDは1です。~
 +''第2引数''は、負荷の値を保存する変数のアドレスを指定します。
 +----
 +変数名のLoadDataは、負荷データという意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。
---- ----
-#ref(RobotHand_p8.png, 100%) +fd_DXGetLoad関数の後に読み込んだ負荷の値を表示します。 
- +負荷の値は[[コントロールテーブル&gt;DXTABLE1#k921f4f9]]で11ビット使用することになっていますが、fd_DXGetLoadではビット10を5桁表示としています。10ビットで表現できる最大数は1023ですが、マイナス符号があるため5桁としています。
-読み込んだ負荷の値を表示します。 +
-fd_DXReadWordDataの後に +
-&nbsp;fd_printf("%6d\r",PresentLoad); +
-と入力します。負荷の値がどのような数値なのかまだ分かりませんので、桁数を多めにとります。+
---- ----
Line 78: Line 118:
負荷の値がどのようなものなのか確認するために、コンパイルしてFDIII-HCに書き込み実行してみましょう。 負荷の値がどのようなものなのか確認するために、コンパイルしてFDIII-HCに書き込み実行してみましょう。
-#ref(RobotHand_p9.png, 100%)+#ref(simpleterm.png, 100%)
-**AX-12+の負荷の値 [#a916853d]+**AX-12Aの負荷の値 [#a916853d]
負荷の値を確認するために、手でロボットハンドを開く方向と閉じる方向へ力を加えてみましょう。 負荷の値を確認するために、手でロボットハンドを開く方向と閉じる方向へ力を加えてみましょう。
#ref(RobotHand_p10.png, 100%) #ref(RobotHand_p10.png, 100%)
-閉じる方向へ力を加えると、負荷の値は50や100といった値になります。開く方向へ力を加えると、少し力を加えただけで1050や1100といった値になります。開く方法へ力を加えた場合、何故急に大きな値になるのでしょうか。+閉じる方向へ力を加えると、負荷の値は50や100といった値になります。開く方向へ力を加えると、-50や-100といった値になります。手を閉じて物をはさむ場合、手が開く方向に負荷はかかります。よって負荷はマイナスの値を使用することになります。
-AX-12+の負荷の値は下表のように2バイト=16ビットで表されます。+**手を少しづつ閉じる [#mdb3661c] 
 +負荷の値によりゴールポジションの値を少しづつ増やしながら、AX-12Aに逐次書き込むことで、手を少しづつ閉じる動作を実現します。 
 +#html{{ 
 +<pre class="brush:c"> 
 +#include <fd.h> 
 +void main (void) { 
 +  int16_t LoadData; 
 +  uint16_t GoalPosition = 300;
-| ビット | Bit15~11 | Bit10 | Bit9 Bit8 Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 | +  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); 
-| 値 | 0 | 負荷の方向 | 負荷の値 | +  fd_SetUVThreshold (7.4); 
- +  DX_ChangeBaudrate (1000000);
-ビット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%)+  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);
 +  }
 +}
 +</pre>
 +}}
先ずゴールポジションを保存する変数を宣言します。 先ずゴールポジションを保存する変数を宣言します。
-uint16_t PresentLoad;の下に +uint16_t GoalPositon = 300は、変数宣言時に予め300という値を入れておくという意味です。~ 
- uint16_t GoalPositon = 300; +ゴールポジションを1加算して、fd_DXSetPositionでゴールポジションの指定を行います。~
-と入力します。 +
-GoalPositon = 300は、変数に予め300という値を入れておくという意味です。+
 +**処理の流れの見直し [#f5709937]
---- ----
 +今回必要な負荷の値はマイナス値となっていますので、閾値もマイナスの値を使用します。マイナス値はプラスの値と異なり、数字が大きくなればなる程、値としては小さくなります。
 +つまり、マイナスでは-100＜-10ですが、プラスでは100＞10となります。~
 +[[処理の流れの図>#i00714fa]]では負荷の値<閾値となっていますので、仮に閾値を-100とした場合、手を閉じる判断は負荷の値<-100となってしまいます。これは、開く方向に大きな負荷がかかった場合のみ手を閉じることになってしまうため、マイナス符号を考慮して、以下のように処理の流れを修正します。
-#ref(RobotHand_p12.png, 100%)+#ref(flow_2.png,100%)
-AX-12+にゴールポジションを書き込みます。 +処理の流れに合わせて、プログラムを修正します。 
-fd_printf文の後に +#html{{ 
-&nbsp;fd_DXWriteWordData (1, 30, GoalPosition); +&lt;pre class="brush: c"> 
-と入力します。+#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);
-#ref(RobotHand_p13.png, 100%)+  fd_DXSetTorqueLimit (1, 154); 
 +  fd_DXSetPosition (1, 512);
-ゴールぽシジョンの値を1増やします。 +  while (!fd_rx_buff () && !fd_GetPB ()) { 
-fd_printf文の後に +    fd_DXGetLoad (1, &LoadData); 
- GoalPosition++; +   fd_printf ("%5d\r", LoadData); 
-と入力します。GoalPositionに1足すという意味です。 +    if (LoadData < -100 && LoadData < 0) { 
- +      fd_DXSetPosition (1, ++GoalPosition); 
-*負荷の値によって分岐 [#m50c25ac] +   } 
- +   fd_Wait (10); 
-負荷の値が、ある閾値より小さい場合だけ、ゴールポジションの値に1足し、そのゴールポジションをAX-12+へ書き込みます。 +  } 
- +} 
-#ref(RobotHand_p14.png, 100%) +</pre> 
- +}}
-ゴールポジションに1足す行とゴールポジションを書き込む行をif文で囲みます。if文の条件は、「負荷の値が1124より下だったら」とします。手を閉じて物を挟んだ時、負荷は手が開く方向にかかります。よって、負荷の方向1024と負荷の値100を足した値の1124としました。 +
- +
- if(PresentLoad < 1124) { +
-    GoalPosition++; +
-   fd_DXWriteWordData (1, 30, GoalPosition); +
- }+
**動作確認 [#x3a5a029] **動作確認 [#x3a5a029]
- #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; 
-   } 
- } 
- 
プログラムが完成しました。プログラムを書き込んで動かしてみましょう。 プログラムが完成しました。プログラムを書き込んで動かしてみましょう。
Line 174: Line 203:
[[次のチャプターへ>FDIIICHAPTER5.4]]~ [[次のチャプターへ>FDIIICHAPTER5.4]]~
-[[FDIII-HC Learning Guide]]に戻る+[[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]に戻る
トップ   差分 リロード印刷に適した表示   全ページ一覧 単語検索 最新ページの一覧   最新ページのRSS 1.0 最新ページのRSS 2.0 最新ページのRSS Atom