3: 2011-07-06 (水) 12:54:27 yoshida | 4: 2011-07-06 (水) 17:01:36 yoshida | ||
---|---|---|---|
Line 11: | Line 11: | ||
先ず手を大きく開きます。~ | 先ず手を大きく開きます。~ | ||
- | 負荷を測定し、負荷の値が閾値より小さければ少し手を閉じ、閾値以上ならば何もせず負荷の測定へ戻るを繰り返します。 | + | 負荷の値を取得し、負荷の値が閾値より小さければ少し手を閉じ、閾値以上ならば何もせず負荷の取得へ戻ることを繰り返します。 |
**準備 [#i7d866da] | **準備 [#i7d866da] | ||
Line 37: | Line 37: | ||
**負荷を読み込む [#n3c87f70] | **負荷を読み込む [#n3c87f70] | ||
- | 先ずAX-12+から読み込んだ負荷の値を保存するための変数を宣言します。負荷は、AX-12+コントロールテーブルのアドレス40と41の値で表されることから、読み込んだ負荷の値を入れる変数も、16ビットのデータサイズである必要があります。 | + | 先ずAX-12+から読み込んだ負荷の値を保存するための変数を宣言します。負荷を取得する関数([[fd_DXGetLoad>FDIIILIB#e1ca8211]])が用意されていますので、引数として必要なint16_t型で宣言しています。 |
#ref(gcc_edit_2.png, 100%) | #ref(gcc_edit_2.png, 100%) | ||
+ | - fd_DXGetLoad | ||
+ | アクチュエータの負荷を取得します。~ | ||
+ | 第1引数 DynamixelのIDを指定します。AX-12+のIDは1です。~ | ||
+ | 第2引数 負荷を保存する変数のアドレスを指定します。 | ||
+ | ---- | ||
変数名のLoadDataは、負荷データという意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。 | 変数名のLoadDataは、負荷データという意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。 | ||
- | AX-12+から負荷の値を読込むには、[[fd_DXGetLoad>FDIIILIB#e1ca8211]]関数を使用します。 | ||
- | |||
- | ''第1引数''は、DynamixelのIDです。AX-12+のIDは1です。~ | ||
- | ''第2引数''は、現在の負荷の値をいれるための変数のアドレスです。 | ||
- | |||
---- | ---- | ||
fd_DXGetLoad関数の後に読み込んだ負荷の値を表示します。 | fd_DXGetLoad関数の後に読み込んだ負荷の値を表示します。 | ||
- | 負荷の値は[[コントロールテーブル>DXTABLE1#k921f4f9]]で11ビット使用することになっていますので、4桁表示としています。(11ビットで表現できる最大数は2047) | + | 負荷の値は[[コントロールテーブル>DXTABLE1#k921f4f9]]で11ビット使用することになっていますが、fd_DXGetLoadではビット10を5桁表示としています。10ビットで表現できる最大数は1023ですが、マイナス符号があるため5桁としています。 |
---- | ---- | ||
Line 56: | Line 56: | ||
負荷の値がどのようなものなのか確認するために、コンパイルしてFDIII-HCに書き込み実行してみましょう。 | 負荷の値がどのようなものなのか確認するために、コンパイルしてFDIII-HCに書き込み実行してみましょう。 | ||
- | #ref(RobotHand_p9.png, 100%) | + | #ref(simpleterm.png, 100%) |
**AX-12+の負荷の値 [#a916853d] | **AX-12+の負荷の値 [#a916853d] | ||
Line 63: | Line 63: | ||
#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-12+に逐次書き込むことで、手を少しづつ閉じる動作を実現します。 | ||
- | #ref(RobotHand_p11.png, 100%) | + | #ref(gcc_edit_3.png, 100%) |
先ずゴールポジションを保存する変数を宣言します。 | 先ずゴールポジションを保存する変数を宣言します。 | ||
Line 87: | Line 76: | ||
次に負荷の閾値を決めます。~ | 次に負荷の閾値を決めます。~ | ||
- | 手を閉じて物を挟んだ時、負荷は手が開く方向にかかるので、負荷の方向1024と負荷の値を足した値が閾値となります。~ | + | 手を閉じて物を挟んだ時、負荷は手が開く方向にかかるので、マイナス値となります。閾値がマイナスの場合、[[処理の流れ>#i00714fa]]の条件が一致しませんので、abs関数でマイナス符号を取り払います。負荷の値の閾値を100とします。 |
- | ここでは負荷の値を100として、閾値を1124とします。 | + | |
+ | #ref(gcc_edit_4.png,100%) | ||
---- | ---- | ||
閾値を超えたらfd_DXSetPositionで1加算したゴールポジションを指定します。~ | 閾値を超えたらfd_DXSetPositionで1加算したゴールポジションを指定します。~ | ||
+ | |||
+ | |||
+ | ---- | ||
+ | 負荷の値と閾値の判断 | ||
+ | #ref(flow_2.png,100%) | ||
**動作確認 [#x3a5a029] | **動作確認 [#x3a5a029] | ||
Line 99: | Line 93: | ||
#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 112: | Line 106: | ||
fd_DXGetLoad (1, &LoadData); | fd_DXGetLoad (1, &LoadData); | ||
fd_printf ("%4d\r", LoadData); | fd_printf ("%4d\r", LoadData); | ||
- | if (LoadData < 1124) { | + | if (LoadData > -100) { |
fd_DXSetPosition (1, ++GoalPosition); | fd_DXSetPosition (1, ++GoalPosition); | ||
} | } |