/// <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);
            }
        }
Exemple #5
0
        /// <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);
        }