sick_obstacleDetection/Test/Form1.cs

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 Form1 : Form
    {
        private TcpClient tcpClient;
        NetworkStream stream;
        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;
        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)
                {
                    lock (Lock)
                    {
                        datalist.Clear();
                        flag = sendCmd(startCmd);
                        byte[] buffer = new byte[bufferSize];
                        stream.Read(buffer, 0, buffer.Length);
                        string str = HexStringToASCII(buffer);
                        datalist.Add(str);
                    }
                    Thread.Sleep(1);
                }
            });
            //string testimg = "";
            Task.Factory.StartNew(() =>
            {
                while (!isok)
                {
                    lock (Lock)
                    {
                        for (int i = 0; i < datalist.Count; i++)
                        {
                            string ss = validStr(datalist[i], "sRA LMDscandata 1 ", " not defined 0 0 0");
                            string[] data = ss.Split(' ');
                            string number = data[22];
                            int a = Convert.ToInt32(number, 16);
                            int[] validNum = new int[a];
                            string aws = "";
                            for (int j = 0; j < a; j++)
                            {
                                validNum[j] = Convert.ToInt32(data[23 + j], 16);
                                aws = aws + validNum[j] + " ";
                            }
                            validdatalist.Add(aws);

                            this.Invoke(new Action(() =>
                            {
                                System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
                                sw.Start();
                                aws.TrimEnd(' ');
                                List<double> listy = new List<double>();
                                List<double> listx = new List<double>();
                                string[] origindata = aws.Split(' ');
                                origindata = origindata.Where(s => !string.IsNullOrEmpty(s)).ToArray();
                                int[] origindataTransformed = Array.ConvertAll<string, int>(origindata, int.Parse);
                                int startDegree = (Convert.ToInt32(this.textBox1.Text) + 45) * 3;
                                int endDegree = (Convert.ToInt32(this.textBox2.Text) + 45) * 3 + 1;
                                for (int k = startDegree; k < endDegree; k++)
                                {
                                    double x = origindataTransformed[k] * Math.Cos((-45 + 0.333 * k) * Math.PI / 180);
                                    double y = origindataTransformed[k] * Math.Sin((-45 + 0.333 * k) * Math.PI / 180);
                                    listy.Add(y);
                                    listx.Add(x);
                                }
                                this.chart1.Series[0].Points.Clear();
                                //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);

                                sw.Stop();
                                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(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))
            {
                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;
            }
            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 validStr(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>
        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 = 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");
            this.chart1.Titles[0].Text = "雷达显示";
            this.chart1.Titles[0].ForeColor = Color.RoyalBlue;
            this.chart1.Titles[0].Font = new System.Drawing.Font("Microsoft Sans Serif", 12F);
            //设置图表显示样式
            this.chart1.Series[0].Color = Color.Red;
            this.chart1.Series[0].ChartType = SeriesChartType.Line;
            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], 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]);
            }
        }


        /// <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 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;
            //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;
            }
        }
    }
}