Ejemplo n.º 1
0
        private void RefreshLabels()
        {
            tuple p = r.GetPosition();

            lX.Text = round((p.t1), 4).ToString();
            lY.Text = round(p.t2, 4).ToString();
            lZ.Text = round(p.t3, 4).ToString();
            lA.Text = round(p.t4 / dToR, 4).ToString();
            lB.Text = round(p.t5 / dToR, 4).ToString();
            lC.Text = round(p.t6 / dToR, 4).ToString();
            //  MessageBox.Show("CLR MessageBox", round((p.t1), 4).ToString(), MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
        }
Ejemplo n.º 2
0
        //Given position and orientation, calculate the angles of the motors and draw robot consequently
        public tuple MoveToPositions(double x, double y, double z, double a, double b, double c)
        {
            tuple r = inverse(x, y, z, a, b, c);

            draw(r.t1, r.t2, r.t3, r.t4, r.t5, r.t6);
            _t1 = r.t1;
            _t2 = r.t2;
            _t3 = r.t3;
            _t4 = r.t4;
            _t5 = r.t5;
            _t6 = r.t6;
            return(r);
        }
Ejemplo n.º 3
0
        //Return the actual position and orientation of the top of the arm6
        public tuple GetPosition()
        {
            double nx;
            double ny;
            double nz;
            double sx;
            double sy;
            double sz;
            double ax;
            double ay;
            double az;
            double par = 30 * pi / 180;
            double s1;
            double c1;
            double s2;
            double c2;
            double s3;
            double c3;
            double s23;
            double c23;
            double s4;
            double c4;
            double s5;
            double c5;
            double s6;
            double c6;

            s1  = Math.Sin(_t1);
            s2  = Math.Sin(_t2);
            s3  = Math.Sin(_t3);
            s23 = Math.Sin(_t2 + _t3);
            s4  = Math.Sin(_t4);
            s5  = Math.Sin(_t5);
            s6  = Math.Sin(_t6);
            c1  = Math.Cos(_t1);
            c2  = Math.Cos(_t2);
            c3  = Math.Cos(_t3);
            c23 = Math.Cos(_t2 + _t3);
            c4  = Math.Cos(_t4);
            c5  = Math.Cos(_t5);
            c6  = Math.Cos(_t6);
            //nx = ((s1*s4 - c1*s23*c4)*c5 - c1*c23*s5)*s6 + (-c1*s23*s4 - s1*c4)*c6;
            //ny = ((-c1*s4 - s1*s23*c4)*c5 - s1*c23*s5)*s6 + (c1*c4 - s1*s23*s4)*c6;
            nz = (c23 * c4 * c5 - s23 * s5) * s6 + c23 * s4 * c6;
            //sx = ((s1*s4 - c1*s23*c4)*c5 - c1*c23*s5)*c6 - (-c1*s23*s4 - s1*c4)*s6;
            //sy = ((-c1*s4 - s1*s23*c4)*c5 - s1*c23*s5)*c6 - (c1*c4 - s1*s23*s4)*s6;
            sz = (c23 * c4 * c5 - s23 * s5) * c6 - c23 * s4 * s6;
            ax = (s1 * s4 - c1 * s23 * c4) * s5 + c1 * c23 * c5;
            ay = (-c1 * s4 - s1 * s23 * c4) * s5 + s1 * c23 * c5;
            az = c23 * c4 * s5 + s23 * c5;


            //nx = round2(nx, 6);
            //ny = round2(ny, 6);
            nz = round2(nz, 6);
            //sy = round2(sy, 6);
            sz = round2(sz, 6);
            az = round2(az, 6);
            ay = round2(ay, 6);
            ax = round2(ax, 6);
            Point3D p = Arm6(_t1, _t2, _t3, _t4, _t5, _t6, 0, 0, d6);
            tuple   t = new tuple();


            t.t1 = p.x;
            t.t2 = p.y;
            t.t3 = p.z;
            //I calculate this value with brute force, see below
            t.t5 = Math.Atan2(-az, Math.Sqrt(ax * ax + ay * ay)); // it works also with atan2(-az, sqrt(sz*sz + nz*nz));
            t.t4 = Math.Atan2(ay / Math.Cos(t.t5), ax / Math.Cos(t.t5));
            t.t6 = Math.Atan2(sz / Math.Cos(t.t5), nz / Math.Cos(t.t5)) - pi / 2;


            //This quoted procedure needs to find, brute force, the right configuration of parameters... there is to go crazy
            //Remember to initiate the class MyForm in Robot::Robot

            //double m[9];
            //System::Text::StringBuilder^ st = gcnew System::Text::StringBuilder("");
            //m[0] = round2(nx,6);
            //m[1] = round2(ny, 6);
            //m[2] = round2(nz, 6);
            //m[3] = round2(sx, 6);
            //m[4] = round2(sy, 6);
            //m[5] = round2(sz, 6);
            //m[6] = round2(ax, 6);
            //m[7] = round2(ay, 6);
            //m[8] = round2(az, 6);

            //for (int i = 0; i < 9;i++)
            //{
            //	for (int j = 0; j < 9; j++)
            //	{

            //		/*for (int k = 0; k < 9; k++)
            //		{
            //		*/	st->Append(i);
            //			st->Append(j);
            //		// st->Append(k);
            //			st->Append("=");
            //			st->Append(atan2(m[i] / cos(t->t5), m[j] / cos(t->t5)) / dToR);
            //			// st->Append(atan2(m[i],sqrt(m[j]*m[j]+m[k]*m[k]))/dToR);
            //			st->Append("\r\n");
            //	/*}*/

            //	}


            //}


            //f->textBox1->Text = st->ToString();
            return(t);
        }
Ejemplo n.º 4
0
        //Given position and orientation, calculate the angles of the motors
        private tuple inverse(double x, double y, double z, double a, double b, double c)
        {
            tuple p;

            p = new tuple();
            double pWx;
            double pWy;
            double pWz;
            double px;
            double py;
            double pz;
            double s3;
            double c3;
            double s2;
            double c2;
            double xi;
            double fi;

            int sgn;

            pWx = x - Math.Cos(a) * Math.Cos(b) * (d6 + a5);
            pWy = y - Math.Sin(a) * Math.Cos(b) * (d6 + a5);
            pWz = z + Math.Sin(b) * (d6 + a5);
            double u;
            int    sg = 1;

            p.t1 = Math.Atan2(pWy, pWx);
            ///////Dunno what it does
            //if ((int)(pWx * pWx + pWy * pWy - a1 * a1 < 0) != 0)
            //{
            //    sg = -1;
            //}
            //distance of the wirst from the Motor 2
            px = pWx - a1 * Math.Cos(p.t1);
            py = pWy - a1 * Math.Sin(p.t1);
            pz = pWz - d1 - h;



            c3 = (px * px + py * py + pz * pz - Math.Pow(a3 + d4, 2) - a2 * a2) / (2 * (a3 + d4) * a2);
            s3 = -Math.Sqrt(1 - c3 * c3);

            p.t3 = Math.Atan2(s3, c3);

            //the follow code show in the MyForm f, some parameters. MyForm get shown in the creator
            //f->textBox1->Text = System::String::Concat("c3num =", (px*px + py*py + pz*pz - pow((a3 + d4), 2) ), "\r\n"
            //	, "mod =", px*px + py*py + pz*pz, "\r\n"
            //	, "pWx =", pWx, "\r\n"
            //	, "pWy =", pWy, "\r\n"
            //	, "pWz =", pWz, "\r\n"
            //	, "px =", px, "\r\n"
            //	, "py =", py, "\r\n"
            //	, "pz =", pz, "\r\n"
            //	, "c3 =", c3, "\r\n"
            //	, "s3 =", s3, "\r\n"
            //	, "t3 =", p->t3, "\r\n"
            //	);
            // http://www.rob.uni-luebeck.de/Lehre/2008w/Robotik/Vorlesung/Robotik1VL5_1_vers1.pdf  pag. 50
            xi = Math.Atan2(pz, sg * Math.Sqrt(px * px + py * py)); // sg is my addition
            fi = Math.Atan2((a3 + d4) * s3, (a2 + (a3 + d4) * c3));

            p.t2 = xi - fi;

            double nx;
            double ny;
            double nz;
            double sx;
            double ax;
            double sa;
            double sb;
            double sc;
            double s1;
            double ca;
            double cb;
            double cc;
            double c1;
            double c23;
            double s23;

            sa  = Math.Sin(a);
            sb  = Math.Sin(b);
            sc  = Math.Sin(c);
            s1  = Math.Sin(p.t1);
            s23 = Math.Sin(p.t3 + p.t2);

            ca  = Math.Cos(a);
            cb  = Math.Cos(b);
            cc  = Math.Cos(c);
            c1  = Math.Cos(p.t1);
            c23 = Math.Cos(p.t3 + p.t2);

            nx = round2(-sb * s23 + cb * c23 * Math.Cos(a - p.t1), 6);
            ny = round2(-cb * s23 * Math.Cos(a - p.t1) - sb * c23, 6);
            nz = round2(cb * Math.Sin(p.t1 - a), 6);
            sx = round2(cb * sc * s23 + (sa * sb * sc + ca * cc) * s1 * c23 + (ca * sb * sc - sa * cc) * c1 * c23, 6);
            ax = round2(cb * cc * s23 + (sa * sb * cc - ca * sc) * s1 * c23 + (sa * sc + ca * sb * cc) * c1 * c23, 6);

            p.t5 = Math.Atan2(Math.Sqrt(nz * nz + ny * ny), (nx));
            if (p.t5 < 0)
            {
                p.t4 = Math.Atan2((-nz), (-ny));

                p.t5 = Math.Atan2(-Math.Sqrt(nz * nz + ny * ny), (nx));
            }
            else
            {
                //p->t4 = atan2((nz), (ny));

                //p->t5 = atan2(sqrt(nz *nz + ny *ny), (nx));
                p.t4 = Math.Atan2((nz), (ny));
            }
            p.t6 = Math.Atan2(-sx, -ax);
            if (sg < 0)
            {
                p.t6 = Math.Atan2(sx, ax);

                if (p.t6 < 0)
                {
                    p.t6 = Math.Atan2(-sx, -ax);
                }
                else
                {
                    p.t6 = Math.Atan2(sx, ax);
                }
            }


            return(p);
        }