Esempio n. 1
0
        private Solution Line(Point a, Point b, List <Point> ContourPoint, int dot, ref Approx T)
        {
            var      LinearY  = 0;
            Solution solution = new Solution();
            var      coef     = Coefficients(a.X, a.Y, b.X, b.Y);
            var      randX    = Math.Abs((a.X + b.X) / 2) + 1;

            if (coef.Item2 == 0)
            {
                LinearY = a.Y;
            }
            else
            {
                LinearY = (int)(((-coef.Item1 * randX) - coef.Item3) / coef.Item2);
            }
            Point probe = new Point(randX, LinearY);// пробная точка

            //выявление нахождения позиции точки прямая//кривая
            if (IsPoint(randX, LinearY, ContourPoint, 1))
            {
                T.TypeLine      = Approx.TypeApprox.Linear;
                T.P0_Start      = a;
                T.P1_Bend_Point = a;
                T.P2_End        = b;
                solution.Flag   = true;
                solution.Point  = 0;
                return(solution);
            }
            else
            {
                Point         bend;
                SplineServise solutionBezie = new SplineServise();
                // определение направления
                if (b.X > a.X)
                {
                    if (b.Y > a.Y)
                    {
                        bend = new Point(b.X, b.Y);
                    }
                    else
                    {
                        bend = new Point(b.X, a.Y);
                    }
                }
                else
                {
                    if (b.Y > a.Y)
                    {
                        bend = new Point(a.X, b.Y);
                    }
                    else
                    {
                        bend = new Point(a.X, a.Y);
                    }
                }
                var BezieY = solutionBezie.QuadraticityCheck(a, bend, b, randX);
                if (IsPoint(randX, BezieY, ContourPoint, 2))
                {
                    T.TypeLine      = Approx.TypeApprox.Bezier_Curve;
                    T.P0_Start      = a;
                    T.P1_Bend_Point = bend;
                    T.P2_End        = b;
                    solution.Flag   = true;
                    solution.Point  = 0;
                    return(solution);
                }
                solution.Flag  = false;
                solution.Point = dot;
                return(solution);
            }
        }
Esempio n. 2
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);
        }