5: 2013-08-06 (火) 20:21:18 yoshida  |
現: 2013-08-07 (水) 20:32:08 yoshida |
| | { 1, DEV_AX12, fd_AxisOfs(0), +341, { 0, 1023}, 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, { 0, 1023}, 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, { 0, 1023}, 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] = { 40, 28, 68, 40 }; | + | const int L[4] = { 42, 27, 83, 55 }; |
| | TApplyPart HandParts = { // 3DOFロボットハンド | | TApplyPart HandParts = { // 3DOFロボットハンド |
| | Priority: 1, | | Priority: 1, |
| | double Degree (double rad) { | | double Degree (double rad) { |
| | return (rad / (atan(1) * 4)) * 180.0; | | return (rad / (atan(1) * 4)) * 180.0; |
| | + | } |
| | + | |
| | + | // プッシュボタンが押下されるとプログラムが終了します |
| | + | void USER_TASK1 (void) { |
| | + | while (!fd_GetPB ()) { |
| | + | fd_Wait (10); |
| | + | } |
| | + | fd_SoftReset (); |
| | } | | } |
| | | | |
| | void main (void) { | | void main (void) { |
| | double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3; | | double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3; |
| | + | double pos[3]; |
| | + | int in[3], i; |
| | | | |
| | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); | | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); |
| | | | |
| | fd_Wait (2000); | | fd_Wait (2000); |
| | + | |
| | + | act_tsk (TASK1); |
| | | | |
| | D2D3 = L[2]; | | D2D3 = L[2]; |
| | D3POS = L[3]; | | D3POS = L[3]; |
| - | D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + (pos[2] - (L[0]+L[1])) * (pos[2] - (L[0]+L[1]))); | + | while (1) { |
| - | D2j = pos[2] - (L[0] + L[1]); | + | fd_puts ("\nset position (x,y,z) = "); |
| - | jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); | + | fd_scanf ("%d,%d,%d", &in[0], &in[1], &in[2]); // 座標の指示 |
| - | tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); | + | for (i = 0; i < 3; i++) pos[i] = (double)in[i]; |
| - | tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); | + | D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + (pos[2] - (L[0]+L[1])) * (pos[2] - (L[0]+L[1]))); |
| - | cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); | + | D2j = pos[2] - (L[0] + L[1]); |
| - | radD1 = atan2 (pos[1], pos[0]); | + | jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); |
| - | radD2 = acos(tmpcos2) - acos(tmpcos1); | + | tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); |
| - | radD3 = acos(-1) - acos (cosD3); | + | tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); |
| - | WorkMotion.Structure[0] = (int)(Degrees(radD1) * 10); // D1の角度 | + | cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); |
| - | WorkMotion.Structure[1] = (int)(Degrees(radD2) * 10); // D2の角度 | + | radD1 = atan2 (pos[1], pos[0]); |
| - | WorkMotion.Structure[2] = (int)(Degrees(radD3) * 10); // D3の角度 | + | radD2 = acos(tmpcos2) - acos(tmpcos1); |
| - | fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts); // モーションの再生 | + | radD3 = acos(-1) - acos (cosD3); |
| - | while (fd_GetMotionStat (&HandParts)) fd_Wait (10); | + | WorkMotion.Structure[0] = (int)(Degree(radD1) * 10); // D1の角度 |
| | + | WorkMotion.Structure[1] = (int)(Degree(radD2) * 10); // D2の角度 |
| | + | WorkMotion.Structure[2] = (int)(Degree(radD3) * 10); // D3の角度 |
| | + | fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts); // モーションの再生 |
| | + | while (fd_GetMotionStat (&HandParts)) fd_Wait (10); |
| | + | } |
| | } | | } |
| | </pre> | | </pre> |
| | }} | | }} |
| | + | このプログラムはSIMPLE TERM(ターミナルプログラム)から座標を指定できるようになっており、プッシュボタンが押されると終了してしまいます。 |
| | | | |
| | ** まとめ [#o814be71] | | ** まとめ [#o814be71] |
| - | Chapter10ではFREEDOM III libraryに用意された関数を使用してモーションを動かしてみました。~ | + | 本章ではFREEDOM III libraryに用意された関数を使用してモーションでロボットハンドを動かしてみました。~ |
| | 又、指定された座標から角度を算出しました。今回は一つの考え方を説明しましたが、IKによる計算方法は様々な手法が考えられており、ほとんどが専門の知識を必要とします。これを機に調べてみるのも良いかも知れません。 | | 又、指定された座標から角度を算出しました。今回は一つの考え方を説明しましたが、IKによる計算方法は様々な手法が考えられており、ほとんどが専門の知識を必要とします。これを機に調べてみるのも良いかも知れません。 |
| | | | |
| | ** 課題 [#xf0a3d27] | | ** 課題 [#xf0a3d27] |
| - | 範囲外の座標が指示された場合、プログラムの途中で計算が破綻をきたします。これを回避するよう、プログラムを修正して下さい。~ | + | 指定された座標によってはプログラムの途中で計算が破綻をきたし、不正な角度を設定してしまいます。これを回避するよう、プログラムを修正して下さい。~ |
| | ヒントは計算の破綻は決まった関数(acos/atan2)で起こります。~ | | ヒントは計算の破綻は決まった関数(acos/atan2)で起こります。~ |
| | 又、取り外したハンド部分のAX-12+を分解してアーム部分を延長することで、産業用のロボットアームと同様の構成にすることができます。~ | | 又、取り外したハンド部分のAX-12+を分解してアーム部分を延長することで、産業用のロボットアームと同様の構成にすることができます。~ |
ページへ戻る
