修复防撞雷达卡顿bug，雷达更新耗时低于7ms，PLC更新可达到20ms以内。

encTest/Form1.cs

@@ -20,7 +20,7 @@ namespace Test
     {
         private SickTimEnc laserLeft;
         private SickTimEnc laserRight;
-        private int max_axis_Y = 2600;//2250;//画图时Y轴最大量程
+        private int max_axis_Y = 2000;//2250;//画图时Y轴最大量程
         private List<DispMsg> dataDispList;
         private List<string> colors;//雷达数据颜色
         private bool obstacleDetected;
@@ -97,8 +97,8 @@ namespace Test
             //初始化雷达与数据显示
             Task.Factory.StartNew(() =>
             {
-                //laserLeft = new SickTimEnc("192.168.0.77", 2111);
-                laserRight = new SickTimEnc("192.168.0.88", 2111);
+                laserLeft = new SickTimEnc("192.168.0.77", 2111);
+                //laserRight = new SickTimEnc("192.168.0.88", 2111);
                 while (!IsHandleCreated)
                 {
                     Thread.Sleep(500);
@@ -110,13 +110,13 @@ namespace Test
                     if (laserLeft != null)
                     {
                         Pause();
-                        laserLeft.SetLeftRange(65);
-                        laserLeft.SetRightRange(115);
-                        laserLeft.ground_truth_height = 4050;
+                        laserLeft.SetLeftRange(75);
+                        laserLeft.SetRightRange(105);
+                        laserLeft.ground_truth_height = 1700;
                         laserLeft.min_num_point_of_hinder = 20;
-                        laserLeft.min_obstacle_height = 100;
-                        tb_angleLeft.Text = "65";
-                        tb_angleRight.Text = "115";
+                        laserLeft.min_obstacle_height = 50;
+                        tb_angleLeft.Text = "75";
+                        tb_angleRight.Text = "105";
                         Pause();
                         List<List<double>> temp = new List<List<double>>();
                         temp.Add(laserLeft.yFiltered);
@@ -126,11 +126,11 @@ namespace Test
                     if (laserRight != null)
                     {
                         Pause();
-                        laserRight.SetLeftRange(75);
-                        laserRight.SetRightRange(105);
-                        laserRight.ground_truth_height = 2200;
+                        laserRight.SetLeftRange(45);
+                        laserRight.SetRightRange(135);
+                        laserRight.ground_truth_height = 600;
                         laserRight.min_num_point_of_hinder = 20;
-                        laserRight.min_obstacle_height = 100;
+                        laserRight.min_obstacle_height = 50;
                         tb_angleLeft.Text = "75";
                         tb_angleRight.Text = "125";
                         Pause();
@@ -181,108 +181,127 @@ namespace Test
                     }
                     catch { Stop(); }
                     sw.Stop();
-                    Console.WriteLine("laser: " + sw.ElapsedMilliseconds + "ms");
+                    Console.WriteLine("laser: " + sw.ElapsedMilliseconds + "ms"+", obstacle: "+obstacleDetected);
                     Thread.Sleep(30);
                 }
             });
 
-            ////************* PLC 相关逻辑 *************
-            //bool result = PLCConnect();
-            //if (!result)
-            //{
-            //    Console.WriteLine("PLC连接失败");
-            //    Environment.Exit(0);
-            //}
-            //else
-            //{
-            //    MsgNode msg = new MsgNode();
-            //    msg.db = DB;
-            //    msg.length = 2;
-            //    Task.Factory.StartNew(() =>
-            //    {
-            //        while (!closed)
-            //        {
-            //            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
-            //            sw.Start();
-            //            if (plc == null)
-            //            {
-            //                Thread.Sleep(1000);
-            //                continue;
-            //            }
-            //            msg.start = ZPosition;
-            //            ReadFromPLC(ref msg);
-            //            ZPositionValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
-
-            //            msg.start = ZStart;
-            //            ReadFromPLC(ref msg);
-            //            ZStartValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
-
-            //            msg.start = ZStop;
-            //            ReadFromPLC(ref msg);
-            //            ZStopValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
-
-            //            msg.start = ZPause;
-            //            ReadFromPLC(ref msg);
-            //            ZPauseValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
-            //            //Console.WriteLine("position, start, stop, pause: " + ZPositionValue + ", " + ZStartValue + ", " + ZStopValue + ", " + ZPauseValue);
-
-            //            if (ZPauseValue == 1)
-            //            {
-            //                Pause();
-            //                //Console.WriteLine("暂停");
-            //            }
-            //            else
-            //            {
-            //                Pause();
-            //                //Console.WriteLine("恢复");
-            //            }
-
-            //            //收到开始下降信号
-            //            if (ZStartValue == 1 && ZStopValue != 1)
-            //            {
-            //                startGoingDown = true;
-            //                //Console.WriteLine("下降过程");
-            //            }
-            //            else
-            //            {
-            //                startGoingDown = false;
-            //                //Console.WriteLine("非下降过程");
-            //            }
-
-            //            //进入下降阶段
-            //            if (startGoingDown && ZPositionValue > ZThreshold)
-            //            {
-            //                if (!laserWorking)
-            //                {
-            //                    Console.WriteLine("启动雷达");
-            //                    //Start();
-            //                    laserWorking = true;
-            //                }
-            //            }
-            //            //下降结束复位工作状态
-            //            if (laserWorking && ZStopValue == 1)
-            //            {
-            //                Console.WriteLine("雷达工作结束");
-            //                laserWorking = false;
-            //            }
-
-            //            if (laserWorking)
-            //            {
-            //                MsgNode writeMsg = new MsgNode();
-            //                writeMsg.db = 41;
-            //                writeMsg.length = 2;
-            //                writeMsg.start = 60;
-            //                byte[] temp = { 0, 0 };
-            //                if (obstacleDetected)
-            //                    temp[1] = 1;
-            //                WriteToPLC(writeMsg);
-            //            }
-            //            sw.Stop();
-            //            Console.WriteLine("PLC: " + sw.ElapsedMilliseconds + "ms");
-            //            Thread.Sleep(1000);
-            //        }
-            //    });
-            //}
+            //************* PLC 相关逻辑 *************
+            bool result = PLCConnect();
+            if (!result)
+            {
+                Console.WriteLine("PLC连接失败");
+                Environment.Exit(0);
+            }
+            else
+            {
+                isConnected = true;
+                //更新PLC数据
+                Thread PLCUpdateThread = new Thread(PLCStateMachine);
+                PLCUpdateThread.Start();
+            }
+        }
+
+        private void PLCStateMachine()
+        {
+            MsgNode msg = new MsgNode();
+            msg.db = DB;
+            msg.length = 2;
+            bool paused = false;
+            while (!closed)
+            {
+                System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
+                sw.Start();
+                if (plc == null)
+                {
+                    Thread.Sleep(1000);
+                    continue;
+                }
+                msg.start = ZPosition;
+                ReadFromPLC(ref msg);
+                ZPositionValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
+                //Thread.Sleep(1);
+
+                msg.start = ZStart;
+                ReadFromPLC(ref msg);
+                ZStartValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
+                //Thread.Sleep(1);
+
+                msg.start = ZStop;
+                ReadFromPLC(ref msg);
+                ZStopValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
+                //Thread.Sleep(1);
+
+                msg.start = ZPause;
+                ReadFromPLC(ref msg);
+                ZPauseValue = BitConverter.ToInt16(msg.value.ToArray(), 0);
+                //Console.WriteLine("position, start, stop, pause: " + ZPositionValue + ", " + ZStartValue + ", " + ZStopValue + ", " + ZPauseValue);
+                //Thread.Sleep(1);
+
+                //if (ZPauseValue == 1)
+                //{
+                //    if (!paused)
+                //    {
+                //        Pause();
+                //        paused = true;
+                //    }
+                //    //Console.WriteLine("暂停");
+                //}
+                //else
+                //{
+                //    if (paused)
+                //    {
+                //        Pause();
+                //        paused = false;
+                //    }
+                //    //Console.WriteLine("恢复");
+                //}
+
+                //收到开始下降信号
+                if (ZStartValue == 1 && ZStopValue != 1)
+                {
+                    startGoingDown = true;
+                    //Console.WriteLine("下降过程");
+                }
+                else
+                {
+                    startGoingDown = false;
+                    //Console.WriteLine("非下降过程");
+                }
+
+                //进入下降阶段
+                if (startGoingDown && ZPositionValue > ZThreshold)
+                {
+                    if (!laserWorking)
+                    {
+                        Console.WriteLine("启动雷达");
+                        //Start();
+                        laserWorking = true;
+                    }
+                }
+                //下降结束复位工作状态
+                if (laserWorking && ZStopValue == 1)
+                {
+                    Console.WriteLine("雷达工作结束");
+                    laserWorking = false;
+                }
+
+                //if (laserWorking)
+                //{
+                    MsgNode writeMsg = new MsgNode();
+                    writeMsg.db = 41;
+                    writeMsg.length = 2;
+                    writeMsg.start = 64;
+                    byte[] temp = { 0, 0 };
+                    if (obstacleDetected)
+                        temp[0] = 1;
+                    writeMsg.value = temp.ToList();
+                    WriteToPLC(writeMsg);
+                //}
+                sw.Stop();
+                Thread.Sleep(10);
+                Console.WriteLine("PLC: " + sw.ElapsedMilliseconds + "ms");
+            }
         }
         /// <summary>
         /// 暂停雷达数据处理线程
@@ -311,6 +330,7 @@ namespace Test
             {
                 laserRight.Stop();
             }
+            closed = true;
         }
 
         public obstacleDetectionForm()
@@ -373,8 +393,8 @@ namespace Test
             this.laserDataChart.ChartAreas[0].AxisY.Minimum = 0;
             this.laserDataChart.ChartAreas[0].AxisY.Maximum = max_axis_Y;
             this.laserDataChart.ChartAreas[0].AxisY.Interval = 50;
-            this.laserDataChart.ChartAreas[0].AxisX.Minimum = -2500;// -200;
-            this.laserDataChart.ChartAreas[0].AxisX.Maximum = 2500;// 200;
+            this.laserDataChart.ChartAreas[0].AxisX.Minimum = -1500;// -200;
+            this.laserDataChart.ChartAreas[0].AxisX.Maximum = 1500;// 200;
             this.laserDataChart.ChartAreas[0].AxisX.Interval = 100;// 100;
             this.laserDataChart.ChartAreas[0].AxisX.MajorGrid.LineColor = System.Drawing.Color.Silver;
             this.laserDataChart.ChartAreas[0].AxisY.MajorGrid.LineColor = System.Drawing.Color.Silver;

encTest/SickTimEnc.cs

@@ -237,8 +237,8 @@ namespace Test
                     {
                         if (working)
                         {
-                            //System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
-                            //sw.Start();
+                            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
+                            sw.Start();
                             //开辟线程存储雷达原始数据字符串
                             lock (Lock)
                             {
@@ -253,8 +253,8 @@ namespace Test
                                 }
                                 if (originDatalist.Count != 0)
                                 {
-                                    System.Diagnostics.Stopwatch sw1 = new System.Diagnostics.Stopwatch();
-                                    sw1.Start();
+                                    //System.Diagnostics.Stopwatch sw1 = new System.Diagnostics.Stopwatch();
+                                    //sw1.Start();
                                     List<byte> result = new List<byte>();
                                     for (int i = 0; i < originDatalist.Count; i++)
                                     {
@@ -344,10 +344,10 @@ namespace Test
                                         //dataCount++;
                                         //Console.WriteLine("laser:: "+dataCount);
                                     }
-                                    sw1.Stop();
-                                    Console.WriteLine("laserDataFilter: " + sw1.ElapsedMilliseconds + "ms");
                                 }
                             }
+                            sw.Stop();
+                            //Console.WriteLine("laserDataFilter: " + sw.ElapsedMilliseconds + "ms");
                             Thread.Sleep(1);
                         }
                         else