Пример #1
0
        public void LaserStart()
        {
            Laser.Init();

            #region vars
            int temp_x = 0;
            #endregion

            Thread laser_thrd = new Thread(new ThreadStart(LaserThread));
            laser_thrd.Start();



            void LaserThread()
            {
                while (true)
                {
                    Dispatcher.CurrentDispatcher.Invoke(() => {
                        Laser.GetProfile(out double[] X, out double[] Z);
                        List <LPoint> data = Helper.GetLaserData(X, Z, true);
                        LPoint res         = LVoronej.Type1_1point(data);

                        RPoint findPoint = Transform.Trans(new RPoint(temp_x, 0, 0, 0, 0, 0), res);
                        if (Viewer2D)
                        {
                            _LV.SetData(data);
                            _LV.SetPoint(res);
                        }
                        if (Map2D)
                        {
                            _map2d.AddLaserPoint(findPoint);
                        }
                        if (Map3D)
                        {
                            _map3d.AddPoint(findPoint.ToDoubleMas());
                        }
                    });
                    Thread.Sleep(36);
                }
                void InvokerFun()
                {
                }
            }
        }
Пример #2
0
        public void Program1()
        {
            Laser.Init();

            RPoint send_point = new RPoint();
            bool   isFirst    = true;
            bool   start      = false;
            bool   finish     = false;

            //LPoint lp = new LPoint(-11.475, 217.6);
            //RPoint rp = new RPoint(699.44, -66.74, 247.03, -1.99, -1.12, -27.76);
            //var tr = Transform.Trans(rp, lp);
            //Console.WriteLine(tr.ToString());

            List <RPoint> line1 = new List <RPoint>();
            List <RPoint> line2 = new List <RPoint>();

            LViewer aprx_v = new LViewer("Approx");

            aprx_v.Show();

            List <LPoint> aprox_points = new List <LPoint>();
            List <RPoint> seam_points  = new List <RPoint>();
            List <RPoint> seam_points2 = new List <RPoint>();

            List <List <LPoint> > lines = new List <List <LPoint> >();

            List <RPoint> show = new List <RPoint>();

            //RPoint s1 = new RPoint(732.26, -226.4, 368.73);
            //RPoint s2 = new RPoint(901.1, -245.6, 369);
            //RPoint f1 = new RPoint(727.0475, -262.422, 370.675);
            //RPoint f2 = new RPoint(894.9, -260.64, 366.07);

            RPoint s1 = new RPoint(700, -100, 300);
            RPoint s2 = new RPoint(800, -130, 320);
            RPoint f1 = new RPoint(600, -100, 300);
            RPoint f2 = new RPoint(700, -115, 310);


            var pp = RobotCalculate.BaseRotation(s1, s2, f1, f2);

            Console.WriteLine($"Rotation  = {pp.ToString()}");
            _map3d.AddPoint(s1.ToDoubleMas());
            _map3d.AddPoint(s2.ToDoubleMas());
            _map3d.AddPoint(f1.ToDoubleMas(), 1);
            _map3d.AddPoint(f2.ToDoubleMas(), 1);



            //Thread thrd = new Thread(new ThreadStart(t_work)
            //Thread thrd = new Thread(new ThreadStart(LaserApprox));
            Thread thrd = new Thread(new ThreadStart(test_1));

            thrd.Start();



            void t_work()
            {
                while (true)
                {
                    Dispatcher.Invoke(InvokerFun);

                    Thread.Sleep(100);
                }
                void InvokerFun()
                {
                    tb_position.Text     = s.Position.ToString();
                    tb_recive_point.Text = s.recive_p.ToString();
                    // Console.WriteLine(s.work);
                    tb_work.Text       = s.rec.ToString();
                    tb_send_point.Text = send_point.ToString();

                    tb_strRecive.Text = s.StrRecive;


                    Laser.GetProfile(out double[] X, out double[] Z);
                    List <LPoint> data = Helper.GetLaserData(X, Z, true);

                    if (data.Count != 0)
                    {
                        var tee = LFilters.GetLineCount(data, minLineLenght: 50, diff: 100);
                        _LV.SetData(data);
                        if (tee == 2)
                        {
                            //  Console.WriteLine(tee);
                            // Console.Beep(1000, 100);
                        }
                    }



                    LPoint res = new LPoint(0, 0);

                    if (data.Count != 0 && test)
                    {
                        _LV.SetData(data);
                        if (LFilters.GetLineCount(data, minLineLenght: 50, diff: 100) > 1)
                        {
                            res = LVoronej.Type1_1point(data);
                            RPoint findPoint = Transform.Trans(s.Position, res);
                            tb_cur_point.Text = findPoint.ToString();
                            _LV.SetPoint(res);
                            _map2d.AddLaserPoint(findPoint);
                            _map3d.AddPoint(findPoint.ToDoubleMas(), 1);
                            //for (int i = 0; i < data.Count; i++) {
                            //    var t1 = Transform.Trans(s.Position, data[i]);
                            //    _map3d.AddPoint(t1.ToDoubleMas());
                            //}

                            if (l1)
                            {
                                line1.Add(findPoint);
                            }
                            else if (l2)
                            {
                                line2.Add(findPoint);
                            }
                            //if (cal) {
                            //    send_point = RobotCalculate.CalculatePoint_2line(line1, line2);
                            //    _map3d.AddPoint(send_point.ToDoubleMas(),2);
                            //    cal = false;
                            //}
                        }



                        //if (Viewer2D) {
                        //    _LV.SetData(data);
                        //}
                        //if (Map2D) {
                        //    _map2d.AddLaserPoint(findPoint);
                        //}
                        //if (Map3D) {
                        //    _map3d.AddPoint(findPoint.ToDoubleMas());
                        //}
                    }
                    if (cal)
                    {
                        send_point = RobotCalculate.CalculatePoint_2line(line1, line2);
                        _map3d.AddPoint(send_point.ToDoubleMas(), 2);
                        cal = false;
                    }



                    //if (data.Count != 0) {
                    //    Console.WriteLine(LFilters.GetLineCount(data, minLineLenght: 50, diff: 40));

                    //    if (s.work) {
                    //    start = true;
                    //        if (isFirst) {
                    //            line1.Add(findPoint);
                    //        } else {
                    //            line2.Add(findPoint);
                    //        }
                    //    }
                    //} else {
                    //    if (start) {
                    //        isFirst = false;
                    //        finish = true;
                    //    }
                    //    if (finish) {
                    //        // var t = RobotCalculate.CalculatePoint_2line(line1, line2);
                    //        // send_point = t;
                    //    }
                    //}



                    #region
                    //if (s.work) {
                    //    start = true;
                    //    if (data.Count != 0) {

                    //        RPoint findPoint = Transform.Trans(s.Position, res);
                    //        tb_cur_point.Text = findPoint.ToString();

                    //        if (Viewer2D) {
                    //            _LV.SetData(data);
                    //            _LV.SetPoint(res);
                    //        }
                    //        if (Map2D) {
                    //            _map2d.AddLaserPoint(findPoint);
                    //        }
                    //        if (Map3D) {
                    //            _map3d.AddPoint(findPoint.ToDoubleMas());
                    //        }

                    //        if (isFirst) {
                    //            line1.Add(findPoint);
                    //        } else {
                    //            line2.Add(findPoint);
                    //        }
                    //    }
                    //} else {
                    //    if (start) {
                    //        isFirst = false;
                    //        finish = true;
                    //    }
                    //    if (finish) {
                    //       // var t = RobotCalculate.CalculatePoint_2line(line1, line2);
                    //       // send_point = t;
                    //    }
                    //}
                    #endregion



                    //Dispatcher.Invoke(() => {

                    //});
                }
            }

            void LaserApprox()
            {
                var temp_point = Point_X(new LPoint(-25, 191.55), new LPoint(-10, 201.77), new LPoint(10, 167.04), new LPoint(25, 146.053));

                // Console.WriteLine($"X = {temp_point.X} | Z = {temp_point.Z}");

                while (true)
                {
                    Dispatcher.Invoke(InvokerFun);
                    Thread.Sleep(500);
                }
                void InvokerFun()
                {
                    Laser.GetProfile(out double[] X, out double[] Z);
                    List <LPoint> data = Helper.GetLaserData(X, Z, true);

                    data = LFilters.AveragingVerticalPro(data, _everyPoint: true);
                    tb_strRecive.Text = s.StrRecive;


                    if (data.Count != 0 && test)
                    {
                        aprox_points = new List <LPoint>();
                        lines        = new List <List <LPoint> >();
                        _LV.SetData(data);
                        int line_count = LFilters.GetLineCount(data, minLineLenght: data.Count / 6, diff: 70);
                        if (line_count > 0)
                        {
                            //var aprx_mas = LFilters.GetLines(data, minLineLenght: 50, diff: 100);
                            //_LV.SetPoints(aprx_mas);
                            // Console.WriteLine(line_count);

                            var lp  = LVoronej.Type1_1point(data);
                            var t23 = Transform.Trans(s.Position, lp);
                            if (seam_points2.Count > 0)
                            {
                                double xDif = 5;
                                double yDif = 1;
                                double zDif = 1;

                                double x1 = seam_points2[seam_points2.Count - 1].X;
                                double y1 = seam_points2[seam_points2.Count - 1].Y;
                                double z1 = seam_points2[seam_points2.Count - 1].Z;
                                double x2 = t23.X;
                                double y2 = t23.Y;
                                double z2 = t23.Z;
                                if (Math.Abs(x1 - x2) < xDif)
                                {
                                    if (Math.Abs(y1 - y2) < yDif)
                                    {
                                        if (Math.Abs(z1 - z2) < zDif)
                                        {
                                            seam_points2.Add(t23);
                                            _map3d.AddPoint(t23.ToDoubleMas(), 1);
                                        }
                                    }
                                }
                            }
                            else
                            {
                                seam_points2.Add(t23);
                                _map3d.AddPoint(t23.ToDoubleMas(), 1);
                            }


                            List <LPoint> data_aprx = new List <LPoint>();
                            var           aprx_mas  = LFilters.GetLinesByIndexes(data, minLineLenght: data.Count / 6, diff: 70);
                            for (int i = 0; i < aprx_mas.Count; i += 2)
                            {
                                int           start_indx  = aprx_mas[i];
                                int           finish_indx = aprx_mas[i + 1];
                                List <LPoint> l1          = new List <LPoint>();
                                for (int j = start_indx; j <= finish_indx; j++)
                                {
                                    l1.Add(data[j]);
                                }
                                l1 = LFilters.LineAprox(l1);
                                lines.Add(l1);
                                foreach (var item in l1)
                                {
                                    data_aprx.Add(item);
                                }
                                //if (i >= 2) {
                                //    aprox_points.Add(Point_X(data[aprx_mas[i - 2]], data[aprx_mas[i - 1]], data[aprx_mas[i]], data[aprx_mas[i + 1]]));
                                //}
                            }
                            aprx_v.SetData(data_aprx);
                            // aprx_v.SetPoints(aprox_points);



                            //for (int i = 0; i < data_aprx.Count; i++) {
                            //    var t1 = Transform.Trans(s.Position, data_aprx[i]);
                            //    _map3d.AddPoint(t1.ToDoubleMas());
                            //}



                            for (int i = 1; i < lines.Count; i++)
                            {
                                var temp_res_point = Point_X(lines[i - 1][0], lines[i - 1][lines[i - 1].Count - 1], lines[i][0], lines[i][lines[i].Count - 1]);
                                //if (temp_res_point.Z != 0 && DistanceBetweenPoint(temp_res_point, lines[i - 1][lines[i - 1].Count - 1]) < 5 &&
                                //    DistanceBetweenPoint(temp_res_point, lines[i][0]) < 5) {
                                //    aprox_points.Add(temp_res_point);
                                //}
                                if (temp_res_point.Z != 0)
                                {
                                    aprox_points.Add(temp_res_point);
                                }
                            }


                            aprx_v.SetPoints(aprox_points);

                            //for (int i = 0; i < aprox_points.Count; i++) {
                            //    var t1 = Transform.Trans(s.Position, aprox_points[i]);
                            //    _map3d.AddPoint(t1.ToDoubleMas(), 2);
                            //}

                            if (aprox_points.Count > 0)
                            {
                                double minZ     = aprox_points.Min(t => t.Z);
                                LPoint minPoint = aprox_points.Single(t => t.Z == minZ);


                                var t2 = Transform.Trans(s.Position, minPoint);

                                #region С уравнением прямых TODO
                                //if (seam_points.Count > 5) {
                                //    int start_index = 5;
                                //    int finish_index = 5;
                                //    bool add = true;
                                //    while (finish_index < seam_points.Count) {
                                //        if (RobotCalculate.OnLine(seam_points[])) {

                                //        }
                                //    }
                                //} else {
                                //    seam_points.Add(t2);
                                //    _map3d.AddPoint(t2.ToDoubleMas(), 2);
                                //}
                                #endregion

                                #region Каждая точка с предыдущей
                                if (seam_points.Count > 0)
                                {
                                    double xDif = 5;
                                    double yDif = 1;
                                    double zDif = 1;

                                    double x1 = seam_points[seam_points.Count - 1].X;
                                    double y1 = seam_points[seam_points.Count - 1].Y;
                                    double z1 = seam_points[seam_points.Count - 1].Z;
                                    double x2 = t2.X;
                                    double y2 = t2.Y;
                                    double z2 = t2.Z;
                                    if (Math.Abs(x1 - x2) < xDif)
                                    {
                                        if (Math.Abs(y1 - y2) < yDif)
                                        {
                                            if (Math.Abs(z1 - z2) < zDif)
                                            {
                                                seam_points.Add(t2);
                                                _map3d.AddPoint(t2.ToDoubleMas(), 2);
                                            }
                                        }
                                    }
                                }
                                else
                                {
                                    seam_points.Add(t2);
                                    _map3d.AddPoint(t2.ToDoubleMas(), 2);
                                }

                                #endregion

                                line1.Add(t2);
                            }

                            GC.Collect();
                        }
                    }
                    if (cal)
                    {
                        int type_line1 = RobotCalculate.LineType(seam_points);
                        seam_points = RobotCalculate.LineApprox(seam_points, type_line1);
                        foreach (var item in seam_points)
                        {
                            _map3d.AddPoint(item.ToDoubleMas(), 1);
                        }
                        cal = false;
                    }
                }

                void InvokerFunR()
                {
                    tb_strRecive.Text = s.StrRecive;
                }
            }

            void test_1()
            {
                RPoint r1  = new RPoint();
                RPoint r2  = new RPoint();
                RPoint r3  = new RPoint();
                RPoint r4  = new RPoint();
                RPoint res = new RPoint();
                bool   Top = false;


                while (true)
                {
                    Dispatcher.Invoke(InvokerFun);
                    Thread.Sleep(500);
                }
                void InvokerFun()
                {
                    Laser.GetProfile(out double[] X, out double[] Z);
                    List <LPoint> data = Helper.GetLaserData(X, Z, true);

                    data = LFilters.AveragingVerticalPro(data, _everyPoint: true);

                    tb_strRecive.Text = s.StrRecive;
                    _LV.SetData(data);


                    Console.WriteLine(s.work);

                    if (data.Count != 0 && s.Position.X != double.MaxValue)
                    {
                        LPoint tempPoint = new LPoint();
                        //if (Top) {
                        //    double maxZ = data.Max(t => t.Z);
                        //    tempPoint = data.First(t => t.Z == maxZ);
                        //    _LV.SetPoint(tempPoint);
                        //} else {
                        //    double mixZ = data.Min(t => t.Z);
                        //    tempPoint = data.First(t => t.Z == mixZ);
                        //    _LV.SetPoint(tempPoint);
                        //}
                        double mixZ = data.Min(t => t.Z);
                        if (mixZ != double.NaN && mixZ > 0)
                        {
                            tempPoint = data.First(t => t.Z == mixZ);
                            _LV.SetPoint(tempPoint);
                        }



                        if (s.rec == 9)
                        {
                            var ttt = Transform.Trans(s.Position, tempPoint);
                            _map3d.AddPoint(ttt.ToDoubleMas(), 1);
                            show.Add(ttt);
                            s._MAP.Add(ttt);
                        }


                        //if (l1) {
                        //    r1 = Transform.Trans(s.Position, tempPoint);
                        //    _map3d.AddPoint(r1.ToDoubleMas(), 1);
                        //    l1 = false;
                        //} else if (l2) {
                        //    r2 = Transform.Trans(s.Position, tempPoint);
                        //    _map3d.AddPoint(r2.ToDoubleMas(), 1);
                        //    l2 = false;
                        //} else if (l3) {
                        //    r3 = Transform.Trans(s.Position, tempPoint);
                        //    _map3d.AddPoint(r3.ToDoubleMas(), 1);
                        //    l3 = false;
                        //} else if (l4) {
                        //    r4 = Transform.Trans(s.Position, tempPoint);
                        //    _map3d.AddPoint(r4.ToDoubleMas(), 1);
                        //    l4 = false;
                        //} else if (cal) {
                        //    res = RobotCalculate.CalcPoint4(r1, r2, r3, r4);
                        //    Console.WriteLine($"find point = {res.ToString()}");
                        //    _map3d.AddPoint(res.ToDoubleMas(), 2);
                        //    cal = false;
                        //}
                        //if (one) {
                        //    var tem = Transform.Trans(s.Position, tempPoint);
                        //    Console.WriteLine($"Trans point = {tem.ToString()}");
                        //    _map3d.AddPoint(tem.ToDoubleMas(), 2);
                        //    one = false;
                        //}
                    }
                }
            }
        }