public static Theta calculateServoAngles(double x, double y, double z)
{
Theta theta = new Theta();
if (z > (MATH_L1 + MATH_L3 + TopOffset))
{
z = MATH_L1 + MATH_L3 + TopOffset;
}
if (z < (MATH_L1 - MATH_L4 + BottomOffset))
{
z = MATH_L1 - MATH_L4 + BottomOffset;
}
g_y_in = (-y - MATH_L2) / MATH_L3;
g_z_in = (z - MATH_L1) / MATH_L3;
g_l43 = MATH_L4 / MATH_L3;
g_right_all = (1 - g_y_in * g_y_in - g_z_in * g_z_in - g_l43 * g_l43) / (2 * g_l43);
g_sqrt_z_y = Math.Sqrt(g_z_in * g_z_in + g_y_in * g_y_in);
if (x == 0)
{
// Calculate value of theta 1
theta.theta_1 = 90;
// Calculate value of theta 3
if (g_z_in == 0)
{
g_phi = 90;
}
else
{
g_phi = Math.Atan(-g_y_in / g_z_in) * MATH_TRANS;
}
if (g_phi > 0)
g_phi = g_phi - 180;
theta.theta_3 = Math.Asin(g_right_all / g_sqrt_z_y) * MATH_TRANS - g_phi;
if (theta.theta_3 < 0)
{
theta.theta_3 = 0;
}
// Calculate value of theta 2
theta.theta_2 = Math.Asin((z + MATH_L4 * Math.Sin(theta.theta_3 / MATH_TRANS) - MATH_L1) / MATH_L3) * MATH_TRANS;
}
else
{
// Calculate value of theta 1
theta.theta_1 = Math.Atan(y / x) * MATH_TRANS;
if (y / x > 0)
{
theta.theta_1 = theta.theta_1 + 0;
}
if (y / x < 0)
{
theta.theta_1 = theta.theta_1 + 180;
}
if (y == 0)
{
if (x > 0)
theta.theta_1 = 180;
else
theta.theta_1 = 0;
}
// Calculate value of theta 3
g_x_in = (-x / Math.Cos(theta.theta_1 / MATH_TRANS) - MATH_L2) / MATH_L3;
if (g_z_in == 0)
{
g_phi = 90;
}
else
{
g_phi = Math.Atan(-g_x_in / g_z_in) * MATH_TRANS;
}
if (g_phi > 0)
g_phi = g_phi - 180;
g_sqrt_z_x = Math.Sqrt(g_z_in * g_z_in + g_x_in * g_x_in);
g_right_all_2 = -1 * (g_z_in * g_z_in + g_x_in * g_x_in + g_l43 * g_l43 - 1) / (2 * g_l43);
theta.theta_3 = Math.Asin(g_right_all_2 / g_sqrt_z_x) * MATH_TRANS;
theta.theta_3 = theta.theta_3 - g_phi;
if (theta.theta_3 < 0)
{
theta.theta_3 = 0;
}
// Calculate value of theta 2
theta.theta_2 = Math.Asin(g_z_in + g_l43 * Math.Sin(Math.Abs(theta.theta_3 / MATH_TRANS))) * MATH_TRANS;
}
theta.theta_1 = Math.Abs(theta.theta_1);
theta.theta_2 = Math.Abs(theta.theta_2);
if (theta.theta_3 < 0)
{
}
else
{
if ((calYonly(theta.theta_1, theta.theta_2, theta.theta_3) > y + 0.1) || (calYonly(theta.theta_1, theta.theta_2, theta.theta_3) < y - 0.1))
{
theta.theta_2 = 180 - theta.theta_2;
}
}
theta.theta_3 = Math.Abs(theta.theta_3);
return theta;
}
public void MoveTo(double x, double y, double z,int time)
{
attachAll();
Theta theta = new Theta();
double[] x_arr = new double[50];
double[] y_arr = new double[50];
double[] z_arr = new double[50];
double current_x = findX();
double current_y = findY();
double current_z = findZ();
Vector vector = calXYZ();
double _x = vector.x + x;
double _y = vector.y + y;
double _z = vector.z + z;
x_arr = ActionControl.interpolation(current_x, x);
y_arr = ActionControl.interpolation(current_y, y);
z_arr = ActionControl.interpolation(current_z, z);
if (time < 0) time = 0;
for (int i = 0; i < 50; i++)
{
Theta t = ActionControl.calculateServoAngles(x_arr[i], y_arr[i], z_arr[i]);
writeAngles(t.theta_1, t.theta_2, t.theta_3, 0);
Thread.Sleep(time*20);
}
}
private Theta AdjustAngle(Theta theta)
{
theta.theta_1 = Math.Round(theta.theta_1 + servoRotOffset);
theta.theta_2 = Math.Round(theta.theta_2 + servoLeftOffset);
theta.theta_3 = Math.Round(theta.theta_3 + servoRightOffset);
return theta;
}