Esempio n. 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);
        }
Esempio n. 2
0
        private void button5_Click(object sender, EventArgs e)
        {
            if (arm == null)
            {
                return;
            }

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

            arm.links[3] = ArmLink.rotateZ(arm.links[3], angle);
            g.Clear(Color.White);
            arm.drawArmcyl(xdim, ydim, g);

            double[][] effector = arm.linkPosition(6);

            Link3AngleText.Text = Convert.ToString(arm.links[3].anglez * 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;

            EulerParams orientation = arm.linkAngles(6);

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


            if (connected)
            {
                if (alligned)
                {
                    String cmd;
                    if (Servo0BaseIndexRadio.Checked)
                    {
                        cmd = "#2 P";
                    }
                    else
                    {
                        cmd = " #3 P";
                    }
                    int link3pos = RobotArm.link3Servo(arm.links[3].anglez);
                    cmd += Convert.ToString(link3pos);
                    cmd += " T1000\r";

                    serialPort1.Write(cmd);
                }
            }
        }
Esempio n. 3
0
        public ArmLink(ArmLink prevlink, double dispX, double dispY, double dispZ,
                       double rotX, double rotY, double rotZ)
        {
            int     i, j;
            ArmLink newlink = ArmLink.translateXYZ(prevlink, dispX, dispY, dispZ);

            newlink = ArmLink.rotateX(newlink, rotX);
            newlink = ArmLink.rotateY(newlink, rotY);
            newlink = ArmLink.rotateZ(newlink, rotY);

            // copying transfomation matrix from newlink to the object
            for (i = 0; i < 4; i++)
            {
                for (j = 0; j < 4; j++)
                {
                    this.T[i][j] = newlink.T[i][j];
                }
            }
            // transformation matrix copied
        }
Esempio n. 4
0
        public void addDHline(double anglei_1, double ai_1, double di, double homeangle)
        {
            linknum++;
            // creating a new line in the Denavit - Hartenberg matrix
            DHmatrix[linknum - 1] = new double[4];
            // filling the line now
            DHmatrix[linknum - 1][0] = anglei_1; // angle about the x-axis
            // bewteen i-1 and i link
            DHmatrix[linknum - 1][1] = ai_1;     // distance between i-1 and i link
            // on the x-axis
            DHmatrix[linknum - 1][2] = di;       // displace of link i on the z-axis
            DHmatrix[linknum - 1][3] = anglei_1; // home angle of link-i
            // Denavit - Hartenberg line added

            // Now assigning a frame to the link
            links[linknum - 1] = ArmLink.rotateX(links[linknum - 2], anglei_1);         // rotating about x by angle i-1
            // to align with link i
            links[linknum - 1] = ArmLink.translateXYZ(links[linknum - 1], ai_1, 0, di); // translating on x by the distance bewteen
            // i and i-1 and translating on z by displacement di
            links[linknum - 1] = ArmLink.rotateZ(links[linknum - 1], homeangle);        // rotating about z to home position (angle theta)
        }
Esempio n. 5
0
        private void timer1_Tick(object sender, EventArgs e)
        {
            clockticks++;
            if (clockticks <= trajectory.len)
            {
                if (trajectory.moves[clockticks - 1].Dbase != 0)
                {
                    arm.links[1] = ArmLink.rotateY(arm.links[1], trajectory.moves[clockticks - 1].Dbase);
                }
                if (trajectory.moves[clockticks - 1].Dth1 != 0)
                {
                    arm.links[2] = ArmLink.rotateZ(arm.links[2], trajectory.moves[clockticks - 1].Dth1);
                }
                if (trajectory.moves[clockticks - 1].Dth2 != 0)
                {
                    arm.links[3] = ArmLink.rotateZ(arm.links[3], trajectory.moves[clockticks - 1].Dth2);
                }
                if (trajectory.moves[clockticks - 1].Dth3 != 0)
                {
                    arm.links[4] = ArmLink.rotateZ(arm.links[4], trajectory.moves[clockticks - 1].Dth3);
                }
                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);
                Link2AngleText.Text = Convert.ToString(arm.links[2].anglez * 180 / Math.PI);
                Link3AngleText.Text = Convert.ToString(arm.links[3].anglez * 180 / Math.PI);
                Link4AngleText.Text = Convert.ToString(arm.links[4].anglez * 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;

                EulerParams orientation = arm.linkAngles(6);
                yawText.Text   = Convert.ToString(orientation.Yaw * 180 / Math.PI);
                pitchText.Text = Convert.ToString(orientation.Pitch * 180 / Math.PI);
                rollText.Text  = Convert.ToString(orientation.Roll * 180 / Math.PI);

                if (alligned)
                {
                    String cmd;
                    // link#1 motion command
                    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);

                    // link #2 motion command
                    if (Servo0BaseIndexRadio.Checked)
                    {
                        cmd = " #1 P";
                    }
                    else
                    {
                        cmd = " #2 P";
                    }
                    int link2pos = RobotArm.link2Servo(arm.links[2].anglez);
                    cmd += Convert.ToString(link2pos);
                    cmd += " T1000\r";

                    serialPort1.Write(cmd);

                    // link #3 motion command
                    if (Servo0BaseIndexRadio.Checked)
                    {
                        cmd = "#2 P";
                    }
                    else
                    {
                        cmd = " #3 P";
                    }
                    int link3pos = RobotArm.link3Servo(arm.links[3].anglez);
                    cmd += Convert.ToString(link3pos);
                    cmd += " T1000\r";

                    serialPort1.Write(cmd);

                    // link #4 motion command
                    if (Servo0BaseIndexRadio.Checked)
                    {
                        cmd = " #3 P";
                    }
                    else
                    {
                        cmd = " #4 P";
                    }
                    int link4pos = RobotArm.link4Servo(arm.links[4].anglez);
                    cmd += Convert.ToString(link4pos);
                    cmd += " T1000\r";

                    serialPort1.Write(cmd);
                }
            }
            else
            {
                moving         = false;
                timer1.Enabled = false;
            }
        }