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;
}
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;
}