2: 2013-08-02 (金) 19:00:59 yoshida  |
3: 2013-08-02 (金) 20:27:27 yoshida |
| | TITLE:ロボットハンドプログラムの修正 | | TITLE:ロボットハンドプログラムの修正 |
| | ** プログラムの修正 [#w9305379] | | ** プログラムの修正 [#w9305379] |
| | + | IKで求めた角度をモーションに組み込みます。 |
| | #html{{ | | #html{{ |
| | <pre class="brush:c;toolbar:false"> | | <pre class="brush:c;toolbar:false"> |
| | const THomePosition HomePos = { 512, 512, 512 }; | | const THomePosition HomePos = { 512, 512, 512 }; |
| | const TSpec Spec[] = { | | const TSpec Spec[] = { |
| - | { 1, DEV_AX12, fd_AxisOfs(0), +341, { 257, 768}, 0, 1023, { 1, 1, 32, 32} }, | + | { 1, DEV_AX12, fd_AxisOfs(0), +341, { 0, 1023}, 0, 1023, { 1, 1, 32, 32} }, |
| - | { 2, DEV_AX12, fd_AxisOfs(1), +341, { 390, 818}, 0, 1023, { 1, 1, 32, 32} }, | + | { 2, DEV_AX12, fd_AxisOfs(1), +341, { 0, 1023}, 0, 1023, { 1, 1, 32, 32} }, |
| - | { 3, DEV_AX12, fd_AxisOfs(2), +341, { 64, 512}, 0, 1023, { 1, 1, 32, 32} }, | + | { 3, DEV_AX12, fd_AxisOfs(2), +341, { 0, 1023}, 0, 1023, { 1, 1, 32, 32} }, |
| | }; | | }; |
| - | const int L[4] = { 42, 27, 68, 40 }; | + | const int L[4] = { 40, 28, 68, 40 }; |
| | TApplyPart HandParts = { // 3DOFロボットハンド | | TApplyPart HandParts = { // 3DOFロボットハンド |
| | Priority: 1, | | Priority: 1, |
| | }; | | }; |
| | // * モーションデータ | | // * モーションデータ |
| - | TPose WorkMotion [] = { | + | TPose WorkMotion = { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, }; |
| - | { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, }, | + | |
| - | { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, }, | + | |
| - | { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, }, | + | |
| - | }; | + | |
| | | | |
| | double Degree (double rad) { | | double Degree (double rad) { |
| | fd_SetUVThreshold (7.4); | | fd_SetUVThreshold (7.4); |
| | DX_ChangeBaudrate (1000000); | | DX_ChangeBaudrate (1000000); |
| | + | |
| | + | fd_SetSpec (&HomePos, Spec, fd_SpecSize (Spec)); |
| | + | fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts); |
| | + | |
| | fd_Wait (2000); | | fd_Wait (2000); |
| | | | |
| | radD2 = acos(tmpcos2) - acos(tmpcos1); | | radD2 = acos(tmpcos2) - acos(tmpcos1); |
| | radD3 = acos(-1) - acos (cosD3); | | radD3 = acos(-1) - acos (cosD3); |
| - | WorkMotion[1]. | + | WorkMotion.Structure[0] = (int)(Degrees(radD1) * 10); |
| | + | WorkMotion.Structure[1] = (int)(Degrees(radD2) * 10); |
| | + | WorkMotion.Structure[2] = (int)(Degrees(radD3) * 10); |
| | + | fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts); |
| | + | while (fd_GetMotionStat (&HandParts)) fd_Wait (10); |
| | } | | } |
| | </pre> | | </pre> |
| | }} | | }} |
| | + | |
| | + | ** まとめ [#o814be71] |
| | + | 座標から角度を算出してモーションでアーム部分を動かすことができるようになりましたが、稼働部分、及び軸間の短さから指定できる座標は狭い範囲となります。~ |
| | + | 範囲外の座標が指示された場合は途中で計算が破綻をきたしますが、acosの前に値のチェックを入れることで回避することができます。又、atan2はx、y共に0の場合エラーとなりますので、こちらも事前にチェックを行っておきましょう。~ |
| | + | さて、IKで角度を算出することはできましたが、実際にモーションとして使用するには移動にかかる時間や軌道の計算、速度の調節等を行わなければなりません。自前で用意するには大変なので、今回はFREEDOM III libraryに用意された関数を使用しました。 |
| | + | |
| | + | ** 課題 [#xf0a3d27] |
| | + | 取り外したハンド部分のAX-12+を分解してアーム部分を延長してみましょう。~ |
| | + | 産業用のロボットアームに近い構成にすることができます。~ |
| | + | |
| | + | [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る |
ページへ戻る
