Пример #1
0
        public static Vector calXYZ(double theta_1, double theta_2, double theta_3)
        {
            //double g_l3_1 = MATH_L3 * Math.Cos(theta_2 / MATH_TRANS);
            //double g_l4_1 = MATH_L4 * Math.Cos(theta_3 / MATH_TRANS);
            double g_l5 = (MATH_L2 + MATH_L3 * Math.Cos(theta_2 / MATH_TRANS) + MATH_L4 * Math.Cos(theta_3 / MATH_TRANS));
            Vector vector = new Vector();

            vector.x = -Math.Cos(Math.Abs(theta_1 / MATH_TRANS)) * g_l5;
            vector.y = -Math.Sin(Math.Abs(theta_1 / MATH_TRANS)) * g_l5;
            vector.z = MATH_L1 + MATH_L3 * Math.Sin(Math.Abs(theta_2 / MATH_TRANS)) - MATH_L4 * Math.Sin(Math.Abs(theta_3 / MATH_TRANS));
            Console.WriteLine("CalXYZ: vector.x: " + vector.x);
            Console.WriteLine("CalXYZ: vector.y: " + vector.y);
            Console.WriteLine("CalXYZ: vector.z: " + vector.z);
            return vector;
        }
Пример #2
0
 private void init()
 {
     servoRotOffset = readServoOffset(SERVO_ROT_NUM);
     servoLeftOffset = readServoOffset(SERVO_LEFT_NUM);
     servoRightOffset = readServoOffset(SERVO_RIGHT_NUM);
     readLinearOffset(SERVO_ROT_NUM);
     readLinearOffset(SERVO_LEFT_NUM);
     readLinearOffset(SERVO_RIGHT_NUM);
     readLinearOffset(SERVO_ROT_NUM);
     readLinearOffset(SERVO_LEFT_NUM);
     readLinearOffset(SERVO_RIGHT_NUM);
     servoHandRotOffset = 0;
     last_xyz = new Vector();
     last_xyz.x = 0;
     last_xyz.y = -15;
     last_xyz.z = 15;
 }