158 lines
3.1 KiB
C++
158 lines
3.1 KiB
C++
//#include "WProgram.h"
|
|
#include "InverseKinemetics.h"
|
|
|
|
void InverseKinemetics::RL3 (int i)
|
|
{
|
|
l3 = sqrt(pow(abs(COR_2_IK[0+(i*3)]), 2) + pow(abs(COR_2_IK[2+(i*3)]), 2));
|
|
}
|
|
|
|
float InverseKinemetics::ZX (int i)
|
|
{
|
|
RL3(i);
|
|
if (COR_2_IK[2+(i*3)] != 0)
|
|
float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3));
|
|
else
|
|
float temp = 90; // Math Error Protection.
|
|
return temp;
|
|
}
|
|
|
|
float InverseKinemetics::TT (int i)
|
|
{
|
|
int x = pow(abs(COR_2_IK[0+(i*3)]), 2);
|
|
int y = pow(abs(COR_2_IK[1+(i*3)]), 2);
|
|
int z = pow(abs(COR_2_IK[2+(i*3)]), 2);
|
|
s = sqrt(x + y + z);
|
|
|
|
if (s > (l1 + l2) )
|
|
R[i] = 3;
|
|
|
|
float t1 = degrees(acos((pow(s, 2) + pow(COR_2_IK[1+(i*3)], 2) - pow(l3, 2))
|
|
/ (2 * s * COR_2_IK[1+(i*3)])));
|
|
|
|
float t2 = degrees(acos((pow(s, 2) + pow(l1, 2) - pow(l2, 2))
|
|
/ (2 * s * l1)));
|
|
|
|
float temp = (int)t1 + t2;
|
|
//Serial.print("DEBUG: "); Serial.print(t1); Serial.print(", "); Serial.print(t2); Serial.print(", "); Serial.println(temp);
|
|
|
|
return temp;
|
|
}
|
|
|
|
int InverseKinemetics::LL ()
|
|
{
|
|
int temp = degrees(acos((float)(pow(l1, 2) + pow(l2, 2) - pow(s, 2)) / (2 * l1 * l2)));
|
|
return temp;
|
|
}
|
|
|
|
void InverseKinemetics::setleg (int i)
|
|
{
|
|
Serial.print("Set Leg ");
|
|
Serial.println(i);
|
|
if (i >= 3)
|
|
{
|
|
//Error_Coordinate(i);
|
|
DEG_2_CON[0+(i*3)] = 180 - ZX(i);
|
|
DEG_2_CON[1+(i*3)] = 180 - TT(i);
|
|
DEG_2_CON[2+(i*3)] = LL();
|
|
Error_Angle(i);
|
|
}
|
|
else
|
|
{
|
|
//Error_Coordinate(i);
|
|
DEG_2_CON[0+(i*3)] = ZX(i);
|
|
DEG_2_CON[1+(i*3)] = TT(i);
|
|
DEG_2_CON[2+(i*3)] = 180 - LL();
|
|
Error_Angle(i);
|
|
}
|
|
}
|
|
|
|
void InverseKinemetics::setlegs ()
|
|
{
|
|
for (int i=0; i<6; i++)
|
|
{
|
|
setleg(i);
|
|
}
|
|
for (int i=0; i<6; i++)
|
|
{
|
|
if (i != 0)
|
|
Serial.print(", ");
|
|
else
|
|
Serial.print("Cor: ");
|
|
Serial.print(COR_2_IK[0+(i*3)]);
|
|
Serial.print(", ");
|
|
Serial.print(COR_2_IK[1+(i*3)]);
|
|
Serial.print(", ");
|
|
Serial.print(COR_2_IK[2+(i*3)]);
|
|
}
|
|
Serial.println(";");
|
|
}
|
|
|
|
void InverseKinemetics::Error_Angle (int i)
|
|
{
|
|
for (int i2=0; i2<3; i2++)
|
|
R[i2+(i*3)] = 0;
|
|
if (DEG_2_CON[0+(i*3)] < 0)
|
|
R[0+(i*3)] = 1;
|
|
if (DEG_2_CON[0+(i*3)] > 180)
|
|
R[0+(i*3)] = 2;
|
|
if (DEG_2_CON[1+(i*3)] < 0)
|
|
R[1+(i*3)] = 1;
|
|
if (DEG_2_CON[1+(i*3)] > 180)
|
|
R[1+(i*3)] = 2;
|
|
if (DEG_2_CON[2+(i*3)] < 0)
|
|
R[2+(i*3)] = 1;
|
|
if (DEG_2_CON[2+(i*3)] > 180)
|
|
R[2+(i*3)] = 2;
|
|
}
|
|
|
|
void InverseKinemetics::Error_Coordinate (int i)
|
|
{
|
|
Ra[i] = 0;
|
|
float temp = sqrt(pow(COR_2_IK[0+(i*3)], 2) + pow(COR_2_IK[1+(i*3)], 2) + pow(COR_2_IK[2+(i*3)], 2));
|
|
if (temp <= 5) {
|
|
Ra[i] = 1;
|
|
}
|
|
if (temp > l1 + l2) {
|
|
Ra[i] = 2;
|
|
}
|
|
}
|
|
|
|
int InverseKinemetics::CheckForErrorCor()
|
|
{
|
|
for (int i=1; i<19; i++)
|
|
if (R[i-1]!=0)
|
|
return i;
|
|
return 0;
|
|
}
|
|
|
|
String InverseKinemetics::CheckForErrorCor_print()
|
|
{
|
|
String temp = "Error Coordinate: ";
|
|
for (int i=0; i<18; i++)
|
|
{
|
|
temp = temp + (int)DEG_2_CON[i] + ", ";
|
|
}
|
|
temp = temp + "\nCheckForErrorCor returnd: " + CheckForErrorCor();
|
|
return temp;
|
|
}
|
|
|
|
int InverseKinemetics::CheckForErrorAng()
|
|
{
|
|
for (int i=0; i<6; i++)
|
|
if (Ra[i]!=0)
|
|
return i;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|