9: 2013-08-01 (木) 19:17:51 yoshida  |
10: 2013-08-01 (木) 20:19:36 yoshida |
| | #include <math.h> | | #include <math.h> |
| | #include <fd.h> | | #include <fd.h> |
| - | double pos[3] = { 100.0, 150.0, 250.0 }; // 座標(x, y, z) | + | double pos[3] = { 80.0, 0, 50.0 }; // 座標(x, y, z) |
| - | const int L[7] = { 30, 38, 82, 44, 50 }; // 軸間の長さ | + | const int L[4] = { 42, 27, 68, 40 }; // 軸間の長さ |
| | | | |
| | void main (void) { | | void main (void) { |
| | + | fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP); |
| | + | DX_ChangeBaudrate (1000000); |
| | + | fd_Wait (2000); |
| | // 必要な辺の長さを求めます | | // 必要な辺の長さを求めます |
| | double D2D3 = L[2]; | | double D2D3 = L[2]; |
| - | double D3POS = L[3] + L[4]; | + | double D3POS = L[3]; |
| - | double D2POS = sqrt (pow(pos[0], 2) + pow(pos[1], 2) + pow(pos[2], 2)); | + | double D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + pos[2] * pos[2]); |
| - | double D2k = sqrt (pow(pos[0],2) + pow(pos[1],2) + pow(L[0]+L[1],2)); | + | double D2j = pos[2] - (L[0] + L[1]); |
| - | double kPOS = sqrt (pow(pos[3]-(L[0]+L[1]),2)); | + | double jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]); |
| | // コサイン値を求めます | | // コサイン値を求めます |
| - | double cosD3 = (pow(D2D3,2) + pow(D3POS,2) - pow(D2POS,2)) / (2*D2D3*D3POS); | + | double tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS); |
| - | double cosD2 = (pow(D2D3,2) + pow(D2POS,2) - pow(D3POS,2)) / (2*D2D3*D2POS); | + | double tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS); |
| | + | double cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS); |
| | // 角度を出します | | // 角度を出します |
| - | Theta1 = atan2 (pos[2], pos[1]); | + | radD1 = atan2 (pos[1], pos[0]); |
| - | Theta2 = | + | radD2 = acos(tmpcos2) - acos(tmpcos1); |
| | + | radD3 = acos(-1) - acos (cosD3); |
| | + | } |
| | </pre> | | </pre> |
| | }} | | }} |
ページへ戻る
