public void LaserStart()
{
Laser.Init();
#region vars
int temp_x = 0;
#endregion
Thread laser_thrd = new Thread(new ThreadStart(LaserThread));
laser_thrd.Start();
void LaserThread()
{
while (true)
{
Dispatcher.CurrentDispatcher.Invoke(() => {
Laser.GetProfile(out double[] X, out double[] Z);
List <LPoint> data = Helper.GetLaserData(X, Z, true);
LPoint res = LVoronej.Type1_1point(data);
RPoint findPoint = Transform.Trans(new RPoint(temp_x, 0, 0, 0, 0, 0), res);
if (Viewer2D)
{
_LV.SetData(data);
_LV.SetPoint(res);
}
if (Map2D)
{
_map2d.AddLaserPoint(findPoint);
}
if (Map3D)
{
_map3d.AddPoint(findPoint.ToDoubleMas());
}
});
Thread.Sleep(36);
}
void InvokerFun()
{
}
}
}
public void Program1()
{
Laser.Init();
RPoint send_point = new RPoint();
bool isFirst = true;
bool start = false;
bool finish = false;
//LPoint lp = new LPoint(-11.475, 217.6);
//RPoint rp = new RPoint(699.44, -66.74, 247.03, -1.99, -1.12, -27.76);
//var tr = Transform.Trans(rp, lp);
//Console.WriteLine(tr.ToString());
List <RPoint> line1 = new List <RPoint>();
List <RPoint> line2 = new List <RPoint>();
LViewer aprx_v = new LViewer("Approx");
aprx_v.Show();
List <LPoint> aprox_points = new List <LPoint>();
List <RPoint> seam_points = new List <RPoint>();
List <RPoint> seam_points2 = new List <RPoint>();
List <List <LPoint> > lines = new List <List <LPoint> >();
List <RPoint> show = new List <RPoint>();
//RPoint s1 = new RPoint(732.26, -226.4, 368.73);
//RPoint s2 = new RPoint(901.1, -245.6, 369);
//RPoint f1 = new RPoint(727.0475, -262.422, 370.675);
//RPoint f2 = new RPoint(894.9, -260.64, 366.07);
RPoint s1 = new RPoint(700, -100, 300);
RPoint s2 = new RPoint(800, -130, 320);
RPoint f1 = new RPoint(600, -100, 300);
RPoint f2 = new RPoint(700, -115, 310);
var pp = RobotCalculate.BaseRotation(s1, s2, f1, f2);
Console.WriteLine($"Rotation = {pp.ToString()}");
_map3d.AddPoint(s1.ToDoubleMas());
_map3d.AddPoint(s2.ToDoubleMas());
_map3d.AddPoint(f1.ToDoubleMas(), 1);
_map3d.AddPoint(f2.ToDoubleMas(), 1);
//Thread thrd = new Thread(new ThreadStart(t_work)
//Thread thrd = new Thread(new ThreadStart(LaserApprox));
Thread thrd = new Thread(new ThreadStart(test_1));
thrd.Start();
void t_work()
{
while (true)
{
Dispatcher.Invoke(InvokerFun);
Thread.Sleep(100);
}
void InvokerFun()
{
tb_position.Text = s.Position.ToString();
tb_recive_point.Text = s.recive_p.ToString();
// Console.WriteLine(s.work);
tb_work.Text = s.rec.ToString();
tb_send_point.Text = send_point.ToString();
tb_strRecive.Text = s.StrRecive;
Laser.GetProfile(out double[] X, out double[] Z);
List <LPoint> data = Helper.GetLaserData(X, Z, true);
if (data.Count != 0)
{
var tee = LFilters.GetLineCount(data, minLineLenght: 50, diff: 100);
_LV.SetData(data);
if (tee == 2)
{
// Console.WriteLine(tee);
// Console.Beep(1000, 100);
}
}
LPoint res = new LPoint(0, 0);
if (data.Count != 0 && test)
{
_LV.SetData(data);
if (LFilters.GetLineCount(data, minLineLenght: 50, diff: 100) > 1)
{
res = LVoronej.Type1_1point(data);
RPoint findPoint = Transform.Trans(s.Position, res);
tb_cur_point.Text = findPoint.ToString();
_LV.SetPoint(res);
_map2d.AddLaserPoint(findPoint);
_map3d.AddPoint(findPoint.ToDoubleMas(), 1);
//for (int i = 0; i < data.Count; i++) {
// var t1 = Transform.Trans(s.Position, data[i]);
// _map3d.AddPoint(t1.ToDoubleMas());
//}
if (l1)
{
line1.Add(findPoint);
}
else if (l2)
{
line2.Add(findPoint);
}
//if (cal) {
// send_point = RobotCalculate.CalculatePoint_2line(line1, line2);
// _map3d.AddPoint(send_point.ToDoubleMas(),2);
// cal = false;
//}
}
//if (Viewer2D) {
// _LV.SetData(data);
//}
//if (Map2D) {
// _map2d.AddLaserPoint(findPoint);
//}
//if (Map3D) {
// _map3d.AddPoint(findPoint.ToDoubleMas());
//}
}
if (cal)
{
send_point = RobotCalculate.CalculatePoint_2line(line1, line2);
_map3d.AddPoint(send_point.ToDoubleMas(), 2);
cal = false;
}
//if (data.Count != 0) {
// Console.WriteLine(LFilters.GetLineCount(data, minLineLenght: 50, diff: 40));
// if (s.work) {
// start = true;
// if (isFirst) {
// line1.Add(findPoint);
// } else {
// line2.Add(findPoint);
// }
// }
//} else {
// if (start) {
// isFirst = false;
// finish = true;
// }
// if (finish) {
// // var t = RobotCalculate.CalculatePoint_2line(line1, line2);
// // send_point = t;
// }
//}
#region
//if (s.work) {
// start = true;
// if (data.Count != 0) {
// RPoint findPoint = Transform.Trans(s.Position, res);
// tb_cur_point.Text = findPoint.ToString();
// if (Viewer2D) {
// _LV.SetData(data);
// _LV.SetPoint(res);
// }
// if (Map2D) {
// _map2d.AddLaserPoint(findPoint);
// }
// if (Map3D) {
// _map3d.AddPoint(findPoint.ToDoubleMas());
// }
// if (isFirst) {
// line1.Add(findPoint);
// } else {
// line2.Add(findPoint);
// }
// }
//} else {
// if (start) {
// isFirst = false;
// finish = true;
// }
// if (finish) {
// // var t = RobotCalculate.CalculatePoint_2line(line1, line2);
// // send_point = t;
// }
//}
#endregion
//Dispatcher.Invoke(() => {
//});
}
}
void LaserApprox()
{
var temp_point = Point_X(new LPoint(-25, 191.55), new LPoint(-10, 201.77), new LPoint(10, 167.04), new LPoint(25, 146.053));
// Console.WriteLine($"X = {temp_point.X} | Z = {temp_point.Z}");
while (true)
{
Dispatcher.Invoke(InvokerFun);
Thread.Sleep(500);
}
void InvokerFun()
{
Laser.GetProfile(out double[] X, out double[] Z);
List <LPoint> data = Helper.GetLaserData(X, Z, true);
data = LFilters.AveragingVerticalPro(data, _everyPoint: true);
tb_strRecive.Text = s.StrRecive;
if (data.Count != 0 && test)
{
aprox_points = new List <LPoint>();
lines = new List <List <LPoint> >();
_LV.SetData(data);
int line_count = LFilters.GetLineCount(data, minLineLenght: data.Count / 6, diff: 70);
if (line_count > 0)
{
//var aprx_mas = LFilters.GetLines(data, minLineLenght: 50, diff: 100);
//_LV.SetPoints(aprx_mas);
// Console.WriteLine(line_count);
var lp = LVoronej.Type1_1point(data);
var t23 = Transform.Trans(s.Position, lp);
if (seam_points2.Count > 0)
{
double xDif = 5;
double yDif = 1;
double zDif = 1;
double x1 = seam_points2[seam_points2.Count - 1].X;
double y1 = seam_points2[seam_points2.Count - 1].Y;
double z1 = seam_points2[seam_points2.Count - 1].Z;
double x2 = t23.X;
double y2 = t23.Y;
double z2 = t23.Z;
if (Math.Abs(x1 - x2) < xDif)
{
if (Math.Abs(y1 - y2) < yDif)
{
if (Math.Abs(z1 - z2) < zDif)
{
seam_points2.Add(t23);
_map3d.AddPoint(t23.ToDoubleMas(), 1);
}
}
}
}
else
{
seam_points2.Add(t23);
_map3d.AddPoint(t23.ToDoubleMas(), 1);
}
List <LPoint> data_aprx = new List <LPoint>();
var aprx_mas = LFilters.GetLinesByIndexes(data, minLineLenght: data.Count / 6, diff: 70);
for (int i = 0; i < aprx_mas.Count; i += 2)
{
int start_indx = aprx_mas[i];
int finish_indx = aprx_mas[i + 1];
List <LPoint> l1 = new List <LPoint>();
for (int j = start_indx; j <= finish_indx; j++)
{
l1.Add(data[j]);
}
l1 = LFilters.LineAprox(l1);
lines.Add(l1);
foreach (var item in l1)
{
data_aprx.Add(item);
}
//if (i >= 2) {
// aprox_points.Add(Point_X(data[aprx_mas[i - 2]], data[aprx_mas[i - 1]], data[aprx_mas[i]], data[aprx_mas[i + 1]]));
//}
}
aprx_v.SetData(data_aprx);
// aprx_v.SetPoints(aprox_points);
//for (int i = 0; i < data_aprx.Count; i++) {
// var t1 = Transform.Trans(s.Position, data_aprx[i]);
// _map3d.AddPoint(t1.ToDoubleMas());
//}
for (int i = 1; i < lines.Count; i++)
{
var temp_res_point = Point_X(lines[i - 1][0], lines[i - 1][lines[i - 1].Count - 1], lines[i][0], lines[i][lines[i].Count - 1]);
//if (temp_res_point.Z != 0 && DistanceBetweenPoint(temp_res_point, lines[i - 1][lines[i - 1].Count - 1]) < 5 &&
// DistanceBetweenPoint(temp_res_point, lines[i][0]) < 5) {
// aprox_points.Add(temp_res_point);
//}
if (temp_res_point.Z != 0)
{
aprox_points.Add(temp_res_point);
}
}
aprx_v.SetPoints(aprox_points);
//for (int i = 0; i < aprox_points.Count; i++) {
// var t1 = Transform.Trans(s.Position, aprox_points[i]);
// _map3d.AddPoint(t1.ToDoubleMas(), 2);
//}
if (aprox_points.Count > 0)
{
double minZ = aprox_points.Min(t => t.Z);
LPoint minPoint = aprox_points.Single(t => t.Z == minZ);
var t2 = Transform.Trans(s.Position, minPoint);
#region С уравнением прямых TODO
//if (seam_points.Count > 5) {
// int start_index = 5;
// int finish_index = 5;
// bool add = true;
// while (finish_index < seam_points.Count) {
// if (RobotCalculate.OnLine(seam_points[])) {
// }
// }
//} else {
// seam_points.Add(t2);
// _map3d.AddPoint(t2.ToDoubleMas(), 2);
//}
#endregion
#region Каждая точка с предыдущей
if (seam_points.Count > 0)
{
double xDif = 5;
double yDif = 1;
double zDif = 1;
double x1 = seam_points[seam_points.Count - 1].X;
double y1 = seam_points[seam_points.Count - 1].Y;
double z1 = seam_points[seam_points.Count - 1].Z;
double x2 = t2.X;
double y2 = t2.Y;
double z2 = t2.Z;
if (Math.Abs(x1 - x2) < xDif)
{
if (Math.Abs(y1 - y2) < yDif)
{
if (Math.Abs(z1 - z2) < zDif)
{
seam_points.Add(t2);
_map3d.AddPoint(t2.ToDoubleMas(), 2);
}
}
}
}
else
{
seam_points.Add(t2);
_map3d.AddPoint(t2.ToDoubleMas(), 2);
}
#endregion
line1.Add(t2);
}
GC.Collect();
}
}
if (cal)
{
int type_line1 = RobotCalculate.LineType(seam_points);
seam_points = RobotCalculate.LineApprox(seam_points, type_line1);
foreach (var item in seam_points)
{
_map3d.AddPoint(item.ToDoubleMas(), 1);
}
cal = false;
}
}
void InvokerFunR()
{
tb_strRecive.Text = s.StrRecive;
}
}
void test_1()
{
RPoint r1 = new RPoint();
RPoint r2 = new RPoint();
RPoint r3 = new RPoint();
RPoint r4 = new RPoint();
RPoint res = new RPoint();
bool Top = false;
while (true)
{
Dispatcher.Invoke(InvokerFun);
Thread.Sleep(500);
}
void InvokerFun()
{
Laser.GetProfile(out double[] X, out double[] Z);
List <LPoint> data = Helper.GetLaserData(X, Z, true);
data = LFilters.AveragingVerticalPro(data, _everyPoint: true);
tb_strRecive.Text = s.StrRecive;
_LV.SetData(data);
Console.WriteLine(s.work);
if (data.Count != 0 && s.Position.X != double.MaxValue)
{
LPoint tempPoint = new LPoint();
//if (Top) {
// double maxZ = data.Max(t => t.Z);
// tempPoint = data.First(t => t.Z == maxZ);
// _LV.SetPoint(tempPoint);
//} else {
// double mixZ = data.Min(t => t.Z);
// tempPoint = data.First(t => t.Z == mixZ);
// _LV.SetPoint(tempPoint);
//}
double mixZ = data.Min(t => t.Z);
if (mixZ != double.NaN && mixZ > 0)
{
tempPoint = data.First(t => t.Z == mixZ);
_LV.SetPoint(tempPoint);
}
if (s.rec == 9)
{
var ttt = Transform.Trans(s.Position, tempPoint);
_map3d.AddPoint(ttt.ToDoubleMas(), 1);
show.Add(ttt);
s._MAP.Add(ttt);
}
//if (l1) {
// r1 = Transform.Trans(s.Position, tempPoint);
// _map3d.AddPoint(r1.ToDoubleMas(), 1);
// l1 = false;
//} else if (l2) {
// r2 = Transform.Trans(s.Position, tempPoint);
// _map3d.AddPoint(r2.ToDoubleMas(), 1);
// l2 = false;
//} else if (l3) {
// r3 = Transform.Trans(s.Position, tempPoint);
// _map3d.AddPoint(r3.ToDoubleMas(), 1);
// l3 = false;
//} else if (l4) {
// r4 = Transform.Trans(s.Position, tempPoint);
// _map3d.AddPoint(r4.ToDoubleMas(), 1);
// l4 = false;
//} else if (cal) {
// res = RobotCalculate.CalcPoint4(r1, r2, r3, r4);
// Console.WriteLine($"find point = {res.ToString()}");
// _map3d.AddPoint(res.ToDoubleMas(), 2);
// cal = false;
//}
//if (one) {
// var tem = Transform.Trans(s.Position, tempPoint);
// Console.WriteLine($"Trans point = {tem.ToString()}");
// _map3d.AddPoint(tem.ToDoubleMas(), 2);
// one = false;
//}
}
}
}
}