3: 2011-07-06 (水) 12:54:27 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 26: | Line 28: | ||
AX-S1の操作は不要になりましたので、関連する関数を削除します。 | AX-S1の操作は不要になりましたので、関連する関数を削除します。 | ||
合わせて、不要になる変数も削除します。~ | 合わせて、不要になる変数も削除します。~ | ||
- | #ref(gcc_edit_1.png,100%) | + | #html{{ |
- | AX-12+のホーンを回転させれば手は開きますので、[[fd_DXSetPosition>FDIIILIB#nbe84864]]を使用します。~ | + | <pre class="brush:c"> |
+ | #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); | ||
+ | } | ||
+ | } | ||
+ | </pre> | ||
+ | }} | ||
+ | #ref(down_arrow.png) | ||
+ | #html{{ | ||
+ | <pre class="brush:c"> | ||
+ | #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); | ||
+ | } | ||
+ | } | ||
+ | </pre> | ||
+ | }} | ||
+ | AX-12Aのホーンを回転させれば手は開きますので、[[fd_DXSetPosition>FDIIILIB#nbe84864]]を使用します。~ | ||
#ref(AX12Hone.png,100%) | #ref(AX12Hone.png,100%) | ||
- | AX-12+のホーンは0~300°の範囲で動き、それに対するポジション値はそれぞれ0~1023です。 | + | AX-12Aのホーンは0~300°の範囲で動き、それに対するポジション値はそれぞれ0~1023です。 |
#ref(RobotHand_p4.png, 100%) | #ref(RobotHand_p4.png, 100%) | ||
- | 手が開いた状態は、AX-12+のポジションが300の時とします。もし手を反対に付けてしまった場合は、700の時となります(以下300として説明します)。 | + | 手が開いた状態は、AX-12Aのポジションが300の時とします。もし手を反対に付けてしまった場合は、700の時となります(以下300として説明します)。 |
**負荷を読み込む [#n3c87f70] | **負荷を読み込む [#n3c87f70] | ||
- | 先ずAX-12+から読み込んだ負荷の値を保存するための変数を宣言します。負荷は、AX-12+コントロールテーブルのアドレス40と41の値で表されることから、読み込んだ負荷の値を入れる変数も、16ビットのデータサイズである必要があります。 | + | 先ずAX-12Aから読み込んだ負荷の値を保存するための変数を宣言します。負荷を取得する関数([[fd_DXGetLoad>FDIIILIB#e1ca8211]])が用意されていますので、引数として必要なint16_t型で宣言しています。 |
+ | #html{{ | ||
+ | <pre class="brush:c"> | ||
+ | #include <fd.h> | ||
+ | void main (void) { | ||
+ | int16_t LoadData; | ||
- | #ref(gcc_edit_2.png, 100%) | + | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); |
+ | fd_SetUVThreshold (7.4); | ||
+ | DX_ChangeBaudrate (1000000); | ||
- | 変数名のLoadDataは、負荷データという意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。 | + | fd_DXSetTorqueLimit (1, 154); |
- | AX-12+から負荷の値を読込むには、[[fd_DXGetLoad>FDIIILIB#e1ca8211]]関数を使用します。 | + | fd_DXSetPosition (1, 512); |
- | + | ||
- | ''第1引数''は、DynamixelのIDです。AX-12+のIDは1です。~ | + | |
- | ''第2引数''は、現在の負荷の値をいれるための変数のアドレスです。 | + | |
+ | 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は、負荷データという意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。 | ||
---- | ---- | ||
fd_DXGetLoad関数の後に読み込んだ負荷の値を表示します。 | fd_DXGetLoad関数の後に読み込んだ負荷の値を表示します。 | ||
- | 負荷の値は[[コントロールテーブル>DXTABLE1#k921f4f9]]で11ビット使用することになっていますので、4桁表示としています。(11ビットで表現できる最大数は2047) | + | 負荷の値は[[コントロールテーブル>DXTABLE1#k921f4f9]]で11ビット使用することになっていますが、fd_DXGetLoadではビット10を5桁表示としています。10ビットで表現できる最大数は1023ですが、マイナス符号があるため5桁としています。 |
---- | ---- | ||
Line 56: | 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+の負荷の値は下表のように16ビットで表されますが、上位5ビットは使用していません。 | + | |
- | + | ||
- | | ビット | Bit15~11 | Bit10 | Bit9 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となります。 | + | |
**手を少しづつ閉じる [#mdb3661c] | **手を少しづつ閉じる [#mdb3661c] | ||
- | 負荷の値によりゴールポジションの値を少しづつ増やしながら、AX-12+に逐次書き込むことで、手を少しづつ閉じる動作を実現します。 | + | 負荷の値によりゴールポジションの値を少しづつ増やしながら、AX-12Aに逐次書き込むことで、手を少しづつ閉じる動作を実現します。 |
+ | #html{{ | ||
+ | <pre class="brush:c"> | ||
+ | #include <fd.h> | ||
+ | void main (void) { | ||
+ | int16_t LoadData; | ||
+ | uint16_t GoalPosition = 300; | ||
- | #ref(RobotHand_p11.png, 100%) | + | 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); | ||
+ | } | ||
+ | } | ||
+ | </pre> | ||
+ | }} | ||
先ずゴールポジションを保存する変数を宣言します。 | 先ずゴールポジションを保存する変数を宣言します。 | ||
- | uint16_t GoalPositon = 300は、変数宣言時に予め300という値を入れておくという意味です。 | + | uint16_t GoalPositon = 300は、変数宣言時に予め300という値を入れておくという意味です。~ |
+ | ゴールポジションを1加算して、fd_DXSetPositionでゴールポジションの指定を行います。~ | ||
+ | **処理の流れの見直し [#f5709937] | ||
---- | ---- | ||
+ | 今回必要な負荷の値はマイナス値となっていますので、閾値もマイナスの値を使用します。マイナス値はプラスの値と異なり、数字が大きくなればなる程、値としては小さくなります。 | ||
+ | つまり、マイナスでは-100<-10ですが、プラスでは100>10となります。~ | ||
+ | [[処理の流れの図>#i00714fa]]では負荷の値<閾値となっていますので、仮に閾値を-100とした場合、手を閉じる判断は負荷の値<-100となってしまいます。これは、開く方向に大きな負荷がかかった場合のみ手を閉じることになってしまうため、マイナス符号を考慮して、以下のように処理の流れを修正します。 | ||
- | 次に負荷の閾値を決めます。~ | + | #ref(flow_2.png,100%) |
- | 手を閉じて物を挟んだ時、負荷は手が開く方向にかかるので、負荷の方向1024と負荷の値を足した値が閾値となります。~ | + | |
- | ここでは負荷の値を100として、閾値を1124とします。 | + | |
- | ---- | + | 処理の流れに合わせて、プログラムを修正します。 |
- | + | ||
- | 閾値を超えたらfd_DXSetPositionで1加算したゴールポジションを指定します。~ | + | |
- | + | ||
- | **動作確認 [#x3a5a029] | + | |
#html{{ | #html{{ | ||
<pre class="brush: c"> | <pre class="brush: c"> | ||
#include <fd.h> | #include <fd.h> | ||
void main (void) { | void main (void) { | ||
- | uint16_t LoadData; | + | int16_t LoadData; |
uint16_t GoalPosition = 300; | uint16_t GoalPosition = 300; | ||
Line 107: | Line 177: | ||
fd_DXSetTorqueLimit (1, 154); | fd_DXSetTorqueLimit (1, 154); | ||
- | fd_DXSetPosition (1, 300); | + | fd_DXSetPosition (1, 512); |
while (!fd_rx_buff () && !fd_GetPB ()) { | while (!fd_rx_buff () && !fd_GetPB ()) { | ||
fd_DXGetLoad (1, &LoadData); | fd_DXGetLoad (1, &LoadData); | ||
- | fd_printf ("%4d\r", LoadData); | + | fd_printf ("%5d\r", LoadData); |
- | if (LoadData < 1124) { | + | if (LoadData < -100 && LoadData < 0) { |
fd_DXSetPosition (1, ++GoalPosition); | fd_DXSetPosition (1, ++GoalPosition); | ||
} | } | ||
Line 121: | Line 191: | ||
}} | }} | ||
+ | **動作確認 [#x3a5a029] | ||
プログラムが完成しました。プログラムを書き込んで動かしてみましょう。 | プログラムが完成しました。プログラムを書き込んで動かしてみましょう。 | ||
Line 132: | Line 203: | ||
[[次のチャプターへ>FDIIICHAPTER5.4]]~ | [[次のチャプターへ>FDIIICHAPTER5.4]]~ | ||
- | [[FDIII-HC Learning Guide]]に戻る | + | [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]に戻る |