/// <summary>
/// Возвращает корректные пары углов поворота плеча (φ1 и φ2) по координатам центра схвата.
/// </summary>
/// <param name="designParams">Конструктивные параметры манипулятора</param>
/// <param name="x">Координата х центра схвата в базовой системе координат</param>
/// <param name="y">Координата y центра схвата в базовой системе координат</param>
/// <param name="digits">Количество знаков после запятой для сравнения координат</param>
/// <returns>Коллекция структур AnglesOfRotation - Значения углов поворота плеча (φ1 и φ2)</returns>
public static List <AnglesOfRotation> GetAngles(DesignParameters designParams, double x, double y, int digits)
{
AnglesCalculation.digits = digits;
var instance = new AnglesCalculation(designParams);
var validPairs = new List <AnglesOfRotation>();
foreach (var pair in instance.GetAnglesPairs(x, y))
{
if (instance.IsRootsAreValid(pair, x, y) && IsAnglesAreValid(designParams, pair))
{
validPairs.Add(pair);
}
}
if (validPairs.Count() == 0)
{
throw new DesignParametersException(string.Format("Невозможно достичь заданные координаты X={0:f3} Y{1:f3}", x, y));
}
AnglesCalculation.digits = 2;
return(validPairs);
}
/// <summary>
/// Проверяет пару углов на соответствие конструктивным параметрам робота.
/// </summary>
/// <param name="dp">Конструктивные параметры робота</param>
/// <param name="angles">Структура. Пара углов φ1 и φ2</param>
/// <returns>Истина, если пара углов соответствует конструктивным параметрам робота</returns>
public static bool IsAnglesAreValid(DesignParameters dp, AnglesOfRotation angles)
{
var phi1 = Round(angles.Phi1);
var phi2 = Round(angles.Phi2);
return((phi1 >= Round(dp.Lever1.PhiMin) && phi1 <= Round(dp.Lever1.PhiMax)) &&
(phi2 >= Round(dp.Lever2.PhiMin) && phi2 <= Round(dp.Lever2.PhiMax)));
}
/// <summary>
/// Возвращает положение центра схвата по оси Y при текущих положениях плеч робота.
/// </summary>
/// <param name="dp">Конструктивные параметры робота-манипулятора</param>
/// <returns>Положение центра схвата по оси Y</returns>
public static double GetCurrentY(DesignParameters dp)
{
return(GetY(dp, dp.Lever1.Phi, dp.Lever2.Phi));
}
/// <summary>
/// Возвращает положение центра схвата по оси Y при заданных положениях плеч робота.
/// </summary>
/// <param name="dp">Конструктивные параметры робота-манипулятора</param>
/// <param name="phi1">Угол поворота плеча (φ1)</param>
/// <param name="phi2">Угол поворота плеча (φ2)</param>
/// <returns>Положение центра схвата по оси Y</returns>
public static double GetY(DesignParameters dp, double phi1, double phi2)
{
return(dp.Lc + (dp.L2 * Math.Sin(phi2 * deg)) + (dp.L1 * Math.Sin(phi1 * deg)));
}
/// <summary>
/// Возвращает положение центра схвата по оси X при заданных положениях плеч робота.
/// </summary>
/// <param name="dp">Конструктивные параметры робота-манипулятора</param>
/// <param name="phi1">Угол поворота плеча (φ1)</param>
/// <param name="phi2">Угол поворота плеча (φ2)</param>
/// <returns>Положение центра схвата по оси X</returns>
public static double GetX(DesignParameters dp, double phi1, double phi2)
{
return((dp.L2 * Math.Cos(phi2 * deg)) + (dp.L1 * Math.Cos(phi1 * deg)));
}
/// <summary>
/// <c>AnglesCalculation</c> - Класс для вычисления значений углов поворота плеча (φ1 и φ2) по координатам центра схвата.
/// </summary>
/// <param name="designParams">Конструктивные параметры манипулятора</param>
private AnglesCalculation(DesignParameters designParams)
{
this.dp = designParams;
}
