public void CalculationVelocities(ref PlanPositionParameters input, ref PlanVelositiesParameters res)
{
alpha_2n = fi_2n + omegaDirect * Math.PI / 2;
OperatorFunction.F2(X_P, Y_P, MechanismAnalys.v_B, alpha_2n, ref res.x_BVn, ref res.y_BVn);
OperatorFunction.F1(res.x_BVn, res.y_BVn, fi_3n + Math.PI / 2, X_P, Y_P, fi_4n + Math.PI / 2, ref res.x_CVn, ref res.y_CVn);
OperatorFunction.F4(X_P, Y_P, res.x_CVn, res.y_CVn, ref v_Cn);
OperatorFunction.F4(res.x_BVn, res.y_BVn, res.x_CVn, res.y_CVn, ref res.v_CBn);
OperatorFunction.F3(res.x_BVn, res.y_BVn, res.x_CVn, res.y_CVn, res.v_CBn, ref alpha_3n);
OperatorFunction.F3(X_P, Y_P, res.x_CVn, res.y_CVn, v_Cn, ref alpha_4n);
//Кутові швидкості ланок 3 і 4
OperatorFunction.F6(res.v_CBn, l3, alpha_3n, fi_3n, ref omega_3n);
OperatorFunction.F6(v_Cn, l4, alpha_4n, fi_4n, ref omega_4n);
//Визначаємо швидкість точки S2
OperatorFunction.F5(X_P, Y_P, res.x_BVn, res.y_BVn, l_AS_2, l2 - l_AS_2, 1, ref res.X_S2Vn, ref res.Y_S2Vn);
OperatorFunction.F4(X_P, Y_P, res.X_S2Vn, res.Y_S2Vn, ref v_S2n);
OperatorFunction.F3(X_P, Y_P, res.X_S2Vn, input.Y_S2n, v_S2n, ref alpha_S2n);
//Визначаємо швидкість точки S3
OperatorFunction.F5(res.x_BVn, res.y_BVn, res.x_CVn, res.y_CVn, l_BS_3, l3 - l_BS_3, 1, ref res.X_S3Vn, ref res.Y_S3Vn);
OperatorFunction.F4(X_P, Y_P, res.X_S3Vn, res.Y_S3Vn, ref v_S3n);
OperatorFunction.F3(X_P, Y_P, res.X_S3Vn, res.Y_S3Vn, v_S3n, ref alpha_S3n);
//Визначаємо швидкість точки S4
OperatorFunction.F5(X_P, Y_P, res.x_CVn, res.y_CVn, l_DS_4, l4 - l_DS_4, 1, ref res.X_S4Vn, ref res.Y_S4Vn);
OperatorFunction.F4(X_P, Y_P, res.X_S4Vn, res.Y_S4Vn, ref v_S4n);
OperatorFunction.F3(X_P, Y_P, res.X_S4Vn, res.Y_S4Vn, v_S4n, ref alpha_S4n);
}
public void Calculation(int n)
{
if (!isInitialise)
{
init_vars();
}
PlanPositionParameters res_position = new PlanPositionParameters();
PlanVelositiesParameters res_velocity;
PlanAccelerationsParameters res_acceleration;
PlanForcesParameters res_forces;
CalculationPlanPosition(ref res_position, n);
if (calculation_mode > CalculationModes.CalcOnlyPlanePosition)
{
res_velocity = new PlanVelositiesParameters();
CalculationVelocities(ref res_position, ref res_velocity);
}
if (calculation_mode > CalculationModes.CalcVelocities)
{
res_acceleration = new PlanAccelerationsParameters();
CalculationAcceleration(ref res_acceleration);
}
if (calculation_mode > CalculationModes.CalcAcceleration)
{
res_forces = new PlanForcesParameters();
CalculationForces(ref res_position, ref res_forces);
}
}
public void CalculationPlanPosition(ref PlanPositionParameters res, int n)
{
double L_BDn = new double();
double Fi_BDn = new double();
double delta_n = new double();
fi_2n = Fi_10 + n * 2 * Math.PI / quantIteration; //значення узагальненої координати
OperatorFunction.F2(X_A, Y_A, l2, fi_2n, ref res.X_Bn, ref res.Y_Bn); //знаходимо координати точки В
OperatorFunction.F4(res.X_Bn, res.Y_Bn, _X_D, _Y_D, ref L_BDn); //знаходимо довжину відрізка ВD
OperatorFunction.F3(res.X_Bn, res.Y_Bn, _X_D, _Y_D, L_BDn, ref Fi_BDn); //знаходимо кут BDn
delta_n = Math.Acos((l3 * l3 + L_BDn * L_BDn - l4 * l4) / (2 * l3 * L_BDn)); //
fi_3n = delta_n + Fi_BDn;
OperatorFunction.F2(res.X_Bn, res.Y_Bn, l3, fi_3n, ref res.X_Cn, ref res.Y_Cn);
OperatorFunction.F3(res.X_Cn, res.Y_Cn, _X_D, _Y_D, l4, ref fi_4n);
OperatorFunction.F5(X_A, Y_A, res.X_Bn, res.Y_Bn, l_AS_2, l2 - l_AS_2, 1, ref res.X_S2n, ref res.Y_S2n);
OperatorFunction.F5(res.X_Bn, res.Y_Bn, res.X_Cn, res.Y_Cn, l_BS_3, l3 - l_BS_3, 1, ref res.X_S3n, ref res.Y_S3n);
OperatorFunction.F5(_X_D, _Y_D, res.X_Cn, res.Y_Cn, l_DS_4, l4 - l_DS_4, 1, ref res.X_S4n, ref res.Y_S4n);
if (isAdditionalPoint)
{
if (point_0 == NumberPoints.B)
{
OperatorFunction.F2(res.X_Bn, res.Y_Bn, l5, fi_3n, ref res.X_En, ref res.Y_En);
}
else if (point_0 == NumberPoints.C)
{
OperatorFunction.F2(res.X_Cn, res.Y_Cn, l5, fi_3n + Math.PI, ref res.X_En, ref res.Y_En);
}
}
}
public void CalculationVelocities(ref PlanPositionParameters input, ref PlanVelositiesParameters res)
{
if (epsilon == 0)
{
omega_2n = omega;
}
v_Bn = omega_2n * l2;
alpha_2n = fi_2n + omegaDirect * Math.PI / 2;
res.x_BVn = X_P + v_B * Math.Cos(alpha_2n);
res.y_BVn = Y_P + v_B * Math.Sin(alpha_2n);
double k3n = Math.Tan(fi_3n + Math.PI / 2);
double k4n = Math.Tan(fi_4n + Math.PI / 2);
res.x_CVn = (k3n * res.x_BVn - res.y_BVn - k4n * X_P + Y_P) / (k3n - k4n);
res.y_CVn = k3n * (res.x_CVn - res.x_BVn) + res.y_BVn;
v_Cn = Math.Sqrt((res.x_CVn - X_P) * (res.x_CVn - X_P) + (res.y_CVn - Y_P) * (res.y_CVn - Y_P));
OperatorFunction.F2(X_P, Y_P, v_Bn, alpha_2n, ref res.x_BVn, ref res.y_BVn);
OperatorFunction.F1(res.x_BVn, res.y_BVn, fi_3n + Math.PI / 2, X_P, Y_P, fi_4n + Math.PI / 2, ref res.x_CVn, ref res.y_CVn);
OperatorFunction.F4(X_P, Y_P, res.x_CVn, res.y_CVn, ref v_Cn);
OperatorFunction.F4(res.x_BVn, res.y_BVn, res.x_CVn, res.y_CVn, ref res.v_CBn);
OperatorFunction.F3(res.x_BVn, res.y_BVn, res.x_CVn, res.y_CVn, res.v_CBn, ref alpha_3n);
OperatorFunction.F3(X_P, Y_P, res.x_CVn, res.y_CVn, v_Cn, ref alpha_4n);
//Кутові швидкості ланок 3 і 4
OperatorFunction.F6(res.v_CBn, l3, alpha_3n, fi_3n, ref omega_3n);
OperatorFunction.F6(v_Cn, l4, alpha_4n, fi_4n, ref omega_4n);
//Визначаємо швидкість точки S2
OperatorFunction.F5(X_P, Y_P, res.x_BVn, res.y_BVn, l_AS_2, l2 - l_AS_2, 1, ref res.X_S2Vn, ref res.Y_S2Vn);
OperatorFunction.F4(X_P, Y_P, res.X_S2Vn, res.Y_S2Vn, ref v_S2n);
OperatorFunction.F3(X_P, Y_P, res.X_S2Vn, input.Y_S2n, v_S2n, ref alpha_S2n);
//Визначаємо швидкість точки S3
OperatorFunction.F5(res.x_BVn, res.y_BVn, res.x_CVn, res.y_CVn, l_BS_3, l3 - l_BS_3, 1, ref res.X_S3Vn, ref res.Y_S3Vn);
OperatorFunction.F4(X_P, Y_P, res.X_S3Vn, res.Y_S3Vn, ref v_S3n);
OperatorFunction.F3(X_P, Y_P, res.X_S3Vn, res.Y_S3Vn, v_S3n, ref alpha_S3n);
//Визначаємо швидкість точки S4
OperatorFunction.F5(X_P, Y_P, res.x_CVn, res.y_CVn, l_DS_4, l4 - l_DS_4, 1, ref res.X_S4Vn, ref res.Y_S4Vn);
OperatorFunction.F4(X_P, Y_P, res.X_S4Vn, res.Y_S4Vn, ref v_S4n);
OperatorFunction.F3(X_P, Y_P, res.X_S4Vn, res.Y_S4Vn, v_S4n, ref alpha_S4n);
//Визначаємо швидкість точки E
if (isAdditionalPoint)
{
if (point_0 == NumberPoints.B)
{
OperatorFunction.F5(input.X_Cn, input.Y_Cn, input.X_Bn, input.Y_Bn, l3 + l5, l3, -1, ref res.X_EVn, ref res.Y_EVn);
OperatorFunction.F4(X_P, Y_P, res.X_EVn, res.Y_EVn, ref v_En);
OperatorFunction.F3(X_P, Y_P, res.X_EVn, res.Y_EVn, v_En, ref alpha_En);
}
else if (point_0 == NumberPoints.C)
{
OperatorFunction.F5(input.X_Bn, input.Y_Bn, input.X_Cn, input.Y_Cn, l3 + l5, l3, -1, ref res.X_EVn, ref res.Y_EVn);
OperatorFunction.F4(X_P, Y_P, res.X_EVn, res.Y_EVn, ref v_En);
OperatorFunction.F3(X_P, Y_P, res.X_EVn, res.Y_EVn, v_En, ref alpha_En);
}
}
}
public void CalculationForces(ref PlanPositionParameters plan_position, ref PlanForcesParameters res)
{
G_2 = m_2 * _g;
G_3 = m_3 * _g;
G_4 = m_4 * _g;
FI_2n = m_2 * a_S2n;
FI_3n = m_3 * a_S3n;
FI_4n = m_4 * a_S4n;
gamma_FI2n = beta_S2n + Math.PI;
gamma_FI3n = beta_S3n + Math.PI;
gamma_FI4n = beta_S4n + Math.PI;
M_FI2n = -_J_S2 * epsilon;
M_FI3n = -_J_S3 * epsilon_3n;
M_FI4n = -_J_S4 * epsilon_4n;
res.x_S22n = plan_position.X_S2n * Math.Cos(theta) - plan_position.Y_S2n * Math.Sin(theta);
res.x_S32n = plan_position.X_S3n * Math.Cos(theta) - plan_position.Y_S3n * Math.Sin(theta);
res.x_S42n = plan_position.X_S4n * Math.Cos(theta) - plan_position.Y_S4n * Math.Sin(theta);
res.x_C2n = plan_position.X_Cn * Math.Cos(theta) - plan_position.Y_Cn * Math.Sin(theta);
res.x_A2n = X_A * Math.Cos(theta) - Y_A * Math.Sin(theta);
if (plan_position.Y_S3n >= plan_position.Y_Cn)
{
res.fi_CS3n = Math.Sign(plan_position.Y_S2n - plan_position.Y_Cn) * Math.Acos((plan_position.X_S3n - plan_position.X_Cn) / plan_position.l_CS3);
}
else
{
res.fi_CS3n = Math.Sign(plan_position.Y_S2n - plan_position.Y_Cn) * Math.Acos((plan_position.X_S3n - plan_position.X_Cn) / plan_position.l_CS3) + 2 * Math.PI;
};
res.R_23taun = -(M_FI3n + FI_3n * res.l_CS3 * Math.Sin(gamma_FI3n - res.fi_CS3n) + G_3 * (res.x_C2n - res.x_S32n)) / l3;
if (res.R_23taun > 0)
{
res.gamma_R23taun = fi_3n - Math.PI / 2;
}
else
{
res.gamma_R23taun = fi_3n + Math.PI / 2;
};
if (plan_position.Y_S4n >= plan_position.Y_Cn)
{
res.fi_CS4n = Math.Sign(plan_position.Y_S4n - plan_position.Y_Cn) * Math.Acos((plan_position.X_S4n - plan_position.X_Cn) / l_CS4);
}
else
{
res.fi_CS4n = Math.Sign(plan_position.Y_S4n - plan_position.Y_Cn) * Math.Acos((plan_position.X_S4n - plan_position.X_Cn) / l_CS4) + 2 * Math.PI;
};
res.R_14taun = -(M_4 + M_FI4n + FI_4n * l_CS4 * Math.Sin(gamma_FI4n - res.fi_CS4n) + G_4 * (res.x_C2n - res.x_S42n)) / l4;
if (res.R_14taun > 0)
{
res.gamma_R14taun = fi_4n - Math.PI / 2;
}
else
{
res.gamma_R14taun = fi_4n + Math.PI / 2;
};
res.fi_G = 1.5 * Math.PI - theta;
res.fi_44n = Math.PI - fi_4n;
res.c_4n = res.R_14taun * Math.Cos(res.gamma_R14taun + res.fi_44n) + FI_4n * Math.Cos(gamma_FI4n + res.fi_44n) + (G_3 + G_4) * Math.Cos(res.fi_G + res.fi_44n) + FI_3n * Math.Cos(gamma_FI3n + res.fi_44n) + res.R_23taun * Math.Cos(res.gamma_R23taun + res.fi_44n);
res.d_4n = res.R_14taun * Math.Cos(res.gamma_R14taun - fi_3n) + FI_4n * Math.Cos(gamma_FI4n - fi_3n) + (G_3 + G_4) * Math.Cos(res.fi_G - fi_3n) + FI_3n * Math.Cos(gamma_FI3n - fi_3n) + res.R_23taun * Math.Cos(res.gamma_R23taun - fi_3n);
res.R_23nn = (res.c_4n * Math.Cos(fi_4n - fi_3n - Math.PI) - res.d_4n) / (1 - Math.Cos(fi_3n + res.fi_44n) * Math.Cos(fi_4n - fi_3n - Math.PI));
if (res.R_23nn > 0)
{
res.gamma_R23nn = fi_3n;
}
else
{
res.gamma_R23nn = fi_3n - Math.PI;
};
res.R_14nn = -(res.c_4n + res.R_23nn * Math.Cos(fi_3n + res.fi_44n));
if (res.R_14nn > 0)
{
res.gamma_R14nn = fi_4n - Math.PI;
}
else
{
res.gamma_R14nn = fi_4n;
};
R_23n = Math.Sqrt(res.R_23taun * res.R_23taun + res.R_23nn * res.R_23nn);
res.R_23xn = res.R_23nn * Math.Cos(res.gamma_R23nn) + res.R_23taun * Math.Cos(res.gamma_R23taun);
res.R_23yn = res.R_23nn * Math.Sin(res.gamma_R23nn) + res.R_23taun * Math.Sin(res.gamma_R23taun);
if (res.R_23yn >= 0)
{
gamma_R23n = Math.Acos(res.R_23xn / R_23n);
}
else
{
gamma_R23n = -Math.Acos(res.R_23xn / R_23n) + 2 * Math.PI;
};
R_14n = Math.Sqrt(res.R_14taun * res.R_14taun + res.R_14nn * res.R_14nn);
res.R_14xn = res.R_14nn * Math.Cos(res.gamma_R14nn) + res.R_14taun * Math.Cos(res.gamma_R14taun);
res.R_14yn = res.R_14nn * Math.Sin(res.gamma_R14nn) + res.R_14taun * Math.Sin(res.gamma_R14taun);
if (res.R_14yn >= 0)
{
gamma_R14n = Math.Acos(res.R_14xn / R_14n);
}
else
{
gamma_R14n = -Math.Acos(res.R_14xn / R_14n) + 2 * Math.PI;
};
res.R_34yn = -(R_14n * Math.Sin(gamma_R14n) + FI_4n * Math.Sin(gamma_FI4n) + G_4 * Math.Sin(res.fi_G));
res.R_34xn = -(R_14n * Math.Cos(gamma_R14n) + FI_4n * Math.Cos(gamma_FI4n) + G_4 * Math.Cos(res.fi_G));
R_34n = Math.Sqrt(res.R_34xn * res.R_34xn + res.R_34yn * res.R_34yn);
if (res.R_34yn >= 0)
{
gamma_R34n = Math.Acos(res.R_34xn / R_34n);
}
else
{
gamma_R34n = -Math.Acos(res.R_34xn / R_34n) + 2 * Math.PI;
};
if (plan_position.Y_S2n >= Y_A)
{
res.fi_AS2n = Math.Sign(plan_position.Y_S2n - Y_A) * Math.Acos((plan_position.X_S2n - X_A) / l_AS_2);
}
else
{
res.fi_AS2n = Math.Sign(plan_position.Y_S2n - Y_A) * Math.Acos((plan_position.X_S2n - X_A) / l_AS_2) + 2 * Math.PI;
};
if (plan_position.Y_Cn >= Y_A)
{
res.fi_ACn = Math.Sign(plan_position.Y_Cn - Y_A) * Math.Acos((plan_position.X_Cn - X_A) / l2);
}
else
{
res.fi_ACn = Math.Sign(plan_position.Y_Cn - Y_A) * Math.Acos((plan_position.X_Cn - X_A) / l2) + 2 * Math.PI;
};
//
res.fi_ACn = 2.45;//EDIT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//
gamma_R32n = gamma_R23n + Math.PI;
res.R_32n = R_23n;
res.M_F2n = -(res.R_32n * l2 * Math.Sin(gamma_R32n - res.fi_ACn) + FI_2n * l_AS_2 * Math.Sin(gamma_FI2n - res.fi_AS2n) + G_2 * (res.x_A2n - res.x_S22n) + M_FI2n);
F_2n = Math.Abs(res.M_F2n / (r_omega * Math.Cos(alpha_omega)));
if (res.M_F2n > 0)
{
gamma_F2n = -alpha_omega;
}
else
{
gamma_F2n = Math.PI - alpha_omega;
};
res.R_12xn = -(res.R_32n * Math.Cos(gamma_R32n) + G_2 * Math.Cos(res.fi_G) + FI_2n * Math.Cos(gamma_FI2n) + F_2n * Math.Cos(gamma_F2n));
res.R_12yn = -(res.R_32n * Math.Sin(gamma_R32n) + G_2 * Math.Sin(res.fi_G) + FI_2n * Math.Sin(gamma_FI2n) + F_2n * Math.Sin(gamma_F2n));
R_12n = Math.Sqrt(res.R_12xn * res.R_12xn + res.R_12yn * res.R_12yn);
if (res.R_12yn >= 0)
{
gamma_R12n = Math.Acos(res.R_12xn / R_12n);
}
else
{
gamma_R12n = -Math.Acos(res.R_12xn / R_12n) + 2 * Math.PI;
};
}
