bool check_temp = true;

double angle, i_roll, i_pitch, i_yaw;

j_inter = 0;
try
{
double temp1 = double.Parse(tb3circularang.Text);
}
catch
{
check_temp = false;
MessageBox.Show("Data type is wrong. Please check again", "Error",
MessageBoxButtons.OK, MessageBoxIcon.Error);
}
angle = double.Parse(tb3circularang.Text);
if (angle > 5 && angle < -5)
{
check_temp = false;
MessageBox.Show("The range of Angle are [-5,5]. Please check again",
"Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
if (check_servoon && check_temp)
{
for (double j = 0; j < (2 * Math.PI); j += 0.005)
{
i_roll = angle * Math.Cos(j);
i_pitch = angle * Math.Sin(j);
i_yaw = angle * Math.Sin(j);

kine.I_PX = 0;
kine.I_PY = 0;
kine.I_PZ = 0;
kine.I_ROLL = i_roll;
kine.I_PITCH = i_pitch;
kine.I_YAW = i_yaw;
kine.kinematic_trans();
for (int i = 0; i < 6; i++)
{
pos_axis[i] = (kine.length[i] * 20000) / 2.54;
}
interpol1[j_inter] = Math.Round(pos_axis[0]);
interpol2[j_inter] = Math.Round(pos_axis[1]);
interpol3[j_inter] = Math.Round(pos_axis[2]);
interpol4[j_inter] = Math.Round(pos_axis[3]);
interpol5[j_inter] = Math.Round(pos_axis[4]);
interpol6[j_inter] = Math.Round(pos_axis[5]);
j_inter++;
}
for (double j = 0; j < (4 * Math.PI); j += 0.01)
{
i_roll = angle * Math.Cos(j);
i_pitch = angle * Math.Sin(j);
i_yaw = angle * Math.Sin(j);
 kine.I_PX = 0;
kine.I_PY = 0;
kine.I_PZ = 0;
kine.I_ROLL = i_roll;
kine.I_PITCH = i_pitch;
kine.I_YAW = i_yaw;
kine.kinematic_trans();
for (int i = 0; i < 6; i++)
{
pos_axis[i] = (kine.length[i] * 20000) / 2.54;
}
interpol1[j_inter] = Math.Round(pos_axis[0]);
interpol2[j_inter] = Math.Round(pos_axis[1]);
interpol3[j_inter] = Math.Round(pos_axis[2]);
interpol4[j_inter] = Math.Round(pos_axis[3]);
interpol5[j_inter] = Math.Round(pos_axis[4]);
interpol6[j_inter] = Math.Round(pos_axis[5]);
j_inter++;
}
for (double j = 0; j < (8 * Math.PI); j += 0.02)
{
i_roll = angle * Math.Cos(j);
i_pitch = angle * Math.Sin(j);
i_yaw = angle * Math.Sin(j);

kine.I_PX = 0;
kine.I_PY = 0;
kine.I_PZ = 0;
kine.I_ROLL = i_roll;
kine.I_PITCH = i_pitch;
kine.I_YAW = i_yaw;
kine.kinematic_trans();
for (int i = 0; i < 6; i++)
{
pos_axis[i] = (kine.length[i] * 20000) / 2.54;
}
interpol1[j_inter] = Math.Round(pos_axis[0]);
interpol2[j_inter] = Math.Round(pos_axis[1]);
interpol3[j_inter] = Math.Round(pos_axis[2]);
interpol4[j_inter] = Math.Round(pos_axis[3]);
interpol5[j_inter] = Math.Round(pos_axis[4]);
interpol6[j_inter] = Math.Round(pos_axis[5]);
j_inter++;
}

}
vel_ang_combined = 30000;
bt3circularmedium.Enabled = false;
timer2.Start();
}
If (j_inter < 1256)
{
vel_ang_combined = 20000;

Else if (j_inter>= 1256 && j_inter < 2510)

{
vel_ang_combined = 30000;

Else

{
vel_ang_combined = 40000;