// joint position {x,y,z]
int TJ[][3] = {{ -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}}; //posiotion relative to rotation
int TJO[][3] = {{ -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}}; //posiotion relative to origin
int POR[] = {0, 0, 0}; // point of rotaion
int New_TJ[6][3];
int BJ[][3] = {{ -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}, { -3, 25, 0}};
double A[6][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double X, Y, Z;
int i = 5;
int j = 0;
int roll, pitch, yaw;
int L[6];
void setup() {
// put your setup code here, to run once:
Serial.begin(57600);
delay(1500);
Serial.println("Hello");
delay(500);
//while(j<6){
//// find relative location to POR
//TJ[i][0]= POR[0] - TJO[i][0];
//TJ[i][1]= POR[1] - TJO[i][1];
//TJ[i][2]= POR[2] - TJO[i][2];
//delay(500);
//Serial.println("h");
//Serial.println(j);
//j++;
//}
//
//for (int i=0; i<=6; i++){
//// find angle in plane to joint from point of rotation
// A[i][0] = atan(TJ[i][0]/TJ[i][2]); // roll plane angle
// A[i][1] = atan(TJ[i][1]/TJ[i][2]); // pitch plane angle
// A[i][2] = atan(TJ[i][0]/TJ[i][1]); // yaw plane angle
//}
//void loop();
}
void loop() {
// put your main code here, to run repeatedly:
//calc new top position relative to POR
for(int i=0; i<=6; i++){
// roll x,z change //cos(angle of rotation + angle to that point from point of rotation in that plane)
New_TJ[i][0] = cos(roll + A[i][0]) * (TJ[i][0]); //X
New_TJ[i][2] = sin(roll + A[i][2]) * (TJ[i][2]); //Z
// pitch y,z change
New_TJ[i][1] = cos(pitch + A[i][1]) * (TJ[i][1]); //Y
New_TJ[i][2] = sin(pitch + A[i][2]) * (New_TJ[i][2]); //Z
// yaw x,y change
New_TJ[i][0] = sin(yaw + A[i][0]) * (New_TJ[i][0]); //X
New_TJ[i][1] = cos(yaw + A[i][1]) * (New_TJ[i][1]); //Y
delay(5);
Serial.println(i);
Serial.println("Complete");
delay(5);
}
delay(5);
Serial.println("found location");
//calc length of joint****************************
for (int i = 0; i <= 6; i++) {
X = New_TJ[i][0] - BJ[i][0] + POR[0];
Y = New_TJ[i][1] - BJ[i][1] + POR[0];
Z = New_TJ[i][2] - BJ[i][2] + POR[0];
L[i] = sqrt((X * X) + (Y * Y) + (Z * Z));
}
//*************************************************
delay(5);
Serial.println("Complete");
Serial.println(L[0]);
Serial.println(L[1]);
Serial.println(L[2]);
Serial.println(L[3]);
Serial.println(L[4]);
Serial.println(L[5]);
}