2: 2010-04-01 (木) 20:05:22 sho | 現: 2010-10-06 (水) 12:11:10 eid7gud | ||
---|---|---|---|
Line 1: | Line 1: | ||
- | 只今編集中です。 | ||
- | |||
|CENTER:BGCOLOR(red):||c | |CENTER:BGCOLOR(red):||c | ||
- | | :idea:|AX-12+に過度な負荷がかかったり、長時間負荷がかかったりすると、ロボットハンドで挟んだ物が壊れるだけでなく、AX-12+のギアが欠けたり、モーターが焼けたりする場合があります。プログラミングの際は十分ご注意下さい。| | + | | :idea:|AX-12+に過度な負荷がかかったり、長時間負荷がかかった状態でいたりすると、ロボットハンドで挟んだ物が壊れるだけでなく、AX-12+のギアが欠けたり、モーターが焼けたりする場合があります。プログラミングの際は十分ご注意下さい。| |
*プログラミング [#x9b50a54] | *プログラミング [#x9b50a54] | ||
Line 12: | Line 10: | ||
#ref(RobotHand_p1.png, 100%) | #ref(RobotHand_p1.png, 100%) | ||
- | 先ず手を大きく開きます。そこから手を少し閉じて負荷を測定し、負荷の値が閾値より小さければ再び少し手を閉じるを繰り返します。手が物を挟むことによってAX-12+の負荷が増えて行き、負荷の値が閾値を超えたら手を閉じるのを止め、物を挟んだ状態でストップします。 | + | 先ず手を大きく開きます。~ |
+ | 負荷を測定し、負荷の値が閾値より小さければ少し手を閉じ、閾値以上ならば何もせず負荷の測定へ戻るを繰り返します。 | ||
**準備 [#i6f753bf] | **準備 [#i6f753bf] | ||
Line 45: | Line 44: | ||
**手を開く [#o0c589b7] | **手を開く [#o0c589b7] | ||
- | AX-12+のホーンを動かして手を開きます。 | + | AX-12+のホーンを回転させることで手を開きます。 |
#ref(RobotHand_p3.png, 100%) | #ref(RobotHand_p3.png, 100%) | ||
+ | |||
+ | AX-12+のホーンは0~300°の範囲で動き、それに対するポジション値はそれぞれ0~1023です。 | ||
+ | |||
#ref(RobotHand_p4.png, 100%) | #ref(RobotHand_p4.png, 100%) | ||
- | 手が開いた状態は、AX-12+のホーンの中心が300の時とします。もし手を反対に付けてしまった場合は、700の時としましょう(以下300として説明します)。 | + | 手が開いた状態は、AX-12+のポジションが300の時とします。もし手を反対に付けてしまった場合は、700の時となります(以下300として説明します)。 |
#ref(RobotHand_p5.png, 100%) | #ref(RobotHand_p5.png, 100%) | ||
Line 56: | Line 58: | ||
AX-12+のホーンを動かすには、アドレス30,31にゴールポジションを書き込みます。2バイトのデータを書き込むにはfd_DXWriteWordData関数を使います。 | AX-12+のホーンを動かすには、アドレス30,31にゴールポジションを書き込みます。2バイトのデータを書き込むにはfd_DXWriteWordData関数を使います。 | ||
- | 手を開くのは最初の1回だけなので、while文の前に | + | 手を開くのは最初の1回だけですので、while文の前に |
fd_DXWriteWordData (1, 30, 300); | fd_DXWriteWordData (1, 30, 300); | ||
- | と入力します。引数はDynamixel ID=1、アドレス=30、書き込むデータ=300です。 | + | と入力します。引数は、DynamixelのID=1、アドレス=30、書き込むデータ=300です。 |
**負荷を読み込む [#x263bcf7] | **負荷を読み込む [#x263bcf7] | ||
- | 本来は手を開いた後は「少し手を閉じる」ですが、その前に負荷がどのような値なのかを見てみましょう。 | + | 先ずAX-12+から読み込んだ負荷の値を保存するための変数を宣言します。負荷は、AX-12+コントロールテーブルのアドレス40と41の2バイドで表されます。よって読み込んだ負荷の値を入れる変数も、2バイトのデータサイズである必要があります。 |
+ | |||
+ | #ref(RobotHand_p6.png, 100%) | ||
+ | |||
+ | メイン関数の初めに | ||
+ | uint16_t PresentLoad; | ||
+ | と入力します。 | ||
+ | |||
+ | uint16_t は、16ビット=2バイトデータの変数の型です。この型はC言語で通常使う型ではありません。通常のC言語で書くとunsigned shortです。 | ||
+ | |||
+ | 変数名のPresentLoadは、現在の負荷という意味です。変数名は何でも構いませんが、後で見直した時に、何のための変数なのかが直ぐ分るような名前にしましょう。 | ||
+ | |||
+ | ---- | ||
+ | |||
+ | AX-12+から負荷の値を読み込みます。 | ||
+ | 2バイトのデータを読み込むには、fd_DXReadWordData関数を使います。 | ||
+ | |||
+ | #ref(RobotHand_p7.png, 100%) | ||
+ | |||
+ | while文の中に | ||
+ | fd_DXReadWordData (1, 40, &PresentLoad); | ||
+ | と入力します。DynamixeのID=1、アドレス=40、読み込んだデータを入れる変数のアドレス=&PresentLoadです。変数のアドレスなのでPresentLoadの前に&を付けます。 | ||
+ | |||
+ | ---- | ||
+ | |||
+ | #ref(RobotHand_p8.png, 100%) | ||
+ | |||
+ | 読み込んだ負荷の値を表示します。 | ||
+ | fd_DXReadWordDataの後に | ||
+ | fd_printf("%6d\r",PresentLoad); | ||
+ | と入力します。負荷の値がどのような数値なのかまだ分かりませんので、桁数を多めにとります。 | ||
+ | |||
+ | ---- | ||
+ | |||
+ | 負荷の値がどのようなものなのか確認するために、コンパイルしてFDIII-HCに書き込み実行してみましょう。 | ||
+ | |||
+ | #ref(RobotHand_p9.png, 100%) | ||
+ | |||
+ | **AX-12+の負荷の値 [#a916853d] | ||
+ | 負荷の値を確認するために、手でロボットハンドを開く方向と閉じる方向へ力を加えてみましょう。 | ||
+ | |||
+ | #ref(RobotHand_p10.png, 100%) | ||
+ | |||
+ | 閉じる方向へ力を加えると、負荷の値は50や100といった値になります。開く方向へ力を加えると、少し力を加えただけで1050や1100といった値になります。開く方法へ力を加えた場合、何故急に大きな値になるのでしょうか。 | ||
+ | |||
+ | AX-12+の負荷の値は下表のように2バイト=16ビットで表されます。 | ||
+ | |||
+ | | ビット | 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となります。 | ||
+ | |||
+ | **手を少しづつ閉じる [#ea5de684] | ||
+ | ゴールポジションの値を少しづつ増やしながら、AX-12+に逐次書き込むことで、手を少しづつ閉じる動作を実現します。 | ||
+ | |||
+ | #ref(RobotHand_p11.png, 100%) | ||
+ | |||
+ | 先ずゴールポジションを保存する変数を宣言します。 | ||
+ | uint16_t PresentLoad;の下に | ||
+ | uint16_t GoalPositon = 300; | ||
+ | と入力します。 | ||
+ | GoalPositon = 300は、変数に予め300という値を入れておくという意味です。 | ||
+ | |||
+ | ---- | ||
+ | |||
+ | #ref(RobotHand_p12.png, 100%) | ||
+ | |||
+ | AX-12+にゴールポジションを書き込みます。 | ||
+ | fd_printf文の後に | ||
+ | fd_DXWriteWordData (1, 30, GoalPosition); | ||
+ | と入力します。 | ||
+ | |||
+ | ---- | ||
+ | |||
+ | #ref(RobotHand_p13.png, 100%) | ||
+ | |||
+ | ゴールぽシジョンの値を1増やします。 | ||
+ | fd_printf文の後に | ||
+ | GoalPosition++; | ||
+ | と入力します。GoalPositionに1足すという意味です。 | ||
+ | |||
+ | *負荷の値によって分岐 [#i046f47d] | ||
+ | |||
+ | 負荷の値が、ある閾値より小さい場合だけ、ゴールポジションの値に1足し、そのゴールポジションをAX-12+へ書き込みます。 | ||
+ | |||
+ | #ref(RobotHand_p14.png, 100%) | ||
+ | |||
+ | ゴールポジションに1足す行とゴールポジションを書き込む行をif文で囲みます。if文の条件は、「負荷の値が1124より下だったら」とします。手を閉じて物を挟んだ時、負荷は手が開く方向にかかります。よって、負荷の方向1024と負荷の値100を足した値の1124としました。 | ||
+ | |||
+ | if(PresentLoad < 1124) { | ||
+ | GoalPosition++; | ||
+ | fd_DXWriteWordData (1, 30, GoalPosition); | ||
+ | } | ||
+ | |||
+ | **動作確認 [#ea77a591] | ||
+ | #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; | ||
+ | } | ||
+ | } | ||
+ | |||
+ | プログラムが完成しました。プログラムを書き込んで動かしてみましょう。 | ||
+ | |||
+ | 優しく物を挟んでいるか確認しましょう。 | ||
+ | 万が一プログラムの誤入力で挟む力の制御がされなかった時のために、直ぐに電源を切れるよう準備しましょう。 | ||
+ | #ref(RobotHand_p15.png, 100%) | ||
+ | #html(<object width="480" height="385"><param name="movie" value="http://www.youtube.com/v/F8ZTZNc_tpk?fs=1&hl=ja_JP&rel=0"></param><param name="allowFullScreen" value="true"></param><param name="allowscriptaccess" value="always"></param><embed src="http://www.youtube.com/v/F8ZTZNc_tpk?fs=1&hl=ja_JP&rel=0" type="application/x-shockwave-flash" allowscriptaccess="always" allowfullscreen="true" width="480" height="385"></embed></object>) | ||
- | [[次のチャプターへ>FDIII-HC Starter Kit Guide Chaptert5.4]]~ | + | [[次のチャプターへ>FDIII-HC Starter Kit Guide Chapter5.4]]~ |
[[FDIII-HC Starter Kit Guide]]に戻る | [[FDIII-HC Starter Kit Guide]]に戻る |