Exemplo n.º 1
0
        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);
            }
        }
Exemplo n.º 2
0
        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]);
        }