9: 2013-08-01 (木) 19:17:51 yoshida | 10: 2013-08-01 (木) 20:19:36 yoshida | ||
---|---|---|---|
Line 40: | Line 40: | ||
#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> | ||
}} | }} |