/// <summary>
/// Рассичтывает число импульсов, необходимых для достижения угла phi.
/// </summary>
/// <param name="ldp">Конструктивные параметры плеча робота-манипулятора</param>
/// <param name="phi">Угол поворота плеча, градусы</param>
/// <returns>Число импульсов</returns>
private static long CalculatePulsesCount(LeverDesignParameters ldp, double phi)
{
phi *= ldp.IsABIncreasesOnStepperCW ? -deg : deg;
double pulsesCount = ldp.IsABIncreasesOnStepperCW ? -1 : 1;
pulsesCount *= (1.0 / ldp.I) * ((ldp.AB - Math.Sqrt(Math.Pow(ldp.AO, 2) + Math.Pow(ldp.BO, 2) - (2 * ldp.AO * ldp.BO * Math.Cos(ldp.AlphaRad + ldp.BetaRad + phi)))) / (ldp.P * ldp.RoRad));
return(Convert.ToInt64(pulsesCount));
}
/// <summary>
/// Возвращает число импульсов, выдаваемых УЧПУ.
/// </summary>
/// <param name="ldp">Конструктивные параметры плеча робота-манипулятора</param>>
/// <param name="phi">Угол поворота плеча, градусы</param>
/// <returns>Число импульсов</returns>
public static long GetPulsesCount(LeverDesignParameters ldp, double phi)
{
long pulsesCount = CalculatePulsesCount(ldp, phi);
if (!IsPulsesCountCorrect(ldp, pulsesCount))
{
throw new DesignParametersException("Конструктивные параметры робота-манипулятора (или рабочая зона) не позволяют достичь заданный угол");
}
return(pulsesCount);
}
/// <summary>
/// Рассчитывает значение AB в зависимости от числа импульсов и текущего значения AB.
/// </summary>
/// <param name="ldp">Конструктивные параметры плеча робота-манипулятора</param>
/// <param name="pulsesCount">Число импульсов</param>
/// <returns>Новое значение AB</returns>
public static double GetNewAB(LeverDesignParameters ldp, long pulsesCount)
{
double newValue = CalculateAB(ldp, pulsesCount);
if (!ldp.Workspace.IsBetweenMinAndMax(newValue))
{
throw new DesignParametersException("Новое значение расстояния от оси подвеса ходового винта до точки крепления плеча к гайке ходового винта не удовлетворяет конструктивным параметрам робота или рабочей зоны");
}
return(newValue);
}
/// <summary>
/// Возвращает количество импульсов для достижения положения <paramref name="abTo"/> из положения <paramref name="abFrom"/>
/// плеча <paramref name="ldp"/>.
/// </summary>
public static long GetPulsesCount(LeverDesignParameters ldp, double abFrom, double abTo)
{
if (!ldp.Workspace.IsBetweenMinAndMax(abFrom))
{
throw new DesignParametersException("Значение расстояния от оси подвеса ходового винта до точки крепления плеча к гайке ходового винта не удовлетворяет конструктивным параметрам робота или рабочей зоны");
}
var phi = ldp.GetAngleByABValue(abTo);
phi *= ldp.IsABIncreasesOnStepperCW ? -deg : deg;
double pulsesCount = ldp.IsABIncreasesOnStepperCW ? -1 : 1;
pulsesCount *= (1.0 / ldp.I) * ((abFrom - Math.Sqrt(Math.Pow(ldp.AO, 2) + Math.Pow(ldp.BO, 2) - (2 * ldp.AO * ldp.BO * Math.Cos(ldp.AlphaRad + ldp.BetaRad + phi)))) / (ldp.P * ldp.RoRad));
return(Convert.ToInt64(pulsesCount));
}
private LeverDesignParameters GetLeverDesign(DesignType designType)
{
var design = designs.Single(d => d.DesignType == designType);
var lever = new UM160CalculationLib.LeverDesignParameters(GetValue(design.Items, ValueType.AO),
GetValue(design.Items, ValueType.BO),
GetValue(design.Items, ValueType.P),
GetValue(design.Items, ValueType.Ro),
GetValue(design.Items, ValueType.Alpha),
GetValue(design.Items, ValueType.Beta),
GetValue(design.Items, ValueType.I),
design.IsAbIncreases,
GetValue(design.Items, ValueType.AB),
GetValue(design.Items, ValueType.ABmin),
GetValue(design.Items, ValueType.ABmax));
lever.ABzero = GetAbZero(design, lever);
return(lever);
}
/// <summary>
/// Рассчитывает значение AB в зависимости от числа импульсов и текущего значения AB.
/// </summary>
/// <param name="ldp">Конструктивные параметры плеча робота-манипулятора</param>
/// <param name="pulsesCount">Число импульсов</param>
/// <returns>Новое значение AB</returns>
private static double CalculateAB(LeverDesignParameters ldp, long pulsesCount)
{
pulsesCount = ldp.IsABIncreasesOnStepperCW ? pulsesCount : -pulsesCount;
// Округляем сотые значение миллиментов.
return(Math.Round(ldp.AB + (ldp.RoRad * pulsesCount * ldp.I * ldp.P), 0));
}
/// <summary>
/// Возвращает истину если pulsesCount допустимое значение.
/// </summary>
/// <param name="ldp">Конструктивные параметры плеча робота-манипулятора</param>
/// <param name="pulsesCount">Число импульсов</param>
/// <returns>Истина, если pulsesCount допустимое значение</returns>
private static bool IsPulsesCountCorrect(LeverDesignParameters ldp, long pulsesCount)
{
return(ldp.Workspace.IsBetweenMinAndMax(CalculateAB(ldp, pulsesCount)));
}
