void Robot_op()
{
try
{
int[] ctheta = new int[4];
byte[] btheta = new byte[4];
Robot_arm robot = new Robot_arm(150, 150, 40, orient);
robot.inverse_kinematic(tarX, tarY, tarZ);
txt_T0.Text = Convert.ToString(robot.itheta[0]);
txt_T1.Text = Convert.ToString(robot.itheta[1]);
txt_T2.Text = Convert.ToString(robot.itheta[2]);
txt_T3.Text = Convert.ToString(robot.itheta[3]);
for (int j = 0; j < 4; j++)
{
ctheta[j] = Convert.ToInt32(robot.itheta[j]);
}
robot.forward_kinematic(ctheta);
txtX.Text = Convert.ToString(robot.x);
txtY.Text = Convert.ToString(robot.y);
txtZ.Text = Convert.ToString(robot.z);
if (serial)
{
for (int j = 0; j < 4; j++)
{
if (robot.itheta[j] < 0)
{
robot.itheta[j] = robot.itheta[j] + 90;
}
btheta[j] = Convert.ToByte(robot.itheta[j]);
}
serialPort1.Write(btheta, 0, 4);
}
}
catch (Exception e)
{
string message = e.ToString();
string title = "Error";
MessageBox.Show(message, title);
}
}
void angle_con()
{
int ef;
ef = angle[3] + angle[2] + angle[1];
Robot_arm robot = new Robot_arm(150, 150, 40, ef);
robot.forward_kinematic(angle);
txtX.Text = Convert.ToString(robot.x);
txtY.Text = Convert.ToString(robot.y);
txtZ.Text = Convert.ToString(robot.z);
robot.inverse_kinematic(robot.x, robot.y, robot.z);
txt_T0.Text = Convert.ToString(robot.itheta[0]);
txt_T1.Text = Convert.ToString(robot.itheta[1]);
txt_T2.Text = Convert.ToString(robot.itheta[2]);
txt_T3.Text = Convert.ToString(robot.itheta[3]);
}