双雷达障碍物检测

Test/Form1.Designer.cs

@@ -28,9 +28,9 @@
         /// </summary>
         private void InitializeComponent()
         {
-            System.Windows.Forms.DataVisualization.Charting.ChartArea chartArea6 = new System.Windows.Forms.DataVisualization.Charting.ChartArea();
-            System.Windows.Forms.DataVisualization.Charting.Legend legend6 = new System.Windows.Forms.DataVisualization.Charting.Legend();
-            System.Windows.Forms.DataVisualization.Charting.Series series6 = new System.Windows.Forms.DataVisualization.Charting.Series();
+            System.Windows.Forms.DataVisualization.Charting.ChartArea chartArea1 = new System.Windows.Forms.DataVisualization.Charting.ChartArea();
+            System.Windows.Forms.DataVisualization.Charting.Legend legend1 = new System.Windows.Forms.DataVisualization.Charting.Legend();
+            System.Windows.Forms.DataVisualization.Charting.Series series1 = new System.Windows.Forms.DataVisualization.Charting.Series();
             this.button1 = new System.Windows.Forms.Button();
             this.chart1 = new System.Windows.Forms.DataVisualization.Charting.Chart();
             this.button2 = new System.Windows.Forms.Button();
@@ -59,16 +59,16 @@
             // 
             // chart1
             // 
-            chartArea6.Name = "ChartArea1";
-            this.chart1.ChartAreas.Add(chartArea6);
-            legend6.Name = "Legend1";
-            this.chart1.Legends.Add(legend6);
+            chartArea1.Name = "ChartArea1";
+            this.chart1.ChartAreas.Add(chartArea1);
+            legend1.Name = "Legend1";
+            this.chart1.Legends.Add(legend1);
             this.chart1.Location = new System.Drawing.Point(26, 69);
             this.chart1.Name = "chart1";
-            series6.ChartArea = "ChartArea1";
-            series6.Legend = "Legend1";
-            series6.Name = "Series1";
-            this.chart1.Series.Add(series6);
+            series1.ChartArea = "ChartArea1";
+            series1.Legend = "Legend1";
+            series1.Name = "Series1";
+            this.chart1.Series.Add(series1);
             this.chart1.Size = new System.Drawing.Size(1764, 623);
             this.chart1.TabIndex = 2;
             this.chart1.Text = "chart1";
@@ -123,9 +123,9 @@
             this.label3.AutoSize = true;
             this.label3.Location = new System.Drawing.Point(1180, 30);
             this.label3.Name = "label3";
-            this.label3.Size = new System.Drawing.Size(52, 15);
+            this.label3.Size = new System.Drawing.Size(67, 15);
             this.label3.TabIndex = 8;
-            this.label3.Text = "结果：";
+            this.label3.Text = "障碍物：";
             // 
             // label4
             // 

Test/Form1.cs

@@ -1,9 +1,12 @@
-using System;
+using snap7Enc;
+using System;
+using System.Collections.Concurrent;
 using System.Collections.Generic;
 using System.Drawing;
 using System.Linq;
 using System.Net;
 using System.Net.Sockets;
+using System.Runtime.InteropServices;
 using System.Text;
 using System.Threading;
 using System.Threading.Tasks;
@@ -12,31 +15,98 @@ using System.Windows.Forms.DataVisualization.Charting;
 
 namespace Test
 {
+
     public partial class Form1 : Form
     {
         private TcpClient tcpClient;
         NetworkStream stream;
-        private int bufferSize = 4096;
+        private int bufferSize = 16384;
         private string startCmd = "02 73 52 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 03";
         private string endCmd = "02 73 45 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 30 03";
         int flag = 0;
+        static int left_range = 385;//325;//231;//271;//386;//366;
+        static int right_range = 425;//485;//581;//541;//426;//446;
+        static int ground_truth_height = 1700;//2200;//2300;//标准高度
+        static int min_num_point_of_hinder = 30;//20;//判定为障碍物的最少点的数目
+        static int min_obstacle_height = 500;//最小障碍物高度
+        static int max_axis_Y = 2000;//2250;//画图时Y轴最大量程
         private bool isok = false;
         private static readonly object Lock = new object();
         List<string> validdatalist;
-        public Form1()
-        {
-            InitializeComponent();
-            tcpClient = new TcpClient();
-            tcpClient.Connect(IPAddress.Parse("192.168.1.31"), 2111);
-            Console.WriteLine("连接成功");
-            stream = tcpClient.GetStream();
-            validdatalist = new List<string>();
-        }
-        //开始
-        private void button1_Click(object sender, EventArgs e)
+        static List<int> errorIndex;
+        static List<double> dis;
+        List<List<double>> tempSeries_x, tempSeries_y, tempSeries_tempy;
+        List<double> x_update, y_update, templisty_update;
+        List<double> monitor_y;
+        //List<double> templistx, templisty;
+        int indexN = 0;
+        
+        //卡尔曼滤波参数
+        static List<double> prevData, p, q, r, kGain;
+
+        //************** plc相关参数 *************
+        /// <summary>
+        /// PLC 连接状态flag
+        /// </summary>
+        public bool isConnected = false;
+        /// <summary>
+        /// plc中cpu类型
+        /// </summary>
+        private const CpuType cpu = CpuType.S71200;
+        /// <summary>
+        /// PLC的IP地址
+        /// </summary>
+        private const string ip = "192.168.0.1";
+        /// <summary>
+        /// PLC的端口号
+        /// </summary>
+        private const Int16 rack = 0;
+        /// <summary>
+        /// 工作站号
+        /// </summary>
+        private const Int16 slot = 1;
+        /// <summary>
+        /// PLC S7连接对象
+        /// </summary>
+        protected static Plc plc = null;
+        /// <summary>
+        /// 用于将写PLC指令排队的阻塞队列
+        /// </summary>
+        private BlockingCollection<MsgNode> writingBlockingCollection = new BlockingCollection<MsgNode>();
+        /// <summary>
+        /// 写线程，控制PLC写入频率
+        /// </summary>
+        private Thread writeThread;
+        /// <summary>
+        /// 系统关闭
+        /// </summary>
+        private bool closed;
+        /// <summary>
+        /// 读写锁
+        /// </summary>
+        private object readWriteLock = new object();
+        //写入频率
+        private const int writeFreq = 25;
+        private const short DB = 30;
+        private const short ZPosition = 8;
+        private const short ZThreshold = 200;
+        private const short ZStart = 10;
+        private const short ZStop = 12;
+        private const short ZPause = 14;
+        private short ZPositionValue = 0;
+        private short ZStartValue = 0;
+        private short ZStopValue = 0;
+        private short ZPauseValue = 0;
+        private bool startGoingDown = false;
+        private bool laserWorking = false;
+
+        //流程控制
+        private void Start()
         {
+            bool isObstacle = false;
             isok = false;
             List<string> datalist = new List<string>();
+            //开辟线程存储雷达原始数据字符串
             Task.Factory.StartNew(() =>
             {
                 while (!isok)
@@ -50,7 +120,7 @@ namespace Test
                         string str = HexStringToASCII(buffer);
                         datalist.Add(str);
                     }
-                    Thread.Sleep(200);
+                    Thread.Sleep(1);
                 }
             });
             //string testimg = "";
@@ -95,28 +165,218 @@ namespace Test
                                     listx.Add(x);
                                 }
                                 this.chart1.Series[0].Points.Clear();
-                                InitChart(listx, listy);
-                                List<double> dis = lineFit(listx, listy);
+                                //tempSeries_x.Add(listx);
+
+                                List<double> templisty = new List<double>();
+                                for (int j = 0; j < listy.Count; j++)
+                                {
+                                    templisty.Add(listy[j]);
+                                }
+                                tempSeries_tempy.Add(templisty);
+                                listy = kalmanFilter(listy);
+                                tempSeries_y.Add(listy);
+
+
+                                if (tempSeries_y.Count >= 4)
+                                {
+                                    tempSeries_y.RemoveAt(0);
+                                    tempSeries_tempy.RemoveAt(0);
+                                }
+
+                                if (tempSeries_y.Count == 3)
+                                {
+                                    y_update = dataFilter(tempSeries_y, out isObstacle);
+                                    templisty_update = dataFilter(tempSeries_tempy, out isObstacle);
+                                    this.label4.Text = isObstacle.ToString();
+                                    InitChart(listx, y_update, templisty_update);
+                                }
+                                else
+                                {
+                                    //InitChart(listx, listy,templisty);//
+                                }
+
+                                //List<double> dis = lineFit(listx, listy);
+                                //List<double> dis = lineFit2(listy);
                                 double difference = Convert.ToDouble(this.textBox3.Text);
-                                bool isHinder = IsHinder(dis, difference);
-                                this.label4.Text = isHinder.ToString();
+                                //bool isHinder = IsHinder(dis, difference);
+
                                 sw.Stop();
                                 Console.WriteLine(sw.ElapsedMilliseconds + "ms");
-                                if (isHinder)
-                                {
-                                    Console.WriteLine("结果：" + isHinder);
-                                    Console.WriteLine(sw.ElapsedMilliseconds + "ms");
-                                }
+                                //if (isHinder)
+                                //{
+                                //    Console.WriteLine("结果：" + isHinder);
+                                //    Console.WriteLine(sw.ElapsedMilliseconds + "ms");
+
+                                //    for (int j = 0; j < listx.Count; j++)
+                                //    {
+                                //        Log.WriteData("ordata" + indexN, origindataTransformed[startDegree + j].ToString());
+                                //        if (errorIndex.Exists(o => o == j + 1))
+                                //        {
+                                //            Log.WriteData("data" + indexN, origindataTransformed[startDegree + j] + " " + 1);
+                                //        }
+                                //        else
+                                //        {
+                                //            Log.WriteData("data" + indexN, origindataTransformed[startDegree + j] + " " + 0);
+                                //        }
+                                //    }
+                                //    indexN++;
+                                //}                                
                             }));
-                            
+
                         }
                     }
-                    Thread.Sleep(10);
+                    Thread.Sleep(1);
                 }
             });
+        }
+        private void Pause(bool ctrl)
+        {
+            isok = ctrl;
+        }
+        private void Stop()
+        {
+            isok = true;
+            flag = sendCmd(endCmd);
+        }
 
+        public Form1()
+        {
+            InitializeComponent();
+            validdatalist = new List<string>();
+            tempSeries_x = new List<List<double>>();
+            tempSeries_y = new List<List<double>>();
+            tempSeries_tempy = new List<List<double>>();
+            monitor_y = new List<double>();
+            errorIndex = new List<int>();
+            prevData = new List<double>();
+            p = new List<double>();
+            q = new List<double>();
+            r = new List<double>();
+            kGain = new List<double>();
+            tcpClient = new TcpClient();
+            tcpClient.Connect(IPAddress.Parse("192.168.0.77"), 2111);
+            Console.WriteLine("连接成功");
+            stream = tcpClient.GetStream();
+            //templisty = new List<double>();
+            for (int i=0;i < right_range-left_range;i++)
+            {
+                prevData.Add(0);
+                p.Add(450);
+                q.Add(0.0001);
+                r.Add(0.005);
+                kGain.Add(0);
+            }
+
+            //bool result = PLCConnect();
+            //if (!result)
+            //    Environment.Exit(0);
+            //else
+            //{
+            //    MsgNode msg = new MsgNode();
+            //    msg.db = DB;
+            //    msg.length = 2;
+            //    Task.Factory.StartNew(() => {
+            //        while (!closed)
+            //        {
+            //            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(true);
+            //                Console.WriteLine("暂停");
+            //            }
+            //            else
+            //            {
+            //                Pause(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;
+            //            }
+
+            //            Thread.Sleep(50);
+            //        }
+            //    });
+            //}
+        }
+        //界面按钮
+        private void button1_Click(object sender, EventArgs e)
+        {
+            Start();  
+        }
+        private void button3_Click(object sender, EventArgs e)
+        {
+            isok = true;
+        }
+        private void button2_Click(object sender, EventArgs e)
+        {
+            isok = true;
+            //for (int i = 0; i < validdatalist.Count; i++)
+            //{
+            //    string temp = validdatalist[i];
+            //    string[] tempdata = temp.Split(' ');
+            //    for (int j = 0; j < tempdata.Length; j++)
+            //    {
+            //        Log.WriteData("data" + i, tempdata[j]);
+            //    }
+            //}
+            //datalist.Clear();
+            flag = sendCmd(endCmd);
+            //Environment.Exit(0);
+            //this.Close();
         }
 
+        /// <summary>
+        /// 向雷达发送指令
+        /// </summary>
+        /// <param name="cmd"></param>
+        /// <returns></returns>
         private int sendCmd(string cmd)
         {
             if (cmd.Equals(startCmd))
@@ -182,7 +442,7 @@ namespace Test
             return buff;
         }
         /// <summary>
-        /// 获取数据
+        /// 截取数据段
         /// </summary>
         /// <param name="sourse"></param>
         /// <param name="startstr"></param>
@@ -215,23 +475,38 @@ namespace Test
         /// <summary>
         /// 初始化图表
         /// </summary>
-        private void InitChart(List<double> listx, List<double> listy)
+        private void InitChart(List<double> listx, List<double> listy, List<double> templisty)
         {
             //定义图表区域
             this.chart1.ChartAreas.Clear();
             ChartArea chartArea1 = new ChartArea("C1");
             this.chart1.ChartAreas.Add(chartArea1);
             //定义存储和显示点的容器
+
             this.chart1.Series.Clear();
+
             Series series1 = new Series("S1");
             series1.ChartArea = "C1";
+
+            Series series2 = new Series("S2");
+            series2.ChartArea = "C1";
             this.chart1.Series.Add(series1);
+            this.chart1.Series.Add(series2);
+
+            //this.chart1.
             //设置图表显示样式
-            //this.chart1.ChartAreas[0].AxisY.Minimum = 0;
-            //this.chart1.ChartAreas[0].AxisY.Maximum = 100;
-            //this.chart1.ChartAreas[0].AxisX.Interval = 5;
+            this.chart1.ChartAreas[0].AxisY.Minimum = 0;
+            this.chart1.ChartAreas[0].AxisY.Maximum = max_axis_Y;
+            this.chart1.ChartAreas[0].AxisY.Interval = 100;
+            this.chart1.ChartAreas[0].AxisX.Minimum = -2300;// -200;
+            this.chart1.ChartAreas[0].AxisX.Maximum = 2300;// 200;
+            this.chart1.ChartAreas[0].AxisX.Interval = 100;// 100;
             this.chart1.ChartAreas[0].AxisX.MajorGrid.LineColor = System.Drawing.Color.Silver;
             this.chart1.ChartAreas[0].AxisY.MajorGrid.LineColor = System.Drawing.Color.Silver;
+            this.chart1.ChartAreas[0].AxisY.Title = "Y";
+            this.chart1.ChartAreas[0].AxisX.Title = "X";
+            //this.chart1.ChartAreas[0].AxisX.ArrowStyle = AxisArrowStyle.Triangle;
+            //this.chart1.ChartAreas[0].AxisY.ArrowStyle = AxisArrowStyle.Triangle;
             //设置标题
             this.chart1.Titles.Clear();
             this.chart1.Titles.Add("S01");
@@ -241,27 +516,18 @@ namespace Test
             //设置图表显示样式
             this.chart1.Series[0].Color = Color.Red;
             this.chart1.Series[0].ChartType = SeriesChartType.Line;
-            for (int i = 0; i < listx.Count; i++)
+            this.chart1.Series[1].Color = Color.Blue;
+            this.chart1.Series[1].ChartType = SeriesChartType.Line;
+            //List<double> dis = lineFit3(listy);
+            for (int i = left_range; i < right_range; i++)
             {
-                this.chart1.Series[0].Points.AddXY(listx[i], listy[i]);
+                //this.chart1.Series[0].Points.AddXY(listx[i], max_axis_Y - listy[i - left_range]);//listy[i - left_range] 
+                //this.chart1.Series[1].Points.AddXY(listx[i], max_axis_Y - templisty[i - left_range]);
+                this.chart1.Series[0].Points.AddXY(listx[i], listy[i - left_range]);//listy[i - left_range] 
+                this.chart1.Series[1].Points.AddXY(listx[i], templisty[i - left_range]);
             }
         }
 
-        private void button2_Click(object sender, EventArgs e)
-        {
-            isok = true;
-            for (int i = 0; i < validdatalist.Count; i++)
-            {
-                string temp = validdatalist[i];
-                string[] tempdata = temp.Split(' ');
-                for (int j = 0; j < tempdata.Length; j++)
-                {
-                    Log.WriteData("data" + i, tempdata[j]);
-                }
-            }
-            Environment.Exit(0);
-            this.Close();
-        }
 
         /// <summary>
         /// 线性拟合
@@ -303,13 +569,54 @@ namespace Test
             a = Dxy / den;
             b = (lambda - Dxx) / den;
             c = -a * x_mean - b * y_mean;
-            //Console.WriteLine(a + "  " + b + "  " + c);
-            List<double> dis = new List<double>();
+            Console.WriteLine(a + "  " + b + "  " + c);
+            dis = new List<double>();
+            errorIndex.Clear();
             for (int i = 0; i < size; i++)
             {
-                double d = Math.Abs(a * x[i] + b * y[i] + c) / Math.Sqrt(a * a + b * b);
-                dis.Add(d);
-                Log.WriteLog(LogType.PROCESS, d + "");
+                if (y[i] < (-a * x[i] - c) / b)
+                {
+                    double d = Math.Abs(a * x[i] + b * y[i] + c) / Math.Sqrt(a * a + b * b);
+                    
+                    if (d > 40)
+                    {
+                        dis.Add(d);
+                        errorIndex.Add(i);
+                    }
+                    //Log.WriteLog(LogType.PROCESS, d + "");
+                }
+            }
+            return dis;
+        }
+
+        private static List<double> lineFit2(List<double> y)
+        {
+            double b;
+            int size = y.Count;
+            if (size < 2)
+            {
+                b = 0;
+                return null;
+            }
+            double sum = 0;
+            for(int i = 0; i < size; i++)
+            {
+                sum += y[i];
+            }
+            b = sum / size;
+            dis = new List<double>();
+            errorIndex.Clear();
+            for (int i = 0; i < size; i++)
+            {
+                if (y[i] < b)
+                {
+                    double d = Math.Abs(y[i] - b);
+                    if (d > 40)
+                    {
+                        dis.Add(d);
+                        errorIndex.Add(i);
+                    }
+                }
             }
             return dis;
         }
@@ -319,7 +626,82 @@ namespace Test
         /// <param name="testdata"></param>
         /// <param name="deviation"></param>
         /// <returns></returns>
+        private static List<double> lineFit3(List<double> y)
+        {
+            double b;
+            int size = y.Count;
+            if (size < 2)
+            {
+                b = 0;
+                return null;
+            }
+            double sum = 0;
+            for (int i = left_range; i < right_range; i++)
+            {
+                sum += y[i];
+            }
+            b = sum / (right_range - left_range);
+            dis = new List<double>();
+            errorIndex.Clear();
+            for (int i = left_range; i < right_range; i++)
+            {
+                double d = Math.Abs(y[i] - b);
+                if (d > 20)
+                {
+                    double y_update = 0;
+                    dis.Add(y_update);
+                    errorIndex.Add(i);
+                }
+                else
+                {
+                    dis.Add(y[i]);
+                }
+
+            }
+            return dis;
+        }
+
+        private static List<double> dataFilter(List<List<double>> y, out bool isObstacle)
+        {
+            List<double>  y_udt = new List<double>();
+            isObstacle = false;
+            int hinder_point_count = 0;
+            for (int i = left_range; i < right_range; i++)  
+            {
+                int hinder_frame_count = 0;
+                
+                for(int j = 0; j < y.Count; j++)
+                {
+                    if(i!=left_range && i!= right_range-1)
+                    {
+                        y[j][i] = (y[j][i - 1] + y[j][i] + y[j][i + 1]) / 3;//每个点的y为其和相邻两个点和的平均值
+                    }
+                    double d = Math.Abs(y[j][i] - ground_truth_height);
+                    if (d > min_obstacle_height)//300
+                        hinder_frame_count = hinder_frame_count + 1;
+                }
 
+                if (hinder_frame_count == y.Count)
+                    hinder_point_count = hinder_point_count + 1;
+                y_udt.Add(y[y.Count - 1][i]);
+            }
+            if (hinder_point_count >= min_num_point_of_hinder)
+                isObstacle = true;
+            return y_udt;
+        }
+        private List<double> kalmanFilter(List<double> data_in)
+        {
+            for (int i = left_range; i < right_range; i++)
+            {
+                p[i - left_range] = p[i - left_range] + q[i - left_range];
+                kGain[i - left_range] = p[i - left_range] / (p[i - left_range] + r[i - left_range]);
+                data_in[i] = prevData[i - left_range] + (kGain[i - left_range] * (data_in[i] - prevData[i - left_range]));
+                p[i - left_range] = (1 - kGain[i - left_range]) * p[i - left_range];
+                prevData[i - left_range] = data_in[i];
+            }
+               
+            return data_in;
+        }
         private static bool IsHinder(List<double> testdata, double deviation)
         {
             bool isHinder = true;
@@ -374,9 +756,179 @@ namespace Test
             return flag;
         }
 
-        private void button3_Click(object sender, EventArgs e)
+
+        //********************** PLC ****************************
+        /// <summary>
+        /// 连接PLC
+        /// </summary>
+        /// <returns></returns>
+        private bool PLCConnect()
         {
-            isok = true;
+            closed = false;
+            writeThread = new Thread(() =>
+            {
+                //Stopwatch stopwatch = new Stopwatch();
+                while (!closed)
+                {
+                    //stopwatch.Restart();
+                    bool result = WriteAccordingly();
+                    if (!result)
+                        Console.WriteLine("写PLC操作异常");
+                    Thread.Sleep(1000/writeFreq);
+                    //stopwatch.Stop();
+                    //Console.WriteLine(stopwatch.ElapsedMilliseconds / 1000.0f);
+                }
+            });
+            writeThread.IsBackground = true;
+            writeThread.Priority = ThreadPriority.AboveNormal;
+            writeThread.Start();
+
+            if (plc == null)
+            {
+                plc = new Plc(cpu, ip, rack, slot);
+            }
+            if (plc != null)
+            {
+                ErrorCode err = plc.Open();
+                if (err != ErrorCode.NoError) { Console.WriteLine("connection error"); isConnected = false; }
+                else { Console.WriteLine("connected"); isConnected = true; }
+            }
+            else { isConnected = false; }
+            return isConnected;
+        }
+        /// <summary>
+        /// 关闭PLC连接
+        /// </summary>
+        private void PLCDisconnect()
+        {
+            //try
+            //{
+            //    if (writeThread != null)
+            //        writeThread.Abort();
+            //}
+            //catch (Exception ex) { Console.WriteLine("强制终止写PLC线程，" + ex.Message); }
+            closed = true;
+            if (plc != null)
+            {
+                plc.Close();
+            }
+        }
+        /// <summary>
+        /// 从阻塞队列获取需写入PLC数据并写入PLC，时间间隔在线程中控制
+        /// </summary>
+        /// <returns></returns>
+        private bool WriteAccordingly()
+        {
+            MsgNode msg = null;
+            try
+            {
+                msg = writingBlockingCollection.Take();
+                //Console.WriteLine("取出后，" + writingBlockingCollection.Count);
+            }
+            catch (Exception ex) { Console.WriteLine("获取PLC写对象异常，" + ex.Message); }
+            if (msg == null || !msg.valid)
+            {
+                Console.WriteLine("msg is null or invalid");
+                return false;
+            }
+            //更新设备状态
+            isConnected = plc.IsConnected;
+            if (isConnected)
+            {
+                lock (readWriteLock)
+                {
+                    plc.WriteBytes(DataType.DataBlock, msg.db, msg.start, msg.value.ToArray());
+                }
+            }
+            else { Console.WriteLine("掉线"); return false; }
+            return true;
+        }
+
+        /// <summary>
+        /// 读取PLC数据块
+        /// </summary>
+        /// <param name="whichToRead"></param>
+        /// <param name="index"></param>
+        /// <returns></returns>
+        private void ReadFromPLC(ref MsgNode msg)
+        {
+            //更新设备状态
+            isConnected = plc.IsConnected;
+            List<object> result = new List<object>();
+            if (!isConnected)
+            {
+                PLCConnect();
+            }
+            if (isConnected)
+            {
+                lock (readWriteLock)
+                {
+                    msg.value = plc.ReadBytes(DataType.DataBlock, msg.db, msg.start, msg.length).ToList();
+                    if (msg.value.Count != msg.length)
+                    {
+                        msg.valid = false;
+                    }
+                    msg.value.Reverse();
+                }
+            }
+        }
+        /// <summary>
+        /// 写入PLC，只支持1,2或4字节
+        /// </summary>
+        /// <param name="abstractPLCMsg"></param>
+        /// <param name="whoami"></param>
+        /// <returns></returns>
+        private bool WriteToPLC(MsgNode msg)
+        {
+            //更新设备状态
+            isConnected = plc.IsConnected;
+            if (msg != null && isConnected)
+            {
+                msg.value.Reverse();
+                writingBlockingCollection.Add(msg);
+                //Console.WriteLine("加入后，" + writingBlockingCollection.Count);
+                return true;
+            }
+            else
+                return false;
+        }
+
+        public class MsgNode : ICloneable
+        {
+            public int db;
+            public int start;
+            public int length;
+            public List<byte> value;
+            public bool valid;
+            public MsgNode(int db, int start, int length, List<byte> value)
+            {
+                this.db = db;
+                this.start = start;
+                this.length = length;
+                if(value!=null && value.Count != length)
+                {
+                    valid = false;
+                }
+                else
+                {
+                    valid = true;
+                }
+                this.value = value;
+            }
+            public MsgNode():this(0,0,0, new List<byte>())
+            {
+            }
+
+            public object Clone()
+            {
+                MsgNode obj = new MsgNode();
+                obj.db = db;
+                obj.start = start;
+                obj.length = length;
+                obj.value = new List<byte>(value);
+                obj.valid = valid;
+                return obj;
+            }
         }
     }
 }

Test/SickTimEnc.cs

@@ -0,0 +1,362 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace Test
+{
+    class SickTimEnc
+    {
+        //状态指示变量
+        /// <summary>
+        /// 指示初始化是否成功
+        /// </summary>
+        public bool initialized { get; set; }
+        /// <summary>
+        /// 指示是否工作中
+        /// </summary>
+        public bool working { get; set; }
+        /// <summary>
+        /// 指示是否检测到障碍物
+        /// </summary>
+        public bool obstacleDetected { get; set; }
+        /// <summary>
+        /// 发送指令返回值
+        /// </summary>
+        private int flag = 0;
+        //通信参数
+        private static readonly object Lock = new object();
+        private IPAddress ipa;
+        private int port;
+        private TcpClient tcpClient;
+        private NetworkStream stream;
+        private int bufferSize = 16384;
+        private const string startCmd = "02 73 52 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 03";
+        private const string endCmd = "02 73 45 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 30 03";
+        //防撞参数
+        public int maxDataCount { get; set; }//最大保留历史数据个数
+        public int left_range { get; set; } = 385;//325;//231;//271;//386;//366;
+        public int right_range { get; set; } = 425;//485;//581;//541;//426;//446;
+        private int ground_truth_height = 1700;//2200;//2300;//标准高度
+        private int min_num_point_of_hinder = 30;//20;//判定为障碍物的最少点的数目
+        private int min_obstacle_height = 500;//最小障碍物高度
+        private int max_axis_Y = 2000;//2250;//画图时Y轴最大量程
+        private List<string> originDatalist;//原始数据数组
+        private List<int> validNum;//811个整型值
+        private List<List<double>> validNumBuffer;//保存历史点云数据
+        private List<List<double>> validNumBufferKalman;//保存经过kalman滤波的历史点云数据
+        private List<double> listy;
+        private List<double> listx;
+        public List<double> yFiltered { get; set; }
+        public List<double> yFilteredKalman { get; set; }
+        //卡尔曼滤波参数
+        private List<double> prevData, p, q, r, kGain;
+
+        public SickTimEnc(string ip, int port)
+        {
+            initialized = false;
+            maxDataCount = 3;
+            originDatalist = new List<string>();
+            validNum = new List<int>();
+            validNumBuffer = new List<List<double>>();
+            validNumBufferKalman = new List<List<double>>();
+            prevData = new List<double>();
+            p = new List<double>();
+            q = new List<double>();
+            r = new List<double>();
+            kGain = new List<double>();
+            for (int i = 0; i < right_range - left_range; i++)
+            {
+                prevData.Add(0);
+                p.Add(450);
+                q.Add(0.0001);
+                r.Add(0.005);
+                kGain.Add(0);
+            }
+            listy = new List<double>();
+            listx = new List<double>();
+            tcpClient = new TcpClient();
+            if(!IPAddress.TryParse(ip, out ipa))
+            {
+                ipa = null;
+                return;
+            }
+            try
+            {
+                tcpClient.Connect(ipa, port);
+                stream = tcpClient.GetStream();
+            }
+            catch { return; }
+            initialized = true;
+        }
+        /// <summary>
+        /// 启动雷达处理数据
+        /// </summary>
+        public void work()
+        {
+            Task.Factory.StartNew(()=>
+            {
+                while (initialized)
+                {
+                    try
+                    {
+                        if (working)
+                        {
+                        //开辟线程存储雷达原始数据字符串
+                            lock (Lock)
+                            {
+                                originDatalist.Clear();
+                                flag = sendCmd(startCmd);
+                                if (flag == 1)
+                                {
+                                    byte[] buffer = new byte[bufferSize];
+                                    stream.Read(buffer, 0, buffer.Length);
+                                    string str = HexStringToASCII(buffer);
+                                    originDatalist.Add(str);
+                                }
+                                if (originDatalist.Count != 0)
+                                {
+                                    for (int i = 0; i < originDatalist.Count; i++)
+                                    {
+                                        string ss = trimDataStr(originDatalist[i], "sRA LMDscandata 1 ", " not defined 0 0 0");
+                                        string[] data = ss.Split(' ');
+                                        string number = data[22];
+                                        int length = Convert.ToInt32(number, 16);
+                                        int temp = 0;
+                                        //int[] validNum = new int[length];
+                                        //string aws = "";
+                                        //获取所有距离数值
+                                        validNum.Clear();
+                                        listy.Clear();
+                                        listx.Clear();
+                                        for (int j = 0; j < length; j++)
+                                        {
+                                            try
+                                            {
+                                                temp = Convert.ToInt32(data[23 + j], 16);
+                                            }
+                                            catch { temp = ground_truth_height; Console.WriteLine("数据转换异常"); }
+                                            validNum.Add(temp);
+                                        }
+                                        for (int j = 0; j < length; j++)
+                                        {
+                                            double x = validNum[j] * Math.Cos((-45 + 0.333 * j) * Math.PI / 180);
+                                            double y = validNum[j] * Math.Sin((-45 + 0.333 * j) * Math.PI / 180);
+                                            listy.Add(y);
+                                            listx.Add(x);
+                                        }
+                                        validNumBuffer.Add(listy);
+                                        validNumBufferKalman.Add(kalmanFilter(listy));
+                                        while (validNumBufferKalman.Count > maxDataCount)
+                                        {
+                                            validNumBufferKalman.RemoveAt(0);
+                                            validNumBuffer.RemoveAt(0);
+                                        }
+                                        if(validNumBufferKalman.Count == maxDataCount)
+                                        {
+                                            bool obstacle = false;
+                                            yFiltered = dataFilter(validNumBuffer, out obstacle);
+                                            yFilteredKalman = dataFilter(validNumBufferKalman, out obstacle);
+                                            obstacleDetected = obstacle;
+                                        }
+                                    }
+                                }
+                            }
+                            Thread.Sleep(1);
+                        }
+                        else
+                        {
+                            //空闲状态
+                            Thread.Sleep(1000);
+                        }
+                    }
+                    catch (Exception e) { Thread.Sleep(500); Console.WriteLine(e.Message); Console.WriteLine(e.StackTrace); }
+                    
+                }
+            });
+        }
+        /// <summary>
+        /// 暂停工作
+        /// </summary>
+        /// <param name="ctrl"></param>
+        public void Pause(bool ctrl)
+        {
+            working = !ctrl;
+        }
+        /// <summary>
+        /// 停止工作
+        /// </summary>
+        public void Stop()
+        {
+            working = false;
+            flag = sendCmd(endCmd);
+            if (stream != null)
+            {
+                stream.Close();
+            }
+            if (tcpClient != null)
+            {
+                tcpClient.Close();
+            }
+        }
+
+        /// <summary>
+        /// 向雷达发送指令
+        /// </summary>
+        /// <param name="cmd"></param>
+        /// <returns></returns>
+        private int sendCmd(string cmd)
+        {
+            try
+            {
+                if (cmd.Equals(startCmd))
+                {
+                    byte[] byteStartCmd = HexStringToBinary(startCmd);
+                    string result = HexStringToASCII(byteStartCmd);
+                    byte[] b = Encoding.UTF8.GetBytes(result);
+                    stream.Write(b, 0, b.Length);
+                    return 1;
+                }
+                if (cmd.Equals(endCmd))
+                {
+                    byte[] byteStartCmd = HexStringToBinary(endCmd);
+                    string result = HexStringToASCII(byteStartCmd);
+                    byte[] b = Encoding.UTF8.GetBytes(result);
+                    stream.Write(b, 0, b.Length);
+                    return 2;
+                }
+            }
+            catch { return 0; }
+            return 0;
+        }
+        /// <summary>
+        /// 将一条十六进制字符串转换为ASCII
+        /// </summary>
+        /// <param name="hexstring">一条十六进制字符串</param>
+        /// <returns>返回一条ASCII码</returns>
+        private static string HexStringToASCII(byte[] bt)
+        {
+            //byte[] bt = HexStringToBinary(hexstring);
+            string lin = "";
+            for (int i = 0; i < bt.Length; i++)
+            {
+                lin = lin + bt[i] + " ";
+            }
+
+
+            string[] ss = lin.Trim().Split(new char[] { ' ' });
+            char[] c = new char[ss.Length];
+            int a;
+            for (int i = 0; i < c.Length; i++)
+            {
+                a = Convert.ToInt32(ss[i]);
+                c[i] = Convert.ToChar(a);
+            }
+
+            string b = new string(c);
+            return b;
+        }
+        /// <summary>
+        /// 16进制字符串转换为二进制数组
+        /// </summary>
+        /// <param name="hexstring">用空格切割字符串</param>
+        /// <returns>返回一个二进制字符串</returns>
+        private static byte[] HexStringToBinary(string hexstring)
+        {
+
+            string[] tmpary = hexstring.Trim().Split(' ');
+            byte[] buff = new byte[tmpary.Length];
+            for (int i = 0; i < buff.Length; i++)
+            {
+                buff[i] = Convert.ToByte(tmpary[i], 16);
+            }
+            return buff;
+        }
+        /// <summary>
+        /// 截取数据段
+        /// </summary>
+        /// <param name="sourse"></param>
+        /// <param name="startstr"></param>
+        /// <param name="endstr"></param>
+        /// <returns></returns>
+        private static string trimDataStr(string sourse, string startstr, string endstr)
+        {
+            string result = string.Empty;
+            int startindex, endindex;
+            try
+            {
+                startindex = sourse.IndexOf(startstr);
+                if (startindex == -1)
+                {
+                    return result;
+                }
+                string tmpstr = sourse.Substring(startindex + startstr.Length);
+                endindex = tmpstr.IndexOf(endstr);
+                if (endindex == -1)
+                {
+                    return result;
+                }
+                result = tmpstr.Remove(endindex);
+            }
+            catch (Exception) { }
+            return result;
+        }
+        /// <summary>
+        /// 平滑数据，识别障碍物
+        /// </summary>
+        /// <param name="y"></param>
+        /// <param name="isObstacle"></param>
+        /// <returns></returns>
+        private List<double> dataFilter(List<List<double>> y, out bool isObstacle)
+        {
+            List<double> y_udt = new List<double>();
+            isObstacle = false;
+            int hinder_point_count = 0;
+            for (int i = left_range; i < right_range; i++)
+            {
+                int hinder_frame_count = 0;
+
+                for (int j = 0; j < y.Count; j++)
+                {
+                    if (i != left_range && i != right_range - 1)
+                    {
+                        y[j][i] = (y[j][i - 1] + y[j][i] + y[j][i + 1]) / 3;//每个点的y为其和相邻两个点和的平均值
+                    }
+                    double d = Math.Abs(y[j][i] - ground_truth_height);
+                    if (d > min_obstacle_height)//300
+                        hinder_frame_count = hinder_frame_count + 1;
+                }
+
+                if (hinder_frame_count == y.Count)
+                    hinder_point_count = hinder_point_count + 1;
+                y_udt.Add(y[y.Count - 1][i]);
+            }
+            if (hinder_point_count >= min_num_point_of_hinder)
+                isObstacle = true;
+            return y_udt;
+        }
+        /// <summary>
+        /// 卡尔曼滤波
+        /// </summary>
+        /// <param name="data_in"></param>
+        /// <returns></returns>
+        private List<double> kalmanFilter(List<double> data_in)
+        {
+            for (int i = left_range; i < right_range; i++)
+            {
+                p[i - left_range] = p[i - left_range] + q[i - left_range];
+                kGain[i - left_range] = p[i - left_range] / (p[i - left_range] + r[i - left_range]);
+                data_in[i] = prevData[i - left_range] + (kGain[i - left_range] * (data_in[i] - prevData[i - left_range]));
+                p[i - left_range] = (1 - kGain[i - left_range]) * p[i - left_range];
+                prevData[i - left_range] = data_in[i];
+            }
+
+            return data_in;
+        }
+
+    }
+}

Test/Test.csproj

@@ -32,6 +32,9 @@
     <WarningLevel>4</WarningLevel>
   </PropertyGroup>
   <ItemGroup>
+    <Reference Include="snap7Enc">
+      <HintPath>sdk\snap7Enc.dll</HintPath>
+    </Reference>
     <Reference Include="System" />
     <Reference Include="System.Core" />
     <Reference Include="System.Windows.Forms.DataVisualization" />
@@ -57,6 +60,7 @@
     <Compile Include="LogManager.cs" />
     <Compile Include="Program.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="SickTimEnc.cs" />
     <EmbeddedResource Include="Form1.resx">
       <DependentUpon>Form1.cs</DependentUpon>
     </EmbeddedResource>

encTest/App.config

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<configuration>
+    <startup> 
+        <supportedRuntime version="v4.0" sku=".NETFramework,Version=v4.5" />
+    </startup>
+</configuration>

encTest/Form1.Designer.cs

@@ -0,0 +1,208 @@
+namespace Test
+{
+    partial class obstacleDetectionForm
+    {
+        /// <summary>
+        /// 必需的设计器变量。
+        /// </summary>
+        private System.ComponentModel.IContainer components = null;
+
+        /// <summary>
+        /// 清理所有正在使用的资源。
+        /// </summary>
+        /// <param name="disposing">如果应释放托管资源，为 true；否则为 false。</param>
+        protected override void Dispose(bool disposing)
+        {
+            if (disposing && (components != null))
+            {
+                components.Dispose();
+            }
+            base.Dispose(disposing);
+        }
+
+        #region Windows 窗体设计器生成的代码
+
+        /// <summary>
+        /// 设计器支持所需的方法 - 不要修改
+        /// 使用代码编辑器修改此方法的内容。
+        /// </summary>
+        private void InitializeComponent()
+        {
+            System.Windows.Forms.DataVisualization.Charting.ChartArea chartArea1 = new System.Windows.Forms.DataVisualization.Charting.ChartArea();
+            System.Windows.Forms.DataVisualization.Charting.Legend legend1 = new System.Windows.Forms.DataVisualization.Charting.Legend();
+            System.Windows.Forms.DataVisualization.Charting.Series series1 = new System.Windows.Forms.DataVisualization.Charting.Series();
+            this.button1 = new System.Windows.Forms.Button();
+            this.laserDataChart = new System.Windows.Forms.DataVisualization.Charting.Chart();
+            this.button2 = new System.Windows.Forms.Button();
+            this.label1 = new System.Windows.Forms.Label();
+            this.tb_angleLeft = new System.Windows.Forms.TextBox();
+            this.tb_angleRight = new System.Windows.Forms.TextBox();
+            this.label2 = new System.Windows.Forms.Label();
+            this.label3 = new System.Windows.Forms.Label();
+            this.lb_obstacle = new System.Windows.Forms.Label();
+            this.label5 = new System.Windows.Forms.Label();
+            this.textBox3 = new System.Windows.Forms.TextBox();
+            this.button3 = new System.Windows.Forms.Button();
+            ((System.ComponentModel.ISupportInitialize)(this.laserDataChart)).BeginInit();
+            this.SuspendLayout();
+            // 
+            // button1
+            // 
+            this.button1.Font = new System.Drawing.Font("宋体", 10.8F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(134)));
+            this.button1.Location = new System.Drawing.Point(587, 743);
+            this.button1.Name = "button1";
+            this.button1.Size = new System.Drawing.Size(91, 58);
+            this.button1.TabIndex = 1;
+            this.button1.Text = "开始";
+            this.button1.UseVisualStyleBackColor = true;
+            this.button1.Click += new System.EventHandler(this.button1_Click);
+            // 
+            // laserDataChart
+            // 
+            chartArea1.Name = "ChartArea1";
+            this.laserDataChart.ChartAreas.Add(chartArea1);
+            legend1.Name = "Legend1";
+            this.laserDataChart.Legends.Add(legend1);
+            this.laserDataChart.Location = new System.Drawing.Point(26, 69);
+            this.laserDataChart.Name = "laserDataChart";
+            series1.ChartArea = "ChartArea1";
+            series1.Legend = "Legend1";
+            series1.Name = "Series1";
+            this.laserDataChart.Series.Add(series1);
+            this.laserDataChart.Size = new System.Drawing.Size(1764, 623);
+            this.laserDataChart.TabIndex = 2;
+            this.laserDataChart.Text = "雷达数据可视化";
+            // 
+            // button2
+            // 
+            this.button2.Font = new System.Drawing.Font("宋体", 10.8F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(134)));
+            this.button2.Location = new System.Drawing.Point(1032, 743);
+            this.button2.Name = "button2";
+            this.button2.Size = new System.Drawing.Size(103, 58);
+            this.button2.TabIndex = 3;
+            this.button2.Text = "结束";
+            this.button2.UseVisualStyleBackColor = true;
+            this.button2.Click += new System.EventHandler(this.button2_Click);
+            // 
+            // label1
+            // 
+            this.label1.AutoSize = true;
+            this.label1.Location = new System.Drawing.Point(109, 31);
+            this.label1.Name = "label1";
+            this.label1.Size = new System.Drawing.Size(52, 15);
+            this.label1.TabIndex = 4;
+            this.label1.Text = "角度：";
+            // 
+            // tb_angleLeft
+            // 
+            this.tb_angleLeft.Location = new System.Drawing.Point(194, 21);
+            this.tb_angleLeft.Name = "tb_angleLeft";
+            this.tb_angleLeft.Size = new System.Drawing.Size(100, 25);
+            this.tb_angleLeft.TabIndex = 5;
+            this.tb_angleLeft.Text = "-45";
+            // 
+            // tb_angleRight
+            // 
+            this.tb_angleRight.Location = new System.Drawing.Point(376, 21);
+            this.tb_angleRight.Name = "tb_angleRight";
+            this.tb_angleRight.Size = new System.Drawing.Size(100, 25);
+            this.tb_angleRight.TabIndex = 6;
+            this.tb_angleRight.Text = "225";
+            // 
+            // label2
+            // 
+            this.label2.AutoSize = true;
+            this.label2.Location = new System.Drawing.Point(315, 31);
+            this.label2.Name = "label2";
+            this.label2.Size = new System.Drawing.Size(22, 15);
+            this.label2.TabIndex = 7;
+            this.label2.Text = "到";
+            // 
+            // label3
+            // 
+            this.label3.AutoSize = true;
+            this.label3.Location = new System.Drawing.Point(1180, 30);
+            this.label3.Name = "label3";
+            this.label3.Size = new System.Drawing.Size(67, 15);
+            this.label3.TabIndex = 8;
+            this.label3.Text = "障碍物：";
+            // 
+            // lb_obstacle
+            // 
+            this.lb_obstacle.AutoSize = true;
+            this.lb_obstacle.Location = new System.Drawing.Point(1238, 31);
+            this.lb_obstacle.Name = "lb_obstacle";
+            this.lb_obstacle.Size = new System.Drawing.Size(23, 15);
+            this.lb_obstacle.TabIndex = 9;
+            this.lb_obstacle.Text = "-1";
+            // 
+            // label5
+            // 
+            this.label5.AutoSize = true;
+            this.label5.Font = new System.Drawing.Font("宋体", 10.8F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(134)));
+            this.label5.Location = new System.Drawing.Point(679, 31);
+            this.label5.Name = "label5";
+            this.label5.Size = new System.Drawing.Size(66, 19);
+            this.label5.TabIndex = 10;
+            this.label5.Text = "误差：";
+            // 
+            // textBox3
+            // 
+            this.textBox3.Location = new System.Drawing.Point(751, 25);
+            this.textBox3.Name = "textBox3";
+            this.textBox3.Size = new System.Drawing.Size(53, 25);
+            this.textBox3.TabIndex = 11;
+            this.textBox3.Text = "40";
+            // 
+            // button3
+            // 
+            this.button3.Font = new System.Drawing.Font("宋体", 10.8F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(134)));
+            this.button3.Location = new System.Drawing.Point(797, 743);
+            this.button3.Name = "button3";
+            this.button3.Size = new System.Drawing.Size(98, 58);
+            this.button3.TabIndex = 12;
+            this.button3.Text = "暂停";
+            this.button3.UseVisualStyleBackColor = true;
+            this.button3.Click += new System.EventHandler(this.button3_Click);
+            // 
+            // obstacleDetectionForm
+            // 
+            this.AutoScaleDimensions = new System.Drawing.SizeF(8F, 15F);
+            this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
+            this.ClientSize = new System.Drawing.Size(1791, 837);
+            this.Controls.Add(this.button3);
+            this.Controls.Add(this.textBox3);
+            this.Controls.Add(this.label5);
+            this.Controls.Add(this.lb_obstacle);
+            this.Controls.Add(this.label3);
+            this.Controls.Add(this.label2);
+            this.Controls.Add(this.tb_angleRight);
+            this.Controls.Add(this.tb_angleLeft);
+            this.Controls.Add(this.label1);
+            this.Controls.Add(this.button2);
+            this.Controls.Add(this.laserDataChart);
+            this.Controls.Add(this.button1);
+            this.Name = "obstacleDetectionForm";
+            this.Text = "防撞雷达程序";
+            ((System.ComponentModel.ISupportInitialize)(this.laserDataChart)).EndInit();
+            this.ResumeLayout(false);
+            this.PerformLayout();
+
+        }
+
+        #endregion
+        private System.Windows.Forms.Button button1;
+        private System.Windows.Forms.DataVisualization.Charting.Chart laserDataChart;
+        private System.Windows.Forms.Button button2;
+        private System.Windows.Forms.Label label1;
+        private System.Windows.Forms.TextBox tb_angleLeft;
+        private System.Windows.Forms.TextBox tb_angleRight;
+        private System.Windows.Forms.Label label2;
+        private System.Windows.Forms.Label label3;
+        private System.Windows.Forms.Label lb_obstacle;
+        private System.Windows.Forms.Label label5;
+        private System.Windows.Forms.TextBox textBox3;
+        private System.Windows.Forms.Button button3;
+    }
+}
+

encTest/Form1.cs

@@ -0,0 +1,830 @@
+using snap7Enc;
+using System;
+using System.Collections.Concurrent;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Net;
+using System.Net.Sockets;
+using System.Runtime.InteropServices;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+using System.Windows.Forms;
+using System.Windows.Forms.DataVisualization.Charting;
+
+namespace Test
+{
+
+    public partial class obstacleDetectionForm : Form
+    {
+        private SickTimEnc laserLeft;
+        private SickTimEnc laserRight;
+        private int max_axis_Y = 4000;//2250;//画图时Y轴最大量程
+        private List<DispMsg> dataDispList;
+        private List<string> colors;//雷达数据颜色
+        //************** plc相关参数 *************
+        /// <summary>
+        /// PLC 连接状态flag
+        /// </summary>
+        public bool isConnected = false;
+        /// <summary>
+        /// plc中cpu类型
+        /// </summary>
+        private const CpuType cpu = CpuType.S71200;
+        /// <summary>
+        /// PLC的IP地址
+        /// </summary>
+        private const string ip = "192.168.0.1";
+        /// <summary>
+        /// PLC的端口号
+        /// </summary>
+        private const Int16 rack = 0;
+        /// <summary>
+        /// 工作站号
+        /// </summary>
+        private const Int16 slot = 1;
+        /// <summary>
+        /// PLC S7连接对象
+        /// </summary>
+        protected static Plc plc = null;
+        /// <summary>
+        /// 用于将写PLC指令排队的阻塞队列
+        /// </summary>
+        private BlockingCollection<MsgNode> writingBlockingCollection = new BlockingCollection<MsgNode>();
+        /// <summary>
+        /// 写线程，控制PLC写入频率
+        /// </summary>
+        private Thread writeThread;
+        /// <summary>
+        /// 系统关闭
+        /// </summary>
+        private bool closed;
+        /// <summary>
+        /// 读写锁
+        /// </summary>
+        private object readWriteLock = new object();
+        //写入频率
+        private const int writeFreq = 25;
+        private const short DB = 30;
+        private const short ZPosition = 8;
+        private const short ZThreshold = 200;
+        private const short ZStart = 10;
+        private const short ZStop = 12;
+        private const short ZPause = 14;
+        private short ZPositionValue = 0;
+        private short ZStartValue = 0;
+        private short ZStopValue = 0;
+        private short ZPauseValue = 0;
+        private bool startGoingDown = false;
+        private bool laserWorking = false;
+
+        //流程控制
+        private void Start()
+        {
+            colors = new List<string>();
+            colors.Add(Color.Red.Name);
+            colors.Add(Color.Yellow.Name);
+            colors.Add(Color.Cyan.Name);
+            colors.Add(Color.Blue.Name);
+            dataDispList = new List<DispMsg>();
+            Task.Factory.StartNew(() =>
+            {
+                laserLeft = new SickTimEnc("192.168.0.77", 2111);
+                laserRight = new SickTimEnc("192.168.0.88", 2111);
+                while (!IsHandleCreated)
+                {
+                    Thread.Sleep(500);
+                    Console.WriteLine("等待初始化完成");
+                }
+                this.Invoke(new Action(() =>
+                {
+                    InitChart();
+                    if (laserLeft != null)
+                    {
+                        Pause();
+                        laserLeft.SetLeftRange(75);
+                        laserLeft.SetRightRange(105);
+                        laserLeft.ground_truth_height = 2600;
+                        laserLeft.min_num_point_of_hinder = 20;
+                        laserLeft.min_obstacle_height = 100;
+                        tb_angleLeft.Text = "75";
+                        tb_angleRight.Text = "105";
+                        Pause();
+                        List<List<double>> temp = new List<List<double>>();
+                        temp.Add(laserLeft.yFiltered);
+                        temp.Add(laserLeft.yFilteredKalman);
+                        chartAddData(laserLeft.listx, temp, 1);
+                    }
+                    if (laserRight != null)
+                    {
+                        Pause();
+                        laserRight.SetLeftRange(75);
+                        laserRight.SetRightRange(105);
+                        laserRight.ground_truth_height = 2600;
+                        laserRight.min_num_point_of_hinder = 20;
+                        laserRight.min_obstacle_height = 100;
+                        tb_angleLeft.Text = "75";
+                        tb_angleRight.Text = "105";
+                        Pause();
+                        List<List<double>> temp = new List<List<double>>();
+                        temp.Add(laserRight.yFiltered);
+                        temp.Add(laserRight.yFilteredKalman);
+                        chartAddData(laserRight.listx, temp, 2);
+                    }
+                }));
+                //Task.Factory.StartNew(()=>
+                //{
+                //    int height = 2600;
+                //    Thread.Sleep(5000);
+                //    while (height >= 200)
+                //    {
+                //        height--;
+                //        if (laserLeft != null)
+                //            laserLeft.ground_truth_height = height;
+                //        if (laserRight != null)
+                //            laserRight.ground_truth_height = height;
+                //        Console.WriteLine(height);
+                //        Thread.Sleep(3);
+                //    }
+                //});
+                while (!closed)
+                {
+                    //System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
+                    try
+                    {
+                        this.Invoke(new Action(() =>
+                        {
+                            //for (int i = 0; i < dataDispList.Count; i++)
+                            //{
+                            //    updateChartData(dataDispList[i].x, dataDispList[i].y, i);
+                            //}
+                            if (dataDispList.Count >= 2)
+                            {
+                                if (laserLeft != null)
+                                {
+                                    updateChartData(laserLeft.listx, laserLeft.yFiltered, 0);
+                                    updateChartData(laserLeft.listx, laserLeft.yFilteredKalman, 1);
+                                    lb_obstacle.Text = "  left:" + laserLeft.obstacleDetected+"  ";
+                                }
+                                if (dataDispList.Count >= 4 && laserRight != null)
+                                {
+                                    updateChartData(laserRight.listx, laserRight.yFiltered, 2);
+                                    updateChartData(laserRight.listx, laserRight.yFilteredKalman, 3);
+                                    lb_obstacle.Text += "right:" + laserRight.obstacleDetected + "  ";
+                                }
+                                
+                            }
+                        }));
+                    }
+                    catch { Stop(); }
+                    //sw.Stop();
+                    //Console.WriteLine(sw.ElapsedMilliseconds + "ms");
+                    Thread.Sleep(30);
+                }
+            });
+        }
+        /// <summary>
+        /// 暂停雷达数据处理线程
+        /// </summary>
+        private void Pause()
+        {
+            if (laserLeft != null)
+            {
+                laserLeft.Pause();
+            }
+            if (laserRight != null)
+            {
+                laserRight.Pause();
+            }
+        }
+        /// <summary>
+        /// 停止获取雷达数据，断开连接
+        /// </summary>
+        private void Stop()
+        {
+            if (laserLeft != null)
+            {
+                laserLeft.Stop();
+            }
+            if (laserRight != null)
+            {
+                laserRight.Stop();
+            }
+        }
+
+        public obstacleDetectionForm()
+        {
+            InitializeComponent();
+            Start();
+            //************* PLC 相关逻辑 *************
+            //bool result = PLCConnect();
+            //if (!result)
+            //    Environment.Exit(0);
+            //else
+            //{
+            //    MsgNode msg = new MsgNode();
+            //    msg.db = DB;
+            //    msg.length = 2;
+            //    Task.Factory.StartNew(() => {
+            //        while (!closed)
+            //        {
+            //            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(true);
+            //                Console.WriteLine("暂停");
+            //            }
+            //            else
+            //            {
+            //                Pause(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;
+            //            }
+
+            //            Thread.Sleep(50);
+            //        }
+            //    });
+            //}
+        }
+        //界面按钮
+        private void button1_Click(object sender, EventArgs e)
+        {
+            Start();
+        }
+        private void button3_Click(object sender, EventArgs e)
+        {
+            Pause();
+        }
+        private void button2_Click(object sender, EventArgs e)
+        {
+            Stop();
+            //isok = true;
+            //for (int i = 0; i < validdatalist.Count; i++)
+            //{
+            //    string temp = validdatalist[i];
+            //    string[] tempdata = temp.Split(' ');
+            //    for (int j = 0; j < tempdata.Length; j++)
+            //    {
+            //        Log.WriteData("data" + i, tempdata[j]);
+            //    }
+            //}
+            //datalist.Clear();
+            //flag = sendCmd(endCmd);
+            //Environment.Exit(0);
+            //this.Close();
+        }
+
+        /// <summary>
+        /// 初始化图表
+        /// </summary>
+        private void InitChart()
+        {
+            //定义图表区域
+            this.laserDataChart.ChartAreas.Clear();
+            ChartArea chartArea1 = new ChartArea("C1");
+            this.laserDataChart.ChartAreas.Add(chartArea1);
+            //定义存储和显示点的容器
+
+            this.laserDataChart.Series.Clear();
+
+            //Series series1 = new Series("S1");
+            //series1.ChartArea = "C1";
+
+            //Series series2 = new Series("S2");
+            //series2.ChartArea = "C1";
+            //this.laserDataChart.Series.Add(series1);
+            //this.laserDataChart.Series.Add(series2);
+
+            //this.chart1.
+            //设置图表显示样式
+            this.laserDataChart.ChartAreas[0].AxisY.IsReversed = true;
+            this.laserDataChart.ChartAreas[0].AxisY.Minimum = 0;
+            this.laserDataChart.ChartAreas[0].AxisY.Maximum = max_axis_Y;
+            this.laserDataChart.ChartAreas[0].AxisY.Interval = 100;
+            this.laserDataChart.ChartAreas[0].AxisX.Minimum = -2300;// -200;
+            this.laserDataChart.ChartAreas[0].AxisX.Maximum = 2300;// 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;
+            this.laserDataChart.ChartAreas[0].AxisY.Title = "Y";
+            this.laserDataChart.ChartAreas[0].AxisX.Title = "X";
+            //this.chart1.ChartAreas[0].AxisX.ArrowStyle = AxisArrowStyle.Triangle;
+            //this.chart1.ChartAreas[0].AxisY.ArrowStyle = AxisArrowStyle.Triangle;
+
+            //设置标题
+            this.laserDataChart.Titles.Clear();
+            this.laserDataChart.Titles.Add("S01");
+            this.laserDataChart.Titles[0].Text = "雷达显示";
+            this.laserDataChart.Titles[0].ForeColor = Color.RoyalBlue;
+            this.laserDataChart.Titles[0].Font = new System.Drawing.Font("Microsoft Sans Serif", 12F);
+
+            ////设置图表显示样式
+            //this.laserDataChart.Series[0].Color = Color.Red;
+            //this.laserDataChart.Series[0].ChartType = SeriesChartType.Line;
+            //this.laserDataChart.Series[1].Color = Color.Blue;
+            //this.laserDataChart.Series[1].ChartType = SeriesChartType.Line;
+            ////List<double> dis = lineFit3(listy);
+            //for (int i = left_range; i < right_range; i++)
+            //{
+            //    //this.chart1.Series[0].Points.AddXY(listx[i], max_axis_Y - listy[i - left_range]);//listy[i - left_range] 
+            //    //this.chart1.Series[1].Points.AddXY(listx[i], max_axis_Y - templisty[i - left_range]);
+            //    this.laserDataChart.Series[0].Points.AddXY(listx[i], listy[i - left_range]);//listy[i - left_range] 
+            //    this.laserDataChart.Series[1].Points.AddXY(listx[i], templisty[i - left_range]);
+            //}
+        }
+        /// <summary>
+        /// 向图表中增加数据
+        /// </summary>
+        /// <param name="listx">x轴数据</param>
+        /// <param name="listys">y轴数据包</param>
+        /// <param name="laserIndex">雷达编号</param>
+        public void chartAddData(List<double> listx, List<List<double>> listys, int laserIndex)
+        {
+            if (laserIndex != 1 && laserIndex != 2)
+                return;
+            int index = laserDataChart.Series.Count;
+            for (int i = 0; i < listys.Count; i++)
+            {
+                Series series = new Series("S" + index+i);
+                series.ChartArea = "C1";
+                laserDataChart.Series.Add(series);
+                laserDataChart.Series[index + i].Color = Color.FromName(colors[index + i]);
+                laserDataChart.Series[index + i].ChartType = SeriesChartType.Line;
+                int left = 0;
+                int right = 0;
+                if (laserIndex == 1)
+                {
+                    left = laserLeft.GetLeftRange("index");
+                    right = laserLeft.GetRightRange("index");
+                }
+                else
+                {
+                    left = laserRight.GetLeftRange("index");
+                    right = laserRight.GetRightRange("index");
+                }
+                if (listx != null && listys[i] != null && listx.Count >= right && listys[i].Count >= right - left)
+                {
+                    for (int j = left; j < right; j++)
+                    {
+                        laserDataChart.Series[index + i].Points.AddXY(listx[j], listys[i][j - left]);
+                    }
+                }
+                dataDispList.Add(new DispMsg(laserDataChart.Series[index + i], left, right, listx, listys[i]));
+            }
+        }
+        /// <summary>
+        /// 更新图表数据
+        /// </summary>
+        /// <param name="listx"></param>
+        /// <param name="listys"></param>
+        /// <param name="laserIndex"></param>
+        public void updateChartData(List<double> listx, List<double> listy, int index)
+        {
+            try
+            {
+                dataDispList[index].series.Points.Clear();
+                for (int j = dataDispList[index].left; j < dataDispList[index].right; j++)
+                {
+                    if (listx != null && listy != null && listx.Count >= dataDispList[index].right && listy.Count >= dataDispList[index].right - dataDispList[index].left)
+                    {
+                        //Console.WriteLine(listx.Count + ", " + listy.Count + ", " + j + ", " + dataDispList[index].left);
+                        double x = listx[j];
+                        double y = listy[j - dataDispList[index].left];
+                        //dataDispList[index].series.Points.AddXY(listx[j], listy[j-dataDispList[index].left]);
+                        dataDispList[index].series.Points.AddXY(x, y);
+                    }
+                }
+            }
+            catch(Exception e) { Console.WriteLine(e.StackTrace); }
+        }
+
+        ///// <summary>
+        ///// 线性拟合
+        ///// </summary>
+        ///// <param name="x"></param>
+        ///// <param name="y"></param>
+        ///// <returns></returns>
+        //private static List<double> lineFit(List<double> x, List<double> y)
+        //{
+        //    double a, b, c;
+        //    int size = x.Count;
+        //    if (size < 2)
+        //    {
+        //        a = 0;
+        //        b = 0;
+        //        c = 0;
+        //        return null;
+        //    }
+        //    double x_mean = 0;
+        //    double y_mean = 0;
+        //    for (int i = 0; i < size; i++)
+        //    {
+        //        x_mean += x[i];
+        //        y_mean += y[i];
+        //    }
+        //    x_mean /= size;
+        //    y_mean /= size; //至此，计算出了 x y 的均值
+
+        //    double Dxx = 0, Dxy = 0, Dyy = 0;
+        //    for (int i = 0; i < size; i++)
+        //    {
+        //        Dxx += (x[i] - x_mean) * (x[i] - x_mean);
+        //        Dxy += (x[i] - x_mean) * (y[i] - y_mean);
+        //        Dyy += (y[i] - y_mean) * (y[i] - y_mean);
+        //    }
+
+        //    double lambda = ((Dxx + Dyy) - Math.Sqrt((Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy)) / 2.0;
+        //    double den = Math.Sqrt(Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx));
+        //    a = Dxy / den;
+        //    b = (lambda - Dxx) / den;
+        //    c = -a * x_mean - b * y_mean;
+        //    Console.WriteLine(a + "  " + b + "  " + c);
+        //    dis = new List<double>();
+        //    errorIndex.Clear();
+        //    for (int i = 0; i < size; i++)
+        //    {
+        //        if (y[i] < (-a * x[i] - c) / b)
+        //        {
+        //            double d = Math.Abs(a * x[i] + b * y[i] + c) / Math.Sqrt(a * a + b * b);
+
+        //            if (d > 40)
+        //            {
+        //                dis.Add(d);
+        //                errorIndex.Add(i);
+        //            }
+        //            //Log.WriteLog(LogType.PROCESS, d + "");
+        //        }
+        //    }
+        //    return dis;
+        //}
+
+        //private static List<double> lineFit2(List<double> y)
+        //{
+        //    double b;
+        //    int size = y.Count;
+        //    if (size < 2)
+        //    {
+        //        b = 0;
+        //        return null;
+        //    }
+        //    double sum = 0;
+        //    for(int i = 0; i < size; i++)
+        //    {
+        //        sum += y[i];
+        //    }
+        //    b = sum / size;
+        //    dis = new List<double>();
+        //    errorIndex.Clear();
+        //    for (int i = 0; i < size; i++)
+        //    {
+        //        if (y[i] < b)
+        //        {
+        //            double d = Math.Abs(y[i] - b);
+        //            if (d > 40)
+        //            {
+        //                dis.Add(d);
+        //                errorIndex.Add(i);
+        //            }
+        //        }
+        //    }
+        //    return dis;
+        //}
+        ///// <summary>
+        ///// 是否有障碍物
+        ///// </summary>
+        ///// <param name="testdata"></param>
+        ///// <param name="deviation"></param>
+        ///// <returns></returns>
+        //private static List<double> lineFit3(List<double> y)
+        //{
+        //    double b;
+        //    int size = y.Count;
+        //    if (size < 2)
+        //    {
+        //        b = 0;
+        //        return null;
+        //    }
+        //    double sum = 0;
+        //    for (int i = left_range; i < right_range; i++)
+        //    {
+        //        sum += y[i];
+        //    }
+        //    b = sum / (right_range - left_range);
+        //    dis = new List<double>();
+        //    errorIndex.Clear();
+        //    for (int i = left_range; i < right_range; i++)
+        //    {
+        //        double d = Math.Abs(y[i] - b);
+        //        if (d > 20)
+        //        {
+        //            double y_update = 0;
+        //            dis.Add(y_update);
+        //            errorIndex.Add(i);
+        //        }
+        //        else
+        //        {
+        //            dis.Add(y[i]);
+        //        }
+
+        //    }
+        //    return dis;
+        //}
+
+        //private static bool IsHinder(List<double> testdata, double deviation)
+        //{
+        //    bool isHinder = true;
+        //    //Data errordata = new Data();
+        //    List<int> recorddata = new List<int>();
+        //    for (int i = 0; i < testdata.Count; i++)
+        //    {
+        //        if (testdata[i] > deviation)
+        //        {
+        //            //errordata.key = i;
+        //            //errordata.value = testdata[i];
+        //            //Console.WriteLine(i + " " + testdata[i]);
+        //            recorddata.Add(i);
+        //        }
+        //    }
+        //    if (recorddata.Count <= 5 || IsBorderUpon(recorddata, 10) < 1)
+        //    {
+        //        isHinder = false;
+        //    }
+        //    return isHinder;
+        //}
+        ///// <summary>
+        ///// 连续判断
+        ///// </summary>
+        ///// <param name="arr"></param>
+        ///// <param name="num"></param>
+        ///// <returns></returns>
+
+        //private static int IsBorderUpon(List<int> arr, int num)
+        //{
+        //    var query = arr.OrderBy(p => p).Aggregate<int, List<List<int>>>(null, (m, n) =>
+        //    {
+        //        if (m == null) return new List<List<int>>() { new List<int>() { n } };
+        //        if (m.Last().Last() != n - 1)
+        //        {
+        //            m.Add(new List<int>() { n });
+        //        }
+        //        else
+        //        {
+        //            m.Last().Add(n);
+        //        }
+        //        return m;
+        //    });
+        //    int flag = 0;
+        //    for (int i = 0; i < query.Count; i++)
+        //    {
+        //        if (query[i].Count > 5)
+        //        {
+        //            flag++;
+        //        }
+        //    }
+        //    return flag;
+        //}
+
+
+        //********************** PLC ****************************
+
+        /// <summary>
+        /// 连接PLC
+        /// </summary>
+        /// <returns></returns>
+        private bool PLCConnect()
+        {
+            closed = false;
+            writeThread = new Thread(() =>
+            {
+                //Stopwatch stopwatch = new Stopwatch();
+                while (!closed)
+                {
+                    //stopwatch.Restart();
+                    bool result = WriteAccordingly();
+                    if (!result)
+                        Console.WriteLine("写PLC操作异常");
+                    Thread.Sleep(1000 / writeFreq);
+                    //stopwatch.Stop();
+                    //Console.WriteLine(stopwatch.ElapsedMilliseconds / 1000.0f);
+                }
+            });
+            writeThread.IsBackground = true;
+            writeThread.Priority = ThreadPriority.AboveNormal;
+            writeThread.Start();
+
+            if (plc == null)
+            {
+                plc = new Plc(cpu, ip, rack, slot);
+            }
+            if (plc != null)
+            {
+                ErrorCode err = plc.Open();
+                if (err != ErrorCode.NoError) { Console.WriteLine("connection error"); isConnected = false; }
+                else { Console.WriteLine("connected"); isConnected = true; }
+            }
+            else { isConnected = false; }
+            return isConnected;
+        }
+        /// <summary>
+        /// 关闭PLC连接
+        /// </summary>
+        private void PLCDisconnect()
+        {
+            //try
+            //{
+            //    if (writeThread != null)
+            //        writeThread.Abort();
+            //}
+            //catch (Exception ex) { Console.WriteLine("强制终止写PLC线程，" + ex.Message); }
+            closed = true;
+            if (plc != null)
+            {
+                plc.Close();
+            }
+        }
+        /// <summary>
+        /// 从阻塞队列获取需写入PLC数据并写入PLC，时间间隔在线程中控制
+        /// </summary>
+        /// <returns></returns>
+        private bool WriteAccordingly()
+        {
+            MsgNode msg = null;
+            try
+            {
+                msg = writingBlockingCollection.Take();
+                //Console.WriteLine("取出后，" + writingBlockingCollection.Count);
+            }
+            catch (Exception ex) { Console.WriteLine("获取PLC写对象异常，" + ex.Message); }
+            if (msg == null || !msg.valid)
+            {
+                Console.WriteLine("msg is null or invalid");
+                return false;
+            }
+            //更新设备状态
+            isConnected = plc.IsConnected;
+            if (isConnected)
+            {
+                lock (readWriteLock)
+                {
+                    plc.WriteBytes(DataType.DataBlock, msg.db, msg.start, msg.value.ToArray());
+                }
+            }
+            else { Console.WriteLine("掉线"); return false; }
+            return true;
+        }
+
+        /// <summary>
+        /// 读取PLC数据块
+        /// </summary>
+        /// <param name="whichToRead"></param>
+        /// <param name="index"></param>
+        /// <returns></returns>
+        private void ReadFromPLC(ref MsgNode msg)
+        {
+            //更新设备状态
+            isConnected = plc.IsConnected;
+            List<object> result = new List<object>();
+            if (!isConnected)
+            {
+                PLCConnect();
+            }
+            if (isConnected)
+            {
+                lock (readWriteLock)
+                {
+                    msg.value = plc.ReadBytes(DataType.DataBlock, msg.db, msg.start, msg.length).ToList();
+                    if (msg.value.Count != msg.length)
+                    {
+                        msg.valid = false;
+                    }
+                    msg.value.Reverse();
+                }
+            }
+        }
+        /// <summary>
+        /// 写入PLC，只支持1,2或4字节
+        /// </summary>
+        /// <param name="abstractPLCMsg"></param>
+        /// <param name="whoami"></param>
+        /// <returns></returns>
+        private bool WriteToPLC(MsgNode msg)
+        {
+            //更新设备状态
+            isConnected = plc.IsConnected;
+            if (msg != null && isConnected)
+            {
+                msg.value.Reverse();
+                writingBlockingCollection.Add(msg);
+                //Console.WriteLine("加入后，" + writingBlockingCollection.Count);
+                return true;
+            }
+            else
+                return false;
+        }
+
+        public class MsgNode : ICloneable
+        {
+            public int db;
+            public int start;
+            public int length;
+            public List<byte> value;
+            public bool valid;
+            public MsgNode(int db, int start, int length, List<byte> value)
+            {
+                this.db = db;
+                this.start = start;
+                this.length = length;
+                if (value != null && value.Count != length)
+                {
+                    valid = false;
+                }
+                else
+                {
+                    valid = true;
+                }
+                this.value = value;
+            }
+            public MsgNode() : this(0, 0, 0, new List<byte>())
+            {
+            }
+
+            public object Clone()
+            {
+                MsgNode obj = new MsgNode();
+                obj.db = db;
+                obj.start = start;
+                obj.length = length;
+                obj.value = new List<byte>(value);
+                obj.valid = valid;
+                return obj;
+            }
+        }
+
+        public class DispMsg
+        {
+            public Series series;
+            public int left;
+            public int right;
+            public List<double> x;
+            public List<double> y;
+            public DispMsg(Series series, int left, int right, List<double> x, List<double> y)
+            {
+                this.series = series;
+                this.left = left;
+                this.right = right;
+                this.x = x;
+                this.y = y;
+            }
+        }
+    }
+}

encTest/Form1.resx

@@ -0,0 +1,120 @@
+<?xml version="1.0" encoding="utf-8"?>
+<root>
+  <!-- 
+    Microsoft ResX Schema 
+    
+    Version 2.0
+    
+    The primary goals of this format is to allow a simple XML format 
+    that is mostly human readable. The generation and parsing of the 
+    various data types are done through the TypeConverter classes 
+    associated with the data types.
+    
+    Example:
+    
+    ... ado.net/XML headers & schema ...
+    <resheader name="resmimetype">text/microsoft-resx</resheader>
+    <resheader name="version">2.0</resheader>
+    <resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
+    <resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
+    <data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
+    <data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
+    <data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
+        <value>[base64 mime encoded serialized .NET Framework object]</value>
+    </data>
+    <data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
+        <value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
+        <comment>This is a comment</comment>
+    </data>
+                
+    There are any number of "resheader" rows that contain simple 
+    name/value pairs.
+    
+    Each data row contains a name, and value. The row also contains a 
+    type or mimetype. Type corresponds to a .NET class that support 
+    text/value conversion through the TypeConverter architecture. 
+    Classes that don't support this are serialized and stored with the 
+    mimetype set.
+    
+    The mimetype is used for serialized objects, and tells the 
+    ResXResourceReader how to depersist the object. This is currently not 
+    extensible. For a given mimetype the value must be set accordingly:
+    
+    Note - application/x-microsoft.net.object.binary.base64 is the format 
+    that the ResXResourceWriter will generate, however the reader can 
+    read any of the formats listed below.
+    
+    mimetype: application/x-microsoft.net.object.binary.base64
+    value   : The object must be serialized with 
+            : System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
+            : and then encoded with base64 encoding.
+    
+    mimetype: application/x-microsoft.net.object.soap.base64
+    value   : The object must be serialized with 
+            : System.Runtime.Serialization.Formatters.Soap.SoapFormatter
+            : and then encoded with base64 encoding.
+
+    mimetype: application/x-microsoft.net.object.bytearray.base64
+    value   : The object must be serialized into a byte array 
+            : using a System.ComponentModel.TypeConverter
+            : and then encoded with base64 encoding.
+    -->
+  <xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
+    <xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
+    <xsd:element name="root" msdata:IsDataSet="true">
+      <xsd:complexType>
+        <xsd:choice maxOccurs="unbounded">
+          <xsd:element name="metadata">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" />
+              </xsd:sequence>
+              <xsd:attribute name="name" use="required" type="xsd:string" />
+              <xsd:attribute name="type" type="xsd:string" />
+              <xsd:attribute name="mimetype" type="xsd:string" />
+              <xsd:attribute ref="xml:space" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="assembly">
+            <xsd:complexType>
+              <xsd:attribute name="alias" type="xsd:string" />
+              <xsd:attribute name="name" type="xsd:string" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="data">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+                <xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
+              <xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
+              <xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
+              <xsd:attribute ref="xml:space" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="resheader">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" use="required" />
+            </xsd:complexType>
+          </xsd:element>
+        </xsd:choice>
+      </xsd:complexType>
+    </xsd:element>
+  </xsd:schema>
+  <resheader name="resmimetype">
+    <value>text/microsoft-resx</value>
+  </resheader>
+  <resheader name="version">
+    <value>2.0</value>
+  </resheader>
+  <resheader name="reader">
+    <value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+  <resheader name="writer">
+    <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+</root>

encTest/LogManager.cs

@@ -0,0 +1,256 @@
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Text;
+
+namespace Test
+{
+    /// <summary>
+    /// 日志管理
+    /// </summary>
+    public class LogManager
+    {
+        private string filepath = "log";
+        //public static string logAddressStr = "filepath";
+        //public static string logAddress;
+        DateTime time;                                        //文件创建时间
+        private string logFileExtName = "log";     //日志文件扩展名
+        private Encoding logFileEncoding = Encoding.UTF8;   //日志文件编码格式
+        private string logFileName = string.Empty;  //日志文件名
+        private string logPath = "";            //日志文件路径
+        private bool writeLogTime = true;     //log文件是否写时间
+        private bool writeStatus = false;        //是否写入标志位
+        private static object obj = new object();
+        /// <summary>
+        /// 配置文件初始化
+        /// </summary>
+        public static void Init()
+        {
+            //try
+            //{
+            //    logAddress = ConfigurationManager.AppSettings[logAddressStr];
+            //}
+            //catch
+            //{
+            //    Console.WriteLine("配置文件有误");
+            //}
+        }
+        /// <summary>
+        /// 日志文件路径
+        /// </summary>
+        public string CreateLogPath()
+        {
+            if (!Directory.Exists(filepath))
+            {
+                try
+                {
+                    Directory.CreateDirectory(filepath);
+                }
+                catch
+                {
+                    Console.WriteLine("创建文件路径失败");
+                }
+            }
+            if (logPath == null || logPath == string.Empty || time.ToString("yyyy-MM-dd") != System.DateTime.Now.ToString("yyyy-MM-dd"))
+            {
+                try
+                {
+                    time = System.DateTime.Now;
+                    logPath = System.IO.Path.Combine(filepath, time.ToString("yyyy-MM-dd"));
+                }
+                catch
+                {
+                    Console.WriteLine("路径合成失败");
+                }
+            }
+            if (!logPath.EndsWith(@"\"))
+            {
+                logPath += @"\";
+            }
+            if (!Directory.Exists(logPath))
+            {
+                try
+                {
+                    Directory.CreateDirectory(logPath);
+                }
+                catch
+                {
+                    Console.WriteLine("创建文件路径失败");
+                }
+            }
+            return logPath;
+        }
+        /// <summary>
+        /// 写日志
+        /// </summary>
+        public void WriteLog(LogType logType, string logFile, string msg)
+        {
+            CreateLogPath();
+            lock (obj)
+            {
+                try
+                {
+                    //创建log文件
+                    logFileName = string.Format("{0}{1}.{2}", logPath, logType, this.logFileExtName);
+                    using (StreamWriter sw = new StreamWriter(logFileName, true, logFileEncoding))
+                    {
+                        //是否写时间
+                        if (logType == LogType.PROCESS)
+                        {
+                            writeLogTime = true;
+                        }
+                        else
+                        {
+                            writeLogTime = false;
+                        }
+                        ////sql类型
+                        //if (logType == LogType.DATABASE)
+                        //{
+                        //    writeStatus = true;
+                        //}
+                        //else
+                        //{
+                        //    writeStatus = false;
+                        //}
+                        if (writeLogTime)
+                        {
+                            sw.WriteLine(DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss:") + logFile + ":" + msg);
+                        }
+                        else
+                        {
+                            sw.WriteLine(logFile + "" + msg);
+                        }
+                    }
+                }
+                catch { }
+            }
+        }
+        /// <summary>
+        /// log类型不定
+        /// </summary>
+        /// <param name="logType"></param>
+        /// <param name="logFile"></param>
+        /// <param name="msg"></param>
+        public void WriteLog(LogType logType, LogFile logFile, string msg)
+        {
+            this.WriteLog(logType, logFile.ToString(), msg);
+        }
+        /// <summary>
+        /// log类型为null
+        /// </summary>
+        /// <param name="logType"></param>
+        /// <param name="msg"></param>
+        public void WriteLog(LogType logType, string msg)
+        {
+            this.WriteLog(logType, string.Empty, msg);
+        }
+        /// <summary>
+        /// 读日志文件
+        /// </summary>
+        /// <param name="sqlStatus"></param>
+        /// <param name="sqlMsg"></param>
+        public List<string> ReadLog()
+        {
+            List<string> digitList = new List<string>();
+            //string date = System.DateTime.Now.ToString("yyyy-MM-dd");
+            //string filePath = logAddress + "\\\\" + date + "\\\\" + "CREDENCES.log";
+            string filePath = filepath + "\\\\" + "CREDENCES.log";
+            try
+            {
+                List<string> logLines = null;
+                if (File.Exists(filePath))
+                {
+                    logLines = new List<string>(File.ReadAllLines(filePath));
+                    File.Delete(filePath);
+                }
+                if (logLines != null)
+                {
+                    for (int i = 0; i < 300; i++)
+                    {
+                        string logLine = logLines[i];
+                        digitList.Add(logLine);
+                        logLines.RemoveAt(i);
+                    }
+                    File.WriteAllLines(filePath, logLines.ToArray());
+                }
+            }
+            catch (Exception e) { Console.WriteLine(e.Message); }
+            return digitList;
+        }
+        public void WriteCredence(string msg)
+        {
+            lock (obj)
+            {
+                try
+                {
+                    //创建log文件
+                    string logname = string.Format("{0}.{1}", "CREDENCES", this.logFileExtName);
+                    logFileName = filepath + "\\" + logname;
+                    using (StreamWriter sw = new StreamWriter(logFileName, true, logFileEncoding))
+                    {
+                        if (msg.Length == 8)
+                        {
+                            sw.WriteLine(msg);
+                        }
+                        else
+                        {
+                            sw.WriteLine(msg);
+                        }
+                    }
+                }
+                catch { }
+            }
+        }
+
+        public void WriteData(string name, string msg)
+        {
+            CreateLogPath();
+            lock (obj)
+            {
+                try
+                {
+                    //创建log文件
+                    string logname = string.Format("{0}.{1}", name, "txt");
+                    //logFileName = filepath + "\\" + logname;
+                    logFileName = logPath + "\\" + logname;
+                    using (StreamWriter sw = new StreamWriter(logFileName, true, logFileEncoding))
+                    {
+                        if (msg.Length == 8)
+                        {
+                            sw.WriteLine(msg);
+                        }
+                        else
+                        {
+                            sw.WriteLine(msg);
+                        }
+                    }
+                }
+                catch { }
+            }
+        }
+
+
+
+
+    }
+    /// <summary>
+    /// log类型
+    /// </summary>
+    public enum LogFile
+    {
+        LOG,
+        RESET,
+        ERROR,
+        WARNING,
+        INFO,
+        ERROR_NUMBERPLATE
+    }
+    /// <summary>
+    /// log文件类型
+    /// </summary>
+    public enum LogType
+    {
+        PROCESS,
+        CREDENCES
+    }
+}

encTest/Program.cs

@@ -0,0 +1,22 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Threading.Tasks;
+using System.Windows.Forms;
+
+namespace Test
+{
+    static class Program
+    {
+        /// <summary>
+        /// 应用程序的主入口点。
+        /// </summary>
+        [STAThread]
+        static void Main()
+        {
+            Application.EnableVisualStyles();
+            Application.SetCompatibleTextRenderingDefault(false);
+            Application.Run(new obstacleDetectionForm());
+        }
+    }
+}

encTest/Properties/AssemblyInfo.cs

@@ -0,0 +1,36 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+// 有关程序集的一般信息由以下
+// 控制。更改这些特性值可修改
+// 与程序集关联的信息。
+[assembly: AssemblyTitle("encTest")]
+[assembly: AssemblyDescription("")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("")]
+[assembly: AssemblyProduct("encTest")]
+[assembly: AssemblyCopyright("Copyright ©  2019")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// 将 ComVisible 设置为 false 会使此程序集中的类型
+//对 COM 组件不可见。如果需要从 COM 访问此程序集中的类型
+//请将此类型的 ComVisible 特性设置为 true。
+[assembly: ComVisible(false)]
+
+// 如果此项目向 COM 公开，则下列 GUID 用于类型库的 ID
+[assembly: Guid("bf838638-f53a-42e7-81cc-16b3c77c8898")]
+
+// 程序集的版本信息由下列四个值组成: 
+//
+//      主版本
+//      次版本
+//      生成号
+//      修订号
+//
+// 可以指定所有值，也可以使用以下所示的 "*" 预置版本号和修订号
+// 方法是按如下所示使用“*”: :
+// [assembly: AssemblyVersion("1.0.*")]
+[assembly: AssemblyVersion("1.0.0.0")]
+[assembly: AssemblyFileVersion("1.0.0.0")]

encTest/Properties/Resources.Designer.cs

@@ -0,0 +1,71 @@
+//------------------------------------------------------------------------------
+// <auto-generated>
+//     此代码由工具生成。
+//     运行时版本: 4.0.30319.42000
+//
+//     对此文件的更改可能导致不正确的行为，如果
+//     重新生成代码，则所做更改将丢失。
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+namespace encTest.Properties
+{
+
+
+    /// <summary>
+    ///   强类型资源类，用于查找本地化字符串等。
+    /// </summary>
+    // 此类是由 StronglyTypedResourceBuilder
+    // 类通过类似于 ResGen 或 Visual Studio 的工具自动生成的。
+    // 若要添加或删除成员，请编辑 .ResX 文件，然后重新运行 ResGen
+    // (以 /str 作为命令选项)，或重新生成 VS 项目。
+    [global::System.CodeDom.Compiler.GeneratedCodeAttribute("System.Resources.Tools.StronglyTypedResourceBuilder", "4.0.0.0")]
+    [global::System.Diagnostics.DebuggerNonUserCodeAttribute()]
+    [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()]
+    internal class Resources
+    {
+
+        private static global::System.Resources.ResourceManager resourceMan;
+
+        private static global::System.Globalization.CultureInfo resourceCulture;
+
+        [global::System.Diagnostics.CodeAnalysis.SuppressMessageAttribute("Microsoft.Performance", "CA1811:AvoidUncalledPrivateCode")]
+        internal Resources()
+        {
+        }
+
+        /// <summary>
+        ///   返回此类使用的缓存 ResourceManager 实例。
+        /// </summary>
+        [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)]
+        internal static global::System.Resources.ResourceManager ResourceManager
+        {
+            get
+            {
+                if ((resourceMan == null))
+                {
+                    global::System.Resources.ResourceManager temp = new global::System.Resources.ResourceManager("encTest.Properties.Resources", typeof(Resources).Assembly);
+                    resourceMan = temp;
+                }
+                return resourceMan;
+            }
+        }
+
+        /// <summary>
+        ///   覆盖当前线程的 CurrentUICulture 属性
+        ///   使用此强类型的资源类的资源查找。
+        /// </summary>
+        [global::System.ComponentModel.EditorBrowsableAttribute(global::System.ComponentModel.EditorBrowsableState.Advanced)]
+        internal static global::System.Globalization.CultureInfo Culture
+        {
+            get
+            {
+                return resourceCulture;
+            }
+            set
+            {
+                resourceCulture = value;
+            }
+        }
+    }
+}

encTest/Properties/Resources.resx

@@ -0,0 +1,117 @@
+<?xml version="1.0" encoding="utf-8"?>
+<root>
+  <!-- 
+    Microsoft ResX Schema 
+    
+    Version 2.0
+    
+    The primary goals of this format is to allow a simple XML format 
+    that is mostly human readable. The generation and parsing of the 
+    various data types are done through the TypeConverter classes 
+    associated with the data types.
+    
+    Example:
+    
+    ... ado.net/XML headers & schema ...
+    <resheader name="resmimetype">text/microsoft-resx</resheader>
+    <resheader name="version">2.0</resheader>
+    <resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
+    <resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
+    <data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
+    <data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
+    <data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
+        <value>[base64 mime encoded serialized .NET Framework object]</value>
+    </data>
+    <data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
+        <value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
+        <comment>This is a comment</comment>
+    </data>
+                
+    There are any number of "resheader" rows that contain simple 
+    name/value pairs.
+    
+    Each data row contains a name, and value. The row also contains a 
+    type or mimetype. Type corresponds to a .NET class that support 
+    text/value conversion through the TypeConverter architecture. 
+    Classes that don't support this are serialized and stored with the 
+    mimetype set.
+    
+    The mimetype is used for serialized objects, and tells the 
+    ResXResourceReader how to depersist the object. This is currently not 
+    extensible. For a given mimetype the value must be set accordingly:
+    
+    Note - application/x-microsoft.net.object.binary.base64 is the format 
+    that the ResXResourceWriter will generate, however the reader can 
+    read any of the formats listed below.
+    
+    mimetype: application/x-microsoft.net.object.binary.base64
+    value   : The object must be serialized with 
+            : System.Serialization.Formatters.Binary.BinaryFormatter
+            : and then encoded with base64 encoding.
+    
+    mimetype: application/x-microsoft.net.object.soap.base64
+    value   : The object must be serialized with 
+            : System.Runtime.Serialization.Formatters.Soap.SoapFormatter
+            : and then encoded with base64 encoding.
+
+    mimetype: application/x-microsoft.net.object.bytearray.base64
+    value   : The object must be serialized into a byte array 
+            : using a System.ComponentModel.TypeConverter
+            : and then encoded with base64 encoding.
+    -->
+  <xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
+    <xsd:element name="root" msdata:IsDataSet="true">
+      <xsd:complexType>
+        <xsd:choice maxOccurs="unbounded">
+          <xsd:element name="metadata">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" />
+              <xsd:attribute name="type" type="xsd:string" />
+              <xsd:attribute name="mimetype" type="xsd:string" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="assembly">
+            <xsd:complexType>
+              <xsd:attribute name="alias" type="xsd:string" />
+              <xsd:attribute name="name" type="xsd:string" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="data">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+                <xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" msdata:Ordinal="1" />
+              <xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
+              <xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
+            </xsd:complexType>
+          </xsd:element>
+          <xsd:element name="resheader">
+            <xsd:complexType>
+              <xsd:sequence>
+                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
+              </xsd:sequence>
+              <xsd:attribute name="name" type="xsd:string" use="required" />
+            </xsd:complexType>
+          </xsd:element>
+        </xsd:choice>
+      </xsd:complexType>
+    </xsd:element>
+  </xsd:schema>
+  <resheader name="resmimetype">
+    <value>text/microsoft-resx</value>
+  </resheader>
+  <resheader name="version">
+    <value>2.0</value>
+  </resheader>
+  <resheader name="reader">
+    <value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+  <resheader name="writer">
+    <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
+  </resheader>
+</root>

encTest/Properties/Settings.Designer.cs

@@ -0,0 +1,30 @@
+//------------------------------------------------------------------------------
+// <auto-generated>
+//     This code was generated by a tool.
+//     Runtime Version:4.0.30319.42000
+//
+//     Changes to this file may cause incorrect behavior and will be lost if
+//     the code is regenerated.
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+namespace encTest.Properties
+{
+
+
+    [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()]
+    [global::System.CodeDom.Compiler.GeneratedCodeAttribute("Microsoft.VisualStudio.Editors.SettingsDesigner.SettingsSingleFileGenerator", "11.0.0.0")]
+    internal sealed partial class Settings : global::System.Configuration.ApplicationSettingsBase
+    {
+
+        private static Settings defaultInstance = ((Settings)(global::System.Configuration.ApplicationSettingsBase.Synchronized(new Settings())));
+
+        public static Settings Default
+        {
+            get
+            {
+                return defaultInstance;
+            }
+        }
+    }
+}

encTest/Properties/Settings.settings

@@ -0,0 +1,7 @@
+<?xml version='1.0' encoding='utf-8'?>
+<SettingsFile xmlns="http://schemas.microsoft.com/VisualStudio/2004/01/settings" CurrentProfile="(Default)">
+  <Profiles>
+    <Profile Name="(Default)" />
+  </Profiles>
+  <Settings />
+</SettingsFile>

encTest/SickTimEnc.cs

@@ -0,0 +1,465 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace Test
+{
+    class SickTimEnc
+    {
+        //状态指示变量
+        /// <summary>
+        /// 指示初始化是否成功
+        /// </summary>
+        public bool initialized { get; set; }
+        /// <summary>
+        /// 指示是否工作中
+        /// </summary>
+        public bool working { get; set; }
+        /// <summary>
+        /// 指示是否检测到障碍物
+        /// </summary>
+        public bool obstacleDetected { get; set; }
+        /// <summary>
+        /// 发送指令返回值
+        /// </summary>
+        private int flag = 0;
+        //通信参数
+        private readonly object Lock = new object();
+        private IPAddress ipa;
+        private int port;
+        private TcpClient tcpClient;
+        private NetworkStream stream;
+        private int bufferSize = 16384;
+        private const string startCmd = "02 73 52 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 03";
+        private const string endCmd = "02 73 45 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 30 03";
+        //防撞参数
+        public int maxDataCount { get; set; }//最大保留历史数据个数
+        private int left_range = 385;//325;//231;//271;//386;//366;
+        private int right_range = 425;//485;//581;//541;//426;//446;
+        public int ground_truth_height { get; set; } = 2200;//2200;//2300;//标准高度
+        public int min_num_point_of_hinder { get; set; } = 30;//20;//判定为障碍物的最少点的数目
+        public int min_obstacle_height { get; set; } = 500;//最小障碍物高度
+        private List<string> originDatalist;//原始数据数组
+        private List<int> validNum;//811个整型值
+        private List<List<double>> validNumBuffer;//保存历史点云数据
+        private List<List<double>> validNumBufferKalman;//保存经过kalman滤波的历史点云数据
+        private List<double> listy;
+        public List<double> listx { get; set; }
+        public List<double> yFiltered { get; set; }
+        public List<double> yFilteredKalman { get; set; }
+        //卡尔曼滤波参数
+        private List<double> prevData, p, q, r, kGain;
+
+        public SickTimEnc(string ip, int port)
+        {
+            initialized = false;
+            maxDataCount = 3;
+            originDatalist = new List<string>();
+            validNum = new List<int>();
+            validNumBuffer = new List<List<double>>();
+            validNumBufferKalman = new List<List<double>>();
+            prevData = new List<double>();
+            p = new List<double>();
+            q = new List<double>();
+            r = new List<double>();
+            kGain = new List<double>();
+            for (int i = 0; i < right_range - left_range; i++)
+            {
+                prevData.Add(0);
+                p.Add(450);
+                q.Add(0.0001);
+                r.Add(0.005);
+                kGain.Add(0);
+            }
+            listy = new List<double>();
+            listx = new List<double>();
+            tcpClient = new TcpClient();
+            if(!IPAddress.TryParse(ip, out ipa))
+            {
+                ipa = null;
+                return;
+            }
+            try
+            {
+                tcpClient.Connect(ipa, port);
+                stream = tcpClient.GetStream();
+            }
+            catch { return; }
+            initialized = true;
+            working = true;
+            work();
+        }
+        /// <summary>
+        /// 暂停工作
+        /// </summary>
+        /// <param name="ctrl"></param>
+        public void Pause()
+        {
+            working = !working;
+        }
+        /// <summary>
+        /// 停止工作
+        /// </summary>
+        public void Stop()
+        {
+            working = false;
+            flag = sendCmd(endCmd);
+            if (stream != null)
+            {
+                stream.Close();
+            }
+            if (tcpClient != null)
+            {
+                tcpClient.Close();
+            }
+        }
+        /// <summary>
+        /// 获取最小角度阈值，范围-45 - 225
+        /// </summary>
+        /// <param name="type">类型，点编号index或角度degree</param>
+        /// <returns></returns>
+        public int GetLeftRange(string type)
+        {
+            if (type == "degree")
+                return left_range / 3 - 45;
+            else
+                return left_range;
+        }
+        /// <summary>
+        /// 获取最大角度阈值，范围-45 - 225
+        /// </summary>
+        /// <param name="type">类型，点编号index或角度degree</param>
+        /// <returns></returns>
+        public int GetRightRange(string type)
+        {
+            if (type == "degree")
+                return right_range / 3 - 45;
+            else
+                return right_range;
+        }
+        ///// <summary>
+        ///// 获取距地面高度
+        ///// </summary>
+        ///// <returns></returns>
+        //public int GetHeight()
+        //{
+        //    return ground_truth_height;
+        //}
+        /// <summary>
+        /// 设置最小角度阈值
+        /// </summary>
+        /// <param name="degree"></param>
+        public void SetLeftRange(int degree)
+        {
+            if (working)
+            {
+                working = false;
+                lock (Lock)
+                {
+                    left_range = (degree + 45) * 3;
+                }
+                working = true;
+            }
+            else
+            {
+                left_range = (degree + 45) * 3;
+            }
+            initKalman();
+        }
+        /// <summary>
+        /// 设置最大角度阈值
+        /// </summary>
+        /// <param name="degree"></param>
+        public void SetRightRange(int degree)
+        {
+            if (working)
+            {
+                working = false;
+                lock (Lock)
+                {
+                    right_range = (degree + 45) * 3;
+                }
+                working = true;
+            }
+            else
+            {
+                right_range = (degree + 45) * 3;
+            }
+            initKalman();
+        }
+        ///// <summary>
+        ///// 设置距地面高度
+        ///// </summary>
+        ///// <param name="height"></param>
+        //public void SetHeight(int height)
+        //{
+        //    ground_truth_height = height;
+        //}
+
+
+        /// <summary>
+        /// 初始化kalman滤波参数
+        /// </summary>
+        private void initKalman()
+        {
+            prevData.Clear();
+            p.Clear();
+            q.Clear();
+            r.Clear();
+            kGain.Clear();
+            for (int i = 0; i < right_range - left_range; i++)
+            {
+                prevData.Add(0);
+                p.Add(450);
+                q.Add(0.0001);
+                r.Add(0.005);
+                kGain.Add(0);
+            }
+        }
+        /// <summary>
+        /// 启动雷达处理数据
+        /// </summary>
+        private void work()
+        {
+            Task.Factory.StartNew(()=>
+            {
+                while (initialized)
+                {
+                    try
+                    {
+                        if (working)
+                        {
+                        //开辟线程存储雷达原始数据字符串
+                            lock (Lock)
+                            {
+                                originDatalist.Clear();
+                                flag = sendCmd(startCmd);
+                                if (flag == 1)
+                                {
+                                    byte[] buffer = new byte[bufferSize];
+                                    stream.Read(buffer, 0, buffer.Length);
+                                    string str = HexStringToASCII(buffer);
+                                    originDatalist.Add(str);
+                                }
+                                if (originDatalist.Count != 0)
+                                {
+                                    for (int i = 0; i < originDatalist.Count; i++)
+                                    {
+                                        string ss = trimDataStr(originDatalist[i], "sRA LMDscandata 1 ", " not defined 0 0 0");
+                                        string[] data = ss.Split(' ');
+                                        string number = data[22];
+                                        int length = Convert.ToInt32(number, 16);
+                                        int temp = 0;
+                                        //int[] validNum = new int[length];
+                                        //string aws = "";
+                                        //获取所有距离数值
+                                        validNum.Clear();
+                                        listy.Clear();
+                                        listx.Clear();
+                                        for (int j = 0; j < length; j++)
+                                        {
+                                            try
+                                            {
+                                                temp = Convert.ToInt32(data[23 + j], 16);
+                                            }
+                                            catch { temp = ground_truth_height; Console.WriteLine("数据转换异常"); }
+                                            validNum.Add(temp);
+                                        }
+                                        for (int j = 0; j < length; j++)
+                                        {
+                                            double x = validNum[j] * Math.Cos((-45 + 0.333 * j) * Math.PI / 180);
+                                            double y = validNum[j] * Math.Sin((-45 + 0.333 * j) * Math.PI / 180);
+                                            listy.Add(y);
+                                            listx.Add(x);
+                                        }
+                                        validNumBuffer.Add(listy);
+                                        validNumBufferKalman.Add(kalmanFilter(listy));
+                                        while (validNumBufferKalman.Count > maxDataCount)
+                                        {
+                                            validNumBufferKalman.RemoveAt(0);
+                                            validNumBuffer.RemoveAt(0);
+                                        }
+                                        if(validNumBufferKalman.Count == maxDataCount)
+                                        {
+                                            bool obstacle = false;
+                                            yFiltered = dataFilter(validNumBuffer, out obstacle);
+                                            yFilteredKalman = dataFilter(validNumBufferKalman, out obstacle);
+                                            obstacleDetected = obstacle;
+                                        }
+                                    }
+                                }
+                            }
+                            Thread.Sleep(1);
+                        }
+                        else
+                        {
+                            //空闲状态
+                            Thread.Sleep(1000);
+                        }
+                    }
+                    catch (Exception e) { Thread.Sleep(500); Console.WriteLine(e.Message); Console.WriteLine(e.StackTrace); }
+                    
+                }
+            });
+        }
+        /// <summary>
+        /// 向雷达发送指令
+        /// </summary>
+        /// <param name="cmd"></param>
+        /// <returns></returns>
+        private int sendCmd(string cmd)
+        {
+            try
+            {
+                if (cmd.Equals(startCmd))
+                {
+                    byte[] byteStartCmd = HexStringToBinary(startCmd);
+                    string result = HexStringToASCII(byteStartCmd);
+                    byte[] b = Encoding.UTF8.GetBytes(result);
+                    stream.Write(b, 0, b.Length);
+                    return 1;
+                }
+                if (cmd.Equals(endCmd))
+                {
+                    byte[] byteStartCmd = HexStringToBinary(endCmd);
+                    string result = HexStringToASCII(byteStartCmd);
+                    byte[] b = Encoding.UTF8.GetBytes(result);
+                    stream.Write(b, 0, b.Length);
+                    return 2;
+                }
+            }
+            catch { return 0; }
+            return 0;
+        }
+        /// <summary>
+        /// 将一条十六进制字符串转换为ASCII
+        /// </summary>
+        /// <param name="hexstring">一条十六进制字符串</param>
+        /// <returns>返回一条ASCII码</returns>
+        private static string HexStringToASCII(byte[] bt)
+        {
+            //byte[] bt = HexStringToBinary(hexstring);
+            string lin = "";
+            for (int i = 0; i < bt.Length; i++)
+            {
+                lin = lin + bt[i] + " ";
+            }
+
+
+            string[] ss = lin.Trim().Split(new char[] { ' ' });
+            char[] c = new char[ss.Length];
+            int a;
+            for (int i = 0; i < c.Length; i++)
+            {
+                a = Convert.ToInt32(ss[i]);
+                c[i] = Convert.ToChar(a);
+            }
+
+            string b = new string(c);
+            return b;
+        }
+        /// <summary>
+        /// 16进制字符串转换为二进制数组
+        /// </summary>
+        /// <param name="hexstring">用空格切割字符串</param>
+        /// <returns>返回一个二进制字符串</returns>
+        private static byte[] HexStringToBinary(string hexstring)
+        {
+
+            string[] tmpary = hexstring.Trim().Split(' ');
+            byte[] buff = new byte[tmpary.Length];
+            for (int i = 0; i < buff.Length; i++)
+            {
+                buff[i] = Convert.ToByte(tmpary[i], 16);
+            }
+            return buff;
+        }
+        /// <summary>
+        /// 截取数据段
+        /// </summary>
+        /// <param name="sourse"></param>
+        /// <param name="startstr"></param>
+        /// <param name="endstr"></param>
+        /// <returns></returns>
+        private static string trimDataStr(string sourse, string startstr, string endstr)
+        {
+            string result = string.Empty;
+            int startindex, endindex;
+            try
+            {
+                startindex = sourse.IndexOf(startstr);
+                if (startindex == -1)
+                {
+                    return result;
+                }
+                string tmpstr = sourse.Substring(startindex + startstr.Length);
+                endindex = tmpstr.IndexOf(endstr);
+                if (endindex == -1)
+                {
+                    return result;
+                }
+                result = tmpstr.Remove(endindex);
+            }
+            catch (Exception) { }
+            return result;
+        }
+        /// <summary>
+        /// 平滑数据，识别障碍物
+        /// </summary>
+        /// <param name="y"></param>
+        /// <param name="isObstacle"></param>
+        /// <returns></returns>
+        private List<double> dataFilter(List<List<double>> y, out bool isObstacle)
+        {
+            List<double> y_udt = new List<double>();
+            isObstacle = false;
+            int hinder_point_count = 0;
+            for (int i = left_range; i < right_range; i++)
+            {
+                int hinder_frame_count = 0;
+
+                for (int j = 0; j < y.Count; j++)
+                {
+                    if (i != left_range && i != right_range - 1)
+                    {
+                        y[j][i] = (y[j][i - 1] + y[j][i] + y[j][i + 1]) / 3;//每个点的y为其和相邻两个点和的平均值
+                    }
+                    double d = Math.Abs(y[j][i] - ground_truth_height);
+                    if (d > min_obstacle_height)//300
+                        hinder_frame_count = hinder_frame_count + 1;
+                }
+
+                if (hinder_frame_count == y.Count)
+                    hinder_point_count = hinder_point_count + 1;
+                y_udt.Add(y[y.Count - 1][i]);
+            }
+            if (hinder_point_count >= min_num_point_of_hinder)
+                isObstacle = true;
+            return y_udt;
+        }
+        /// <summary>
+        /// 卡尔曼滤波
+        /// </summary>
+        /// <param name="data_in"></param>
+        /// <returns></returns>
+        private List<double> kalmanFilter(List<double> data_in)
+        {
+            for (int i = left_range; i < right_range; i++)
+            {
+                p[i - left_range] = p[i - left_range] + q[i - left_range];
+                kGain[i - left_range] = p[i - left_range] / (p[i - left_range] + r[i - left_range]);
+                data_in[i] = prevData[i - left_range] + (kGain[i - left_range] * (data_in[i] - prevData[i - left_range]));
+                p[i - left_range] = (1 - kGain[i - left_range]) * p[i - left_range];
+                prevData[i - left_range] = data_in[i];
+            }
+
+            return data_in;
+        }
+
+    }
+}

encTest/encTest.csproj

@@ -0,0 +1,87 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProjectGuid>{BF838638-F53A-42E7-81CC-16B3C77C8898}</ProjectGuid>
+    <OutputType>WinExe</OutputType>
+    <RootNamespace>encTest</RootNamespace>
+    <AssemblyName>encTest</AssemblyName>
+    <TargetFrameworkVersion>v4.5</TargetFrameworkVersion>
+    <FileAlignment>512</FileAlignment>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="snap7Enc">
+      <HintPath>sdk\snap7Enc.dll</HintPath>
+    </Reference>
+    <Reference Include="System" />
+    <Reference Include="System.Core" />
+    <Reference Include="System.Windows.Forms.DataVisualization" />
+    <Reference Include="System.Windows.Forms.DataVisualization.Design" />
+    <Reference Include="System.Xml.Linq" />
+    <Reference Include="System.Data.DataSetExtensions" />
+    <Reference Include="Microsoft.CSharp" />
+    <Reference Include="System.Data" />
+    <Reference Include="System.Deployment" />
+    <Reference Include="System.Drawing" />
+    <Reference Include="System.Net.Http" />
+    <Reference Include="System.Windows.Forms" />
+    <Reference Include="System.Xml" />
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="Form1.cs">
+      <SubType>Form</SubType>
+    </Compile>
+    <Compile Include="Form1.Designer.cs">
+      <DependentUpon>Form1.cs</DependentUpon>
+    </Compile>
+    <Compile Include="Program.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="SickTimEnc.cs" />
+    <EmbeddedResource Include="Form1.resx">
+      <DependentUpon>Form1.cs</DependentUpon>
+    </EmbeddedResource>
+    <EmbeddedResource Include="Properties\Resources.resx">
+      <Generator>ResXFileCodeGenerator</Generator>
+      <LastGenOutput>Resources.Designer.cs</LastGenOutput>
+      <SubType>Designer</SubType>
+    </EmbeddedResource>
+    <Compile Include="Properties\Resources.Designer.cs">
+      <AutoGen>True</AutoGen>
+      <DependentUpon>Resources.resx</DependentUpon>
+    </Compile>
+    <None Include="Properties\Settings.settings">
+      <Generator>SettingsSingleFileGenerator</Generator>
+      <LastGenOutput>Settings.Designer.cs</LastGenOutput>
+    </None>
+    <Compile Include="Properties\Settings.Designer.cs">
+      <AutoGen>True</AutoGen>
+      <DependentUpon>Settings.settings</DependentUpon>
+      <DesignTimeSharedInput>True</DesignTimeSharedInput>
+    </Compile>
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="App.config" />
+  </ItemGroup>
+  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
+</Project>

encTest/log.cs

@@ -0,0 +1,99 @@
+using System.Collections.Generic;
+
+namespace Test
+{
+    public static class Log
+    {
+        private static LogManager logManager;
+        static Log()
+        {
+            LogManager.Init();
+            logManager = new LogManager();
+        }
+        /// <summary>
+        /// 写文件类型
+        /// </summary>
+        /// <param name="logType"></param>
+        /// <param name="logFile"></param>
+        /// <param name="msg"></param>
+        public static void WriteLog(LogType logType, LogFile logFile, string msg)
+        {
+            try
+            {
+                logManager.WriteLog(logType, logFile, msg);
+            }
+            catch
+            {
+
+            }
+        }
+        /// <summary>
+        /// 文件类型为null
+        /// </summary>
+        /// <param name="logType"></param>
+        /// <param name="msg"></param>
+        public static void WriteLog(LogType logType, string msg)
+        {
+            try
+            {
+                logManager.WriteLog(logType, string.Empty, msg);
+            }
+            catch
+            {
+
+            }
+        }
+        /// <summary>
+        /// 数据库异常的标志位
+        /// 0表示update，1表示insert
+        /// </summary>
+        /// <param name="logType"></param>
+        /// <param name="status"></param>
+        /// <param name="msg"></param>
+        public static void WriteLog(LogType logType, string status, string msg)
+        {
+            try
+            {
+                logManager.WriteLog(logType, status, msg);
+            }
+            catch
+            {
+
+            }
+        }
+        /// <summary>
+        /// 读日志文件
+        /// </summary>
+        /// <param name="sqlStatus"></param>
+        /// <param name="sqlMsg"></param>
+        /// <param name="count"></param>      
+        public static List<string> ReadLog()
+        {
+            List<string> digitList = null;
+            try
+            {
+                digitList = logManager.ReadLog();
+            }
+            catch { }
+            return digitList;
+        }
+
+        public static void WriteCredence(string msg)
+        {
+            try
+            {
+                logManager.WriteCredence(msg);
+            }
+            catch { }
+        }
+
+        public static void WriteData(string name, string msg)
+        {
+            try
+            {
+                logManager.WriteData(name, msg);
+            }
+            catch { }
+        }
+    }
+}

instruction.txt

@@ -0,0 +1,27 @@
+sickTest使用简单，即插即用
+
+1.在解决方案资源管理器中将Test设为启动项。
+2.将计算机ip设为192.168.1.1
+3.运行。
+
+
+核心代码在Test->Form1.cs中
+对激光采集一帧数据的811个点，通过设置left_range和right_range,用点的序号设置显示点的范围；
+ground_truth_height：激光到"地面"的标准高度
+static int min_num_point_of_hinder :判定为障碍物的最少点的数目
+
+button1_Click():开始扫描
+在this.Invoke(new Action(()=>中进行循环采集
+解析好的811个点的x,y坐标分别存放在listx,listy中
+
+listy通过kalmanFilter()进行卡尔曼滤波
+dataFilter():对激光数据在相邻三个点之间去平均的方式进行滤波，还具备报警功能，如果滤波后依然有连续三帧数据
+存在“有大于min_num_point_of_hinder个点与ground_truth_height之间距离大于30mm”的情况，则报警，在界面
+的“障碍物”后面显示为True.
+
+InitChart()是界面显示部分
+this.chart1.ChartAreas[0].AxisX/Y.Minimum : 坐标轴最小值
+this.chart1.ChartAreas[0].AxisX/Y.Maximum : 坐标轴最大值
+this.chart1.ChartAreas[0].AxisX/Y.Interval ：网格大小
+this.chart1.Series[0].Points.AddXY：显示激光点
+

sickTest.sln

@@ -15,6 +15,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ChartTest", "ChartTest\Char
 EndProject
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "DataGathering", "DataGathering\DataGathering.csproj", "{1A3CF0DE-6841-4A05-A77B-D820571065F6}"
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "encTest", "encTest\encTest.csproj", "{BF838638-F53A-42E7-81CC-16B3C77C8898}"
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -45,6 +47,10 @@ Global
 		{1A3CF0DE-6841-4A05-A77B-D820571065F6}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{1A3CF0DE-6841-4A05-A77B-D820571065F6}.Release|Any CPU.ActiveCfg = Release|Any CPU
 		{1A3CF0DE-6841-4A05-A77B-D820571065F6}.Release|Any CPU.Build.0 = Release|Any CPU
+		{BF838638-F53A-42E7-81CC-16B3C77C8898}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{BF838638-F53A-42E7-81CC-16B3C77C8898}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{BF838638-F53A-42E7-81CC-16B3C77C8898}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{BF838638-F53A-42E7-81CC-16B3C77C8898}.Release|Any CPU.Build.0 = Release|Any CPU
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE

sickTest/Form1.cs

@@ -12,6 +12,7 @@ namespace sickTest
     public partial class Form1 : Form
     {
         private TcpClient tcpClient;
+        private UdpClient udpClient;
         NetworkStream stream;
         private int bufferSize = 0x8000;
         private string startCmd = "02 73 45 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 31 03";
@@ -21,7 +22,7 @@ namespace sickTest
         {
             InitializeComponent();
             tcpClient = new TcpClient();
-            tcpClient.Connect(IPAddress.Parse("192.168.1.31"), 2111);
+            tcpClient.Connect(IPAddress.Parse("192.168.1.30"), 2112);
             Console.WriteLine("连接成功");
             stream = tcpClient.GetStream();
         }
@@ -167,4 +168,4 @@ namespace sickTest
             return buff;
         }
     }
-}
+}