/// <summary>
/// Манипулятор остается на заданой высоте, происзодит перемещение по траектории от контура к контуру
/// </summary>
/// <param name="time">Время</param>
/// <param name="ZHight">Высота на которой происходит перемещение (постоянна)</param>
/// <param name="x1">Последняя точка первого контура</param>
/// <param name="y1">Последняя точка первого контура</param>
/// <param name="x2">Первая точка нового контура</param>
/// <param name="y2">Первая точка нового контура</param>
/// <returns></returns>
public IEnumerable <RobotPosition> PenPause(double time, double ZHight, double x1, double y1, double x2, double y2)
{
var coef = Coefficients(x1, y1, x2, y2); // коэффициенты для прямой перехода
for (double i = x1; i < x2; i++) // вычисление точек траектории
{
RobotPosition result = new RobotPosition();
var Yzn = ((-coef.Item1 * i) - coef.Item3) / coef.Item2;
result.time = time;
result.z = i;
result.x = Yzn;
result.y = ZHight;
time = 0.001 + time;
yield return(result);
}
}
/// <summary>
/// Поднятие рабочего органа манипулятора
/// </summary>
/// <param name="time">Время</param>
/// <param name="ZHight">Высота для поднятия манипулятора</param>
/// <param name="x1">Координата в которой осуществляется подьем</param>
/// <param name="y1">Координата в которой осуществляется подьем</param>
/// <param name="timeUP">Время для поднятия пера</param>
/// <param name="Zplot">Высота на которой находится раб.обл</param>
/// <param name="ZplotPause">Высота на которой будет происходить перемещение с контура на контур</param>
/// <returns></returns>
public IEnumerable <RobotPosition> PenUp(double time, double ZHight, int x1, double y1, double timeUP, double Zplot, double ZplotPause)
{
var aY = Math.Abs((2 * (ZplotPause - ZHight)) / Math.Pow(10, 6)) * 12; // ускорение по оси Y
while (ZHight <= 215)
{
RobotPosition result = new RobotPosition();
ZHight = Zplot + (aY * Math.Pow((1000 * timeUP), 2)) / 2;
result.y = ZHight;
result.time = time;
result.z = x1;
result.x = y1;
time = 0.001 + time;
timeUP = 0.001 + timeUP;
yield return(result);
}
}
/// <summary>
/// Старт, опускание рабочего органа манипулятора в первую точку контура
/// </summary>
/// <param name="time">Начальное время</param>
/// <param name="ZHight">Ноль манипулятора по Y</param>
/// <param name="YHight">Ноль манипулятора по Х </param>
/// <param name="x1">Первая точка контура</param>
/// <param name="y1">Первая точка конутра</param>
/// <param name="Zplot">Высота на которой находится раб.обл</param>
/// <returns></returns>
public IEnumerable <RobotPosition> Start(double time, double ZHight, double YHight, double x1, double y1, double Zplot)
{
var aX = (2 * (y1 - YHight) / Math.Pow(10, 6)); // ускорение по оси Х
var aY = Math.Abs((2 * (Zplot - ZHight)) / Math.Pow(10, 6)); // ускорение по оси Y
var aZ = (2 * (x1 - 0)) / Math.Pow(10, 6) * (-1); // ускорение по оси Z
while (ZHight >= Zplot)
{
RobotPosition result = new RobotPosition
{
time = time,
z = 0 - (aZ * Math.Pow(time * 1000, 2)) / 2,
x = Constants.StartY + (aX * Math.Pow(time * 1000, 2)) / 2
};
ZHight = Constants.StartZ - (aY * Math.Pow((1000 * time), 2)) / 2;
result.y = ZHight;
time = 0.001 + time;
ZHight--;
yield return(result);
}
}
/// <summary>
/// Поднятие рабочего органа манипулятора
/// </summary>
/// <param name="time">Время</param>
/// <param name="ZHight">Высота на которую поднимают (Z)</param>
/// <param name="YHight">Координата манипулятора (Y)</param>
/// <param name="x1">Последняя координата контура</param>
/// <param name="y1">Последняя координата контура</param>
/// <param name="Zplot">Высота рабочей области</param>
/// <param name="timeUP">Время подьема</param>
/// <returns></returns>
public IEnumerable <RobotPosition> Stop(double time, double ZHight, double YHight, double x1, double y1, double Zplot, double timeUP)
{
var aX = (2 * (YHight - y1) / Math.Pow(10, 6)); // ускорение по оси Х
var aY = Math.Abs((2 * (ZHight - Zplot)) / Math.Pow(10, 6)); // ускорение по оси Y
var aZ = (2 * (0 - x1)) / Math.Pow(10, 6) * (-1); // ускорение по оси Z
var a = Zplot;
while (a <= Constants.StartZ)
{
RobotPosition result = new RobotPosition
{
time = time,
z = x1 - (aZ * Math.Pow(timeUP * 1000, 2)) / 2,
x = y1 + (aX * Math.Pow(timeUP * 1000, 2)) / 2
};
a = Zplot + (aY * Math.Pow((1000 * timeUP), 2)) / 2;
result.y = a;
timeUP = 0.001 + timeUP;
time = 0.001 + time;
a++;
yield return(result);
}
var g = (int)(time + 1);
while (time <= g)
{
RobotPosition result = new RobotPosition
{
time = time,
z = 0,
x = YHight,
y = ZHight
};
time = 0.001 + time;
yield return(result);
}
}
/// <summary>
/// Интерпритация команд для робота (вычисление траекторий)
/// </summary>
/// <param name="points">Входной лист точек всего контура</param>
/// <param name="Zplot">Высота на которой находится раб.обл</param>
/// <param name="ZplotPause">Высота на которой будет происходить перемещение с контура на контур</param>
/// <returns></returns>
public IEnumerable <RobotPosition> InterpretationOfCommands(IList <ApproxRobot> points, double Zplot, double ZplotPause, double aP, double bP)
{
TimeAll Times = new TimeAll
{
Time = 1 * 1e-3
};
CommandServise Command = new CommandServise();
// Манипулятор из точки(0,390,687) будет медленно опускаться в первую точку контура(x1, y1, z0)
var STRUCT = Command.Start(Times.Time, Constants.StartZ, Constants.StartY, points[0].Approxes[0].P0_Start.X - 100, points[0].Approxes[0].P0_Start.Y + Constants.StartYPlot, Zplot);
foreach (var position in STRUCT)
{
RobotPosition result = new RobotPosition
{
x = position.x,
y = position.y,
z = position.z,
time = position.time
};
Times.Time = 0.001 + Times.Time;
yield return(result);
}
for (int i = 0; i < 1; i++) // Коллекция i - ых контуров ( количество найденых 0..n)
{
for (int m = 0; m < points[i].Approxes.Count; m++) // Обращение к элементам коллекции
{
///рисуем контур на плоскости
SplineServise spline = new SplineServise();
var localtime = 0.0;
var start = (double)(points[i].Approxes[m].P0_Start.X - 100);
double fort = points[i].Approxes[m].P0_Start.Y + Constants.StartYPlot;
var end = points[i].Approxes[m].P2_End.X - 100;
if (start == end) // на одной линии
{
var startY = points[i].Approxes[m].P0_Start.Y + Constants.StartYPlot;
var endY = points[i].Approxes[m].P2_End.Y + Constants.StartYPlot;
var expX = 0.0;
var expY = 0.0;
while (endY < startY)
{
RobotPosition result = new RobotPosition
{
z = spline.PositionXLinear(points[i].Approxes[m].P0_Start.X - 100, points[i].Approxes[m].P2_End.X - 100, localtime),
x = spline.PositionYLinear(points[i].Approxes[m].P0_Start.Y + Constants.StartYPlot, points[i].Approxes[m].P2_End.Y + Constants.StartYPlot, localtime),
y = Zplot,
time = Times.Time
};
expX = start - result.z;
expY = startY - result.x;
startY = result.x;
localtime = 0.025 + localtime;
if (((Math.Abs(expX) > 0) && (Math.Abs(expX) < 0.1)) || ((Math.Abs(expY) > 0) && (Math.Abs(expY) < 0.1)))
{
continue;
}
else
{
Times.Time = 0.001 + Times.Time; yield return(result);
}
}
}
if (end > start) // проход влево
{
var expX = 0.0;
var expY = 0.0;
if (points[i].Approxes[m].TypeLine == Approx.TypeApprox.Linear)
{
//(1)
while (end > start)
{
RobotPosition result = new RobotPosition
{
z = spline.PositionXLinear(points[i].Approxes[m].P0_Start.X - 100, points[i].Approxes[m].P2_End.X - 100, localtime),
x = spline.PositionYLinear(points[i].Approxes[m].P0_Start.Y + Constants.StartYPlot, points[i].Approxes[m].P2_End.Y + Constants.StartYPlot, localtime),
y = Zplot,
time = Times.Time
};
expX = start - result.z;
expY = fort - result.x;
start = result.z;
fort = result.x;
localtime = 0.025 + localtime;
if (Accuracy(expX, expY, aP, bP) == true)
{
Times.Time = 0.001 + Times.Time;
yield return(result);
}
else
{
continue;
}
}
}
if (points[i].Approxes[m].TypeLine == Approx.TypeApprox.Bezier_Curve)
{
while (end > start)
{
RobotPosition result = new RobotPosition
{
z = spline.PositionX(points[i].Approxes[m].P0_Start.X - 100, points[i].Approxes[m].P1_Bend_Point.X - 100, points[i].Approxes[m].P2_End.X - 100, localtime),
x = spline.PositionY(points[i].Approxes[m].P0_Start.Y + Constants.StartYPlot, points[i].Approxes[m].P1_Bend_Point.Y + Constants.StartYPlot, points[i].Approxes[m].P2_End.Y + Constants.StartYPlot, localtime),
y = Zplot,
time = Times.Time
};
expX = start - result.z;
expY = fort - result.x;
start = result.z;
fort = result.x;
localtime = 0.025 + localtime;
if (Accuracy(expX, expY, aP, bP) == true)
{
Times.Time = 0.001 + Times.Time;
yield return(result);
}
else
{
continue;
}
}
}
}
else // проход вправо
{
var expX = 0.0;
var expY = 0.0;
if (points[i].Approxes[m].TypeLine == Approx.TypeApprox.Linear)
{
while (start > end)
{
RobotPosition result = new RobotPosition
{
z = spline.PositionXLinear(points[i].Approxes[m].P0_Start.X - 100, points[i].Approxes[m].P2_End.X - 100, localtime),
x = spline.PositionYLinear(points[i].Approxes[m].P0_Start.Y + Constants.StartYPlot, points[i].Approxes[m].P2_End.Y + Constants.StartYPlot, localtime),
y = Zplot,
time = Times.Time
};
expX = start - result.z;
expY = fort - result.x;
start = result.z;
fort = result.x;
localtime = 0.025 + localtime;
if (Accuracy(expX, expY, aP, bP) == true)
{
Times.Time = 0.001 + Times.Time;
yield return(result);
}
else
{
continue;
}
}
}
if (points[i].Approxes[m].TypeLine == Approx.TypeApprox.Bezier_Curve)
{
while (start > end)
{
RobotPosition result = new RobotPosition
{
z = spline.PositionX(points[i].Approxes[m].P0_Start.X - 100, points[i].Approxes[m].P1_Bend_Point.X - 100, points[i].Approxes[m].P2_End.X - 100, localtime),
x = spline.PositionY(points[i].Approxes[m].P0_Start.Y + Constants.StartYPlot, points[i].Approxes[m].P1_Bend_Point.Y + Constants.StartYPlot, points[i].Approxes[m].P2_End.Y + Constants.StartYPlot, localtime),
y = Zplot,
time = Times.Time
};
expX = start - result.z;
expY = fort - result.x;
start = result.z;
fort = result.x;
localtime = 0.025 + localtime;
if (Accuracy(expX, expY, aP, bP) == true)
{
Times.Time = 0.001 + Times.Time;
yield return(result);
}
else
{
continue;
}
}
}
}
}
///если контур последний , выход из цикла рисования
if (i == points.Count - 1)
{
break;
}
else
{
//// поднятие рабочего органа манипулятора в точке
//STRUCT = Command.PenUp(Times.Time, Zplot, points[i].Approxes[points[i].Approxes.Count - 1].P2_End.X - 100, points[i].Approxes[points[i].Approxes.Count - 1].P2_End.Y + Constants.StartYPlot, Constants.timeUp, Zplot, ZplotPause);
//foreach (var position in STRUCT)
//{
// RobotPosition result = new RobotPosition
// {
// x = position.x,
// y = position.y,
// z = position.z,
// time = position.time
// };
// Times.Time = 0.001 + Times.Time;
// yield return result;
//}
//// вычисление тракетории перемещения из(x1, y1, z0) в(x2, y2, z0)
//STRUCT = Command.PenPause(Times.Time, ZplotPause, points[i].Approxes[points[i].Approxes.Count - 1].P2_End.X - 100, points[i].Approxes[points[i].Approxes.Count - 1].P2_End.Y + Constants.StartYPlot, points[i + 1].Approxes[points[i + 1].Approxes.Count - 1].P0_Start.X - 100, points[i + 1].Approxes[points[i + 1].Approxes.Count - 1].P0_Start.Y + Constants.StartYPlot);
//foreach (var position in STRUCT)
//{
// RobotPosition result = new RobotPosition
// {
// x = position.x,
// y = position.y,
// z = position.z,
// time = position.time
// };
// Times.Time = 0.001 + Times.Time;
// yield return result;
//}
//// опускание рабочего органа манипулятора в точке
//STRUCT = Command.PenDown(Times.Time, ZplotPause, points[i + 1].Approxes[points[i + 1].Approxes.Count - 1].P0_Start.X - 100, points[i + 1].Approxes[points[i + 1].Approxes.Count - 1].P0_Start.Y + Constants.StartYPlot, Constants.timeUp, Zplot);
//foreach (var position in STRUCT)
//{
// RobotPosition result = new RobotPosition
// {
// x = position.x,
// y = position.y,
// z = position.z,
// time = position.time
// };
// Times.Time = 0.001 + Times.Time;
// yield return result;
//}
}
}
// поднятие рабочего органа манипулятора
//STRUCT = Command.Stop(Times.Time, Constants.StartZ, Constants.StartY, points[points.Count - 1].Approxes[points[points.Count - 1].Approxes.Count - 1].P2_End.X - 100, points[points.Count - 1].Approxes[points[points.Count - 1].Approxes.Count - 1].P2_End.Y + Constants.StartYPlot, Zplot, Constants.timeUp);
//foreach (var position in STRUCT)
//{
// RobotPosition result = new RobotPosition
// {
// x = position.x,
// y = position.y,
// z = position.z,
// time = position.time
// };
// Times.Time = 0.001 + Times.Time;
// yield return result;
//}
StateStorageService el = new StateStorageService();
el.SaveMatlabTime(Times);
}