//запись маршрута public void ShowInfo2() { while (true) { Stopwatch sw = new Stopwatch(); sw.Start(); double[] X; // = new double[1]; double[] Z; // = new double[1]; Laser.GetProfile(out X, out Z); List <MyPoint> data = Calculate.ZeroZ(X, Z); MyPoint p = new MyPoint() { X = 0, Z = 0 }; if (data.Count != 0) { p = Calculate.FindPointWithAngle(data, 30, 5); } if (p.X != 0) { if (_R.Recive_data != "" && _R.Recive_data != null) { _R.RX = MyXML.GetValues(_R.Recive_data, "X"); _R.RY = MyXML.GetValues(_R.Recive_data, "Y"); _R.RZ = MyXML.GetValues(_R.Recive_data, "Z"); _R.RA = MyXML.GetValues(_R.Recive_data, "A"); _R.RB = MyXML.GetValues(_R.Recive_data, "B"); _R.RC = MyXML.GetValues(_R.Recive_data, "C"); //_Point pTrans = Transform.Trans(_R.RX, _R.RY, _R.RZ, _R.RA, _R.RB, _R.RC, 0, p.X, p.Z - 350); _Point pTrans = Transform.Trans(_R.RX, _R.RY, _R.RZ, _R.RA, _R.RB, _R.RC, 76.97, p.X + 7.51, p.Z - 350 - 5.87); Dispatcher.Invoke(() => map.LPoint(pTrans.X, pTrans.Y)); Dispatcher.Invoke(() => map.RPoint(_R.RX, _R.RY)); } //_Point pTrans = Transform.Trans(_R.RX, _R.RY, _R.RZ, _R.RA, _R.RB, _R.RC, 0, p.X, p.Z - 350); //if (pTrans.X != 0) { // Currection.WriteToFile("temp2.txt", pTrans); //} } //if (_R.Ipoc != null && Convert.ToInt64(_R.Ipoc) % 10 == 0) { // Console.Clear(); //} sw.Stop(); //Console.WriteLine(sw.ElapsedMilliseconds); } }
public static void WriteToFile(string path, _Point p, string ipoc) { try { using (StreamWriter sw = new StreamWriter(path, true, System.Text.Encoding.Default)) { //sw.WriteLine($"{ipoc}|{p.X}|{p.Y}|{p.Z}"); sw.WriteLine($"{ipoc}|{p.X}|{p.Y}|{p.Z}"); } } catch (Exception e) { Console.WriteLine(e.Message); } }
public static _Point CalcPoint(_Point p1, _Point p2) { double x1 = Math.Min(p1.X, p2.X); double x2 = Math.Max(p1.X, p2.X); double x = Convert.ToInt32(x1) - 1; double y1 = p1.X == x1 ? p1.Y : p2.Y; double y2 = p1.X == x2 ? p1.Y : p2.Y; double yRes = (((x - x1) * (y2 - y1)) / (x2 - x1)) + y1; double z1 = p1.X == x1 ? p1.Z : p2.Z; double z2 = p1.X == x2 ? p1.Z : p2.Z; double zRes = (((x - x1) * (z2 - z1)) / (x2 - x1)) + z1; return(new _Point() { X = x, Y = yRes, Z = zRes }); }
//абсолютная public void ShowInfo4() { while (true) { double[] X; // = new double[1]; double[] Z; // = new double[1]; Laser.GetProfile(out X, out Z); List <MyPoint> data = Calculate.ZeroZ(X, Z); MyPoint p = new MyPoint() { X = 0, Z = 0 }; if (data.Count != 0) { p = Calculate.FindPointWithAngle(data, 10, 5); } _Point pTrans = Transform.Trans(_R.RX, _R.RY, _R.RZ, _R.RA, _R.RB, _R.RC, 0, p.X, p.Z - 350); if (_R.Recive_data != "" && _R.Recive_data != null) { //double rx = MyXML.GetValues(_R.Recive_data, "X"); //double ry = MyXML.GetValues(_R.Recive_data, "Y"); //double rz = MyXML.GetValues(_R.Recive_data, "Z"); _R.RX = MyXML.GetValues(_R.Recive_data, "X"); _R.RY = MyXML.GetValues(_R.Recive_data, "Y"); _R.RZ = MyXML.GetValues(_R.Recive_data, "Z"); _R.RA = MyXML.GetValues(_R.Recive_data, "A"); _R.RB = MyXML.GetValues(_R.Recive_data, "B"); _R.RC = MyXML.GetValues(_R.Recive_data, "C"); _R.CurY = _R.RY; _R.CurZ = _R.RZ; _R.CurX = _R.RX; _R.CurA = _R.RA; _R.CurB = _R.RB; _R.CurC = _R.RC; if (prevIpoc != _R.Ipoc) { _INDX++; prevIpoc = _R.Ipoc; } if (_INDX >= 10) { _INDX = 0; int index = 0; while (_R.RX > CurData[index].X && index < CurData.Count) { index++; } _R.CurY = CurData[index].Y; _R.CurZ = CurData[index].Z; _R.CurX = CurData[index].X; } } } #region //_R.RX = MyXML.GetValues(_R.Recive_data, "X"); //_R.RY = MyXML.GetValues(_R.Recive_data, "Y"); //_R.RZ = MyXML.GetValues(_R.Recive_data, "Z"); //_R.RA = MyXML.GetValues(_R.Recive_data, "A"); //_R.RB = MyXML.GetValues(_R.Recive_data, "B"); //_R.RC = MyXML.GetValues(_R.Recive_data, "C"); //CurData.OrderBy(x => x.X); //if (_INDX < CurData.Count) { // if (_R.isCur == false) { // Console.WriteLine($"Index = {_INDX }"); // _R.CurY = CurData[_INDX].Y; // _R.CurZ = CurData[_INDX].Z; // _R.CurX = CurData[_INDX].X; // _R.CurA = _R.RA; // _R.CurB = _R.RB; // _R.CurC = _R.RC; // _R.isCur = true; // _INDX++; // } //} //if (_R.isCur == false) { // _R.CurY = _R.RY; // _R.CurZ = _R.RZ; // _R.CurX = _R.RX; // _R.CurA = _R.RA; // _R.CurB = _R.RB; // _R.CurC = _R.RC; // _R.isCur = true; //} #endregion }
//относительная public void ShowInfo3() { _Point tempPoint = new _Point(); bool yes = false; while (true) { double[] X; // = new double[1]; double[] Z; // = new double[1]; Laser.GetProfile(out X, out Z); List <MyPoint> data = Calculate.ZeroZ(X, Z); if (_R.Recive_data != "" && _R.Recive_data != null) { // Dispatcher.Invoke(() => tb_ReciveData.Text = _R.Recive_data); MyPoint p = new MyPoint() { X = 0, Z = 0 }; if (data.Count != 0) { p = Calculate.FindPointWithAngle(data, 10, 5); } _R.RX = MyXML.GetValues(_R.Recive_data, "X"); _R.RY = MyXML.GetValues(_R.Recive_data, "Y"); _R.RZ = MyXML.GetValues(_R.Recive_data, "Z"); _R.RA = MyXML.GetValues(_R.Recive_data, "A"); _R.RB = MyXML.GetValues(_R.Recive_data, "B"); _R.RC = MyXML.GetValues(_R.Recive_data, "C"); _Point pTrans = Transform.Trans(_R.RX, _R.RY, _R.RZ, _R.RA, _R.RB, _R.RC, 76.97, p.X + 7.51, p.Z - 350 - 5.87); //if (pTrans.X != 0) { // if (CurData.Count == 0) { // CurData.Add(new _Point() { // X = pTrans.X, // Y = pTrans.Y, // Z = pTrans.Z // }); // Dispatcher.Invoke(() => map.LPoint(pTrans.X, pTrans.Y)); // } else { // double yDif = 2; // разница по игреку меду соседними точками при записи траэктории // if (Math.Abs(pTrans.Y - CurData[CurData.Count - 1].Y) < yDif) { // //Console.WriteLine($"{Math.Abs(pTrans.Y - CurData[CurData.Count - 1].Y)}"); // CurData.Add(new _Point() { // X = pTrans.X, // Y = pTrans.Y, // Z = pTrans.Z // }); // Dispatcher.Invoke(() => map.LPoint(pTrans.X, pTrans.Y)); // } // } if (pTrans.X != 0) { if (!yes) { tempPoint = new _Point() { X = pTrans.X, Y = pTrans.Y, Z = pTrans.Z }; yes = !yes; } else { double yDif = 5; // разница по игреку меду соседними точками при записи траэктории if (Math.Abs(pTrans.Y - tempPoint.Y) < yDif && Math.Abs(pTrans.X - tempPoint.X) >= 1) { //Console.WriteLine($"{Math.Abs(pTrans.Y - CurData[CurData.Count - 1].Y)}"); _Point temp = Calculate.CalcPoint(tempPoint, pTrans); CurData.Add(temp); Dispatcher.Invoke(() => map.LPoint(temp.X, temp.Y)); tempPoint = pTrans; Console.WriteLine($"X = {temp.X} Y = {temp.Y} Z = {temp.Z} "); } } //Dispatcher.Invoke(() => map.LPoint(pTrans.X, pTrans.Y)); CurData.OrderBy(x => x.X); if (CurData.Count > 2) { CurData = Calculate.UsredMap(CurData); } } #region //double rx = MyXML.GetValues(_R.Recive_data, "X"); //double ry = MyXML.GetValues(_R.Recive_data, "Y"); //double rz = MyXML.GetValues(_R.Recive_data, "Z"); //Dispatcher.Invoke(() => map.RPoint(rx, ry)); //#region ////Console.WriteLine($"X = {rx} Y = {ry} Z = {rz} "); //int index = 0; ////for (int i = 0; i < CurData.Count; i++) { //// if (CurData[i].X > rx) { //// index = i; //// break; //// } ////} //double curY = 0; //double curZ = 0; ////шаг - максимальная корекция за один пакет //double step = 0.08; //if (CurData.Count > 0) { // while (index < CurData.Count && rx > CurData[index].X) { // index++; // } // if (index < CurData.Count) { // curY = CurData[index].Y - ry; // curZ = CurData[index].Z - rz; // } // // Console.WriteLine($"Index = {index}"); //} //if (Math.Abs(curY) > step) { // if (curY * -1 < 0) { // curY = step; // } else { // curY = -step; // } //} //if (Math.Abs(curZ) > step) { // if (curZ * -1 < 0) { // curZ = step/2; // } else { // curZ = -step/2; // } //} //_R.CurY = curY; //_R.CurZ = curZ; //// _R.CurX = 0.2; ////Console.WriteLine($"Y = {_R.CurY } Z = {_R.CurZ }"); //if (_INDX < 100) { // _R.CurX = 1; // _INDX++; //} //_R.isCur = true; //#endregion #endregion } } }