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