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