Beispiel #1
0
        private void button1_Click(object sender, EventArgs e)
        {
            RobotArm arm = RobotArm.lynxmotionL6();

            arm.links[1] = ArmLink.rotateY(arm.links[1], Math.PI / 4);
            arm.links[1] = ArmLink.rotateZ(arm.links[1], -Math.PI / 4);

            arm.links[3] = ArmLink.rotateY(arm.links[3], -Math.PI / 4);
            arm.links[2] = ArmLink.rotateY(arm.links[2], Math.PI / 4);
            double[][] effector = arm.linkPosition(4);
            double     x        = effector[0][0];
            double     y        = effector[1][0];
            double     z        = effector[2][0];


            String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z);

            effectorPositionText.Text = str;

            Graphics g      = panel1.CreateGraphics();
            int      panelx = panel1.Width;
            int      panely = panel1.Height;



            arm.drawArm(panelx, panely, g);
        }
Beispiel #2
0
        private void button3_Click(object sender, EventArgs e)
        {
            arm  = RobotArm.lynxmotionL6();
            g    = panel1.CreateGraphics();
            xdim = panel1.Width;
            ydim = panel1.Height;
            g.Clear(Color.White);
            this.arm.drawArmcyl(xdim, ydim, g);

            double[][] effector = arm.linkPosition(4);
            double     x        = effector[0][0];
            double     y        = effector[1][0];
            double     z        = effector[2][0];


            String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z);

            effectorPositionText.Text = str;

            Link1AngleText.Text = Link2AngleText.Text = Link3AngleText.Text = Link4AngleText.Text = Link5AngleText.Text = "0";
            GripWidthText.Text  = "100";

            Link1AngleText.Text = "0";
            Link2AngleText.Text = "0";
            Link3AngleText.Text = "0";
            Link4AngleText.Text = "0";
            Link5AngleText.Text = "0";

            yawText.Text   = "0";
            pitchText.Text = "0";
            rollText.Text  = "0";
        }
Beispiel #3
0
        private void button4_Click(object sender, EventArgs e)
        {
            if (arm == null)
            {
                return;
            }

            int    angdeg = (int)Link1RateNumeric.Value;
            double angle  = angdeg * Math.PI / 180;

            arm.links[1] = ArmLink.rotateY(arm.links[1], angle);
            g.Clear(Color.White);
            arm.drawArmcyl(xdim, ydim, g);
            double[][] effector = arm.linkPosition(6);
            Link1AngleText.Text = Convert.ToString(arm.links[1].angley * 180 / Math.PI);
            double x = effector[0][0];
            double y = effector[1][0];
            double z = effector[2][0];


            String str = Convert.ToString(x) + " , " + Convert.ToString(y) + " , " + Convert.ToString(z);

            effectorPositionText.Text = str;

            //double[] orientation = arm.linkAngles(6);
            //yawText.Text = Convert.ToString(orientation[0]*180/Math.PI);
            //pitchText.Text = Convert.ToString(orientation[1]*180/Math.PI);
            //rollText.Text = Convert.ToString(orientation[2]*180/Math.PI);
            EulerParams effectorOrientation = arm.linkAngles(6);

            yawText.Text   = Convert.ToString(effectorOrientation.Yaw * 180 / Math.PI);
            pitchText.Text = Convert.ToString(effectorOrientation.Pitch * 180 / Math.PI);
            rollText.Text  = Convert.ToString(effectorOrientation.Roll * 180 / Math.PI);


            if (connected)
            {
                if (alligned)
                {
                    String cmd;
                    if (Servo0BaseIndexRadio.Checked)
                    {
                        cmd = " #0 P";
                    }
                    else
                    {
                        cmd = " #1 P";
                    }
                    int basepos = RobotArm.baseServo(arm.links[1].angley);
                    cmd += Convert.ToString(basepos);
                    cmd += " T1000\r";

                    serialPort1.Write(cmd);
                }
            }
        }