10: 2013-08-01 (木) 20:19:36 yoshida  |
11: 2013-08-02 (金) 13:14:30 yoshida |
| | | | |
| | **プログラミング [#bdefef6a] | | **プログラミング [#bdefef6a] |
| - | ではプログラムしてみましょう。~ | + | ではプログラムしてみましょう。 |
| | #html{{ | | #html{{ |
| | <pre class="brush:c;toolbar:false"> | | <pre class="brush:c;toolbar:false"> |
| | double pos[3] = { 80.0, 0, 50.0 }; // 座標(x, y, z) | | double pos[3] = { 80.0, 0, 50.0 }; // 座標(x, y, z) |
| | const int L[4] = { 42, 27, 68, 40 }; // 軸間の長さ | | const int L[4] = { 42, 27, 68, 40 }; // 軸間の長さ |
| | + | |
| | + | // RadianからDegreeへ変換 |
| | + | double Degree (double rad) { |
| | + | return (rad / (atan(1) * 4)) * 180.0; |
| | + | } |
| | + | // RadianからDynamixelの位置へ変換 |
| | + | uint16_t Position (double rad) { |
| | + | return 512 + Degree(rad) * 3.41; |
| | + | } |
| | | | |
| | void main (void) { | | 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_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); |
| | DX_ChangeBaudrate (1000000); | | DX_ChangeBaudrate (1000000); |
| | fd_Wait (2000); | | fd_Wait (2000); |
| | // 必要な辺の長さを求めます | | // 必要な辺の長さを求めます |
| - | double D2D3 = L[2]; | + | D2D3 = L[2]; |
| - | double D3POS = L[3]; | + | D3POS = L[3]; |
| - | double D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + pos[2] * pos[2]); | + | D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + pos[2] * pos[2]); |
| - | double D2j = pos[2] - (L[0] + L[1]); | + | D2j = pos[2] - (L[0] + L[1]); |
| - | double jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); | + | jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); |
| | // コサイン値を求めます | | // コサイン値を求めます |
| - | double tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); | + | tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); |
| - | double tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); | + | tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); |
| - | double cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); | + | cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); |
| | // 角度を出します | | // 角度を出します |
| | radD1 = atan2 (pos[1], pos[0]); | | radD1 = atan2 (pos[1], pos[0]); |
| | radD2 = acos(tmpcos2) - acos(tmpcos1); | | radD2 = acos(tmpcos2) - acos(tmpcos1); |
| | radD3 = acos(-1) - acos (cosD3); | | radD3 = acos(-1) - acos (cosD3); |
| | + | // AX-12+の出力軸の位置指定 |
| | + | fd_DXSetPosition (1, Position(radD1)); |
| | + | fd_DXSetPosition (2, Position(radD2)); |
| | + | fd_DXSetPosition (3, Position(radD3)); |
| | } | | } |
| | </pre> | | </pre> |
| | }} | | }} |
| | + | 座標から角度を算出してアーム部分を動かすことができるようになりましたが、稼働部分、及び軸間の短さから指定できる座標は非常に狭い範囲になります。~ |
| | + | 範囲外の座標が指示された場合は途中で計算が破綻をきたしますので、acosの前に値のチェックを入れることで回避することができます。~ |
| | + | 又、アームを構成しているパーツによっては、指示されても移動できない出力軸の位置ができてしまいます。この場合はAX-12+への位置指定の前にチェックを入れて位置を指定しないようにしましょう。位置を指定してしまった場合にはFDIII-HCのスイッチをOFFして下さい。過度の負荷は故障の原因となります。~ |
| | + | |
| | + | |
| | + | [[次のチャプターへ>FDIIICHAPTER10.5]] |
| | + | |
| | + | [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る |
ページへ戻る
