Hell,
This might be a big vague, but I hope that someone might be able to help me.
I have a program that does virtual simulation of robotic arm using inverse kinematics. The program works fine but right now I'm trying to collect some data, generated inside the program, into an external file for later manipulation (I'm going to train an artificial neural network). The problem that I'm having is that, for some reason, I'm only able to collect subset of generated data, meaning that not all of the data generated inside the program is passed into the external file. I'm also not quite sure about how to determine when the data collection process is stopped. Thanks.
r.
void inverseKinematics()
{
FILE * pFile;
pFile = fopen ("myfile.txt","a");
double Px, Py, Pz;
Px = P[0], Py = P[1], Pz = P[2];
printf("Target Position: x=%6.2f y=%6.2f z=%6.2f \n", Px, Py, Pz);
printf("This is a trick!!!\n");
double tmpL = sqrt(Px * Px + Py * Py);
double P1P = sqrt(Px * Px + Py * Py
+ (Pz - (l[0] + l[1])) * (Pz - (l[0] + l[1])));
double Ca = (l[2] * l[2] + P1P * P1P -l[3] * l[3])/(2 * l[2] * P1P);
double phi = atan2(Pz - (l[0] + l[1]), tmpL);
double alpha = atan2(sqrt(1 - Ca * Ca), Ca);
double Cb = (l[2]*l[2] + l[3]*l[3] - P1P*P1P)/(2 * l[2] * l[3]);
double beta = atan2(sqrt(1- Cb * Cb), Cb);
switch (ANSWER) {
case 1:
THETA[1] = atan2(Py, Px);
THETA[2] = M_PI/2 - phi - alpha;
THETA[3] = M_PI - beta; break;
case 2:
THETA[1] = atan2(Py, Px);
THETA[2] = M_PI/2 - phi + alpha;
THETA[3] = M_PI + beta; break;
case 3:
THETA[1] = atan2(Py, Px) + M_PI;
THETA[2] = -(M_PI/2 - phi - alpha);
THETA[3] = M_PI + beta; break;
case 4:
THETA[1] = atan2(Py, Px) + M_PI;
THETA[2] = -(M_PI/2 - phi + alpha);
THETA[3] = M_PI - beta; break;
}
printf("Arm Angles: theta1=%6.2f theta2=%6.2f theta3=%6.2f \n", THETA[1], THETA[2], THETA[3]);
fprintf (pFile, "theta1: %f",THETA[1]);
fprintf (pFile, "theta2: %f",THETA[2]);
fprintf (pFile, "theta3: %f \n",THETA[3]);
fclose (pFile);
}