Esempio n. 1
0
        public static Theta calculateServoAngles(double x, double y, double z)
        {
            Theta theta = new Theta();

            if (z > (MATH_L1 + MATH_L3 + TopOffset))
            {
                z = MATH_L1 + MATH_L3 + TopOffset;
            }

            if (z < (MATH_L1 - MATH_L4 + BottomOffset))
            {
                z = MATH_L1 - MATH_L4 + BottomOffset;
            }

            g_y_in = (-y - MATH_L2) / MATH_L3;
            g_z_in = (z - MATH_L1) / MATH_L3;
            g_l43 = MATH_L4 / MATH_L3;
            g_right_all = (1 - g_y_in * g_y_in - g_z_in * g_z_in - g_l43 * g_l43) / (2 * g_l43);
            g_sqrt_z_y = Math.Sqrt(g_z_in * g_z_in + g_y_in * g_y_in);

            if (x == 0)
            {
                // Calculate value of theta 1
                theta.theta_1 = 90;

                // Calculate value of theta 3
                if (g_z_in == 0)
                {
                    g_phi = 90;
                }

                else
                {
                    g_phi = Math.Atan(-g_y_in / g_z_in) * MATH_TRANS;
                }

                if (g_phi > 0)
                    g_phi = g_phi - 180;

                theta.theta_3 = Math.Asin(g_right_all / g_sqrt_z_y) * MATH_TRANS - g_phi;
                if (theta.theta_3 < 0)
                {
                    theta.theta_3 = 0;
                }
                // Calculate value of theta 2

                theta.theta_2 = Math.Asin((z + MATH_L4 * Math.Sin(theta.theta_3 / MATH_TRANS) - MATH_L1) / MATH_L3) * MATH_TRANS;
            }

            else
            {
                // Calculate value of theta 1

                theta.theta_1 = Math.Atan(y / x) * MATH_TRANS;

                if (y / x > 0)
                {
                    theta.theta_1 = theta.theta_1 + 0;
                }
                if (y / x < 0)
                {
                    theta.theta_1 = theta.theta_1 + 180;
                }
                if (y == 0)
                {
                    if (x > 0)
                        theta.theta_1 = 180;
                    else
                        theta.theta_1 = 0;
                }

                // Calculate value of theta 3

                g_x_in = (-x / Math.Cos(theta.theta_1 / MATH_TRANS) - MATH_L2) / MATH_L3;

                if (g_z_in == 0)
                {
                    g_phi = 90;
                }

                else
                {
                    g_phi = Math.Atan(-g_x_in / g_z_in) * MATH_TRANS;
                }

                if (g_phi > 0)
                    g_phi = g_phi - 180;

                g_sqrt_z_x = Math.Sqrt(g_z_in * g_z_in + g_x_in * g_x_in);

                g_right_all_2 = -1 * (g_z_in * g_z_in + g_x_in * g_x_in + g_l43 * g_l43 - 1) / (2 * g_l43);
                theta.theta_3 = Math.Asin(g_right_all_2 / g_sqrt_z_x) * MATH_TRANS;
                theta.theta_3 = theta.theta_3 - g_phi;

                if (theta.theta_3 < 0)
                {
                    theta.theta_3 = 0;
                }

                // Calculate value of theta 2

                theta.theta_2 = Math.Asin(g_z_in + g_l43 * Math.Sin(Math.Abs(theta.theta_3 / MATH_TRANS))) * MATH_TRANS;

            }

            theta.theta_1 = Math.Abs(theta.theta_1);
            theta.theta_2 = Math.Abs(theta.theta_2);

            if (theta.theta_3 < 0)
            {
            }
            else
            {
                if ((calYonly(theta.theta_1, theta.theta_2, theta.theta_3) > y + 0.1) || (calYonly(theta.theta_1, theta.theta_2, theta.theta_3) < y - 0.1))
                {
                    theta.theta_2 = 180 - theta.theta_2;
                }
            }

            theta.theta_3 = Math.Abs(theta.theta_3);
            return theta;
        }
Esempio n. 2
0
        public void MoveTo(double x, double y, double z,int time)
        {
            attachAll();
            Theta theta = new Theta();
            double[] x_arr = new double[50];
            double[] y_arr = new double[50];
            double[] z_arr = new double[50];

            double current_x = findX();
            double current_y = findY();
            double current_z = findZ();

            Vector vector = calXYZ();
            double _x = vector.x + x;
            double _y = vector.y + y;
            double _z = vector.z + z;

            x_arr = ActionControl.interpolation(current_x, x);
            y_arr = ActionControl.interpolation(current_y, y);
            z_arr = ActionControl.interpolation(current_z, z);

            if (time < 0) time = 0;

            for (int i = 0; i < 50; i++)
            {
                Theta t = ActionControl.calculateServoAngles(x_arr[i], y_arr[i], z_arr[i]);
                writeAngles(t.theta_1, t.theta_2, t.theta_3, 0);
                Thread.Sleep(time*20);
            }
        }
Esempio n. 3
0
 private Theta AdjustAngle(Theta theta)
 {
     theta.theta_1 = Math.Round(theta.theta_1 + servoRotOffset);
     theta.theta_2 = Math.Round(theta.theta_2 + servoLeftOffset);
     theta.theta_3 = Math.Round(theta.theta_3 + servoRightOffset);
     return theta;
 }