1: 2013-08-02 (金) 17:24:32 yoshida | 2: 2013-08-02 (金) 19:00:59 yoshida | ||
---|---|---|---|
Line 1: | Line 1: | ||
- | TITLE:ロボットハンドまとめ | + | TITLE:ロボットハンドプログラムの修正 |
- | ** まとめ [#r2464685] | + | ** プログラムの修正 [#w9305379] |
- | FREEDOM III libraryにはモーションに関する関数が用意されています。 | + | #html{{ |
- | モーションを実行する | + | <pre class="brush:c;toolbar:false"> |
- | fd_PlayMotion(ポーズ, 開始位置, 終了位置, 再生スピード, モーション適用部位); | + | #include <math.h> |
- | モーションが再生中かどうか判断する | + | #include <fd.h> |
- | fd_GetMotionStat(モーション適用部位); | + | #define fd_SetSpec(a,b,c) fd_SetSpec((PHomePosition)a,(PSpec)b,c) |
- | 諸元を設定する | + | #define fd_PlayMotion(a,b,c,d,e) fd_PlayMotion((PPose)a,b,c,d,e) |
- | fd_SetSpec(ホームポジション, 諸元, 諸元サイズ); | + | |
- | ** 課題 [#k0135b7a] | + | const THomePosition HomePos = { 512, 512, 512 }; |
+ | const TSpec Spec[] = { | ||
+ | { 1, DEV_AX12, fd_AxisOfs(0), +341, { 257, 768}, 0, 1023, { 1, 1, 32, 32} }, | ||
+ | { 2, DEV_AX12, fd_AxisOfs(1), +341, { 390, 818}, 0, 1023, { 1, 1, 32, 32} }, | ||
+ | { 3, DEV_AX12, fd_AxisOfs(2), +341, { 64, 512}, 0, 1023, { 1, 1, 32, 32} }, | ||
+ | }; | ||
+ | const int L[4] = { 42, 27, 68, 40 }; | ||
+ | TApplyPart HandParts = { // 3DOFロボットハンド | ||
+ | Priority: 1, | ||
+ | PartNum: 3, | ||
+ | Part: { fd_AxisOfs(0), fd_AxisOfs(1), fd_AxisOfs(2) } | ||
+ | }; | ||
+ | // * モーションデータ | ||
+ | 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, }, | ||
+ | }; | ||
+ | |||
+ | double Degree (double rad) { | ||
+ | return (rad / (atan(1) * 4)) * 180.0; | ||
+ | } | ||
+ | |||
+ | void main (void) { | ||
+ | double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3; | ||
+ | |||
+ | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); | ||
+ | fd_SetUVThreshold (7.4); | ||
+ | DX_ChangeBaudrate (1000000); | ||
+ | fd_Wait (2000); | ||
+ | |||
+ | D2D3 = L[2]; | ||
+ | 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]))); | ||
+ | D2j = pos[2] - (L[0] + L[1]); | ||
+ | jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); | ||
+ | tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); | ||
+ | tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); | ||
+ | cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); | ||
+ | radD1 = atan2 (pos[1], pos[0]); | ||
+ | radD2 = acos(tmpcos2) - acos(tmpcos1); | ||
+ | radD3 = acos(-1) - acos (cosD3); | ||
+ | WorkMotion[1]. | ||
+ | } | ||
+ | </pre> | ||
+ | }} |