private void GoTopUp()
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface surface = Surface.GetInstance(parameterManager);
try
{
Vector3 cc = mc.GetPoint();
double Zp = surface.UpTop + 0.015;
mc.MoveTo(new Vector3(cc.X, cc.Y, Zp));
mc.Join();
}
catch (ArgumentOutOfRangeException)
{
MessageBox.Show("Cannot move to top surface of upperlayer ");
}
}
private void task()
{
try {
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
CoordManager cm = new CoordManager(parameterManager);
Vector3 InitPoint = mc.GetPoint();
int viewcounter = 0;
//string txtfileName = string.Format(@"{0}\{1}.txt",
// direcotryPath, System.DateTime.Now.ToString("yyyyMMdd_HHmmss_ffff"));
//StreamWriter twriter = File.CreateText(txtfileName);
//text読み込み
// List<pointscan> PSList = new List<pointscan>();
List <Point2d> PSList = new List <Point2d>();
var reader = new StreamReader(File.OpenRead(@"C:\affine_position.txt"));
//bool headerflag = true;
while (!reader.EndOfStream)
{
var line = reader.ReadLine();
string[] delimiter = { " " };
var values = line.Split(delimiter, StringSplitOptions.RemoveEmptyEntries);
Point2d p2 = new Point2d(
double.Parse(values[0]),
double.Parse(values[1])
);
PSList.Add(p2);
}
int pixthre = 500;
for (int pp = 0; pp < PSList.Count(); pp++)
{
viewcounter = 0;
Vector3 movepoint = new Vector3(PSList[pp].X, PSList[pp].Y, InitPoint.Z);
mc.MoveTo(movepoint);
//camera.Start();
//surfrecog(pixthre, 0.003);
//camera.Stop();
//double surfaceZup = mc.GetPoint().Z;
//movepoint.Z = surfaceZup;
//mc.MoveTo(movepoint);
mc.Join();
//SurfPoint = mc.GetPoint();
camera.Start();
led.AdjustLight(parameterManager);
camera.Stop();
Vector3 p = mc.GetPoint();
byte[] b = Ipt.CaptureMain();
Mat src = new Mat(440, 512, MatType.CV_8U, b);
String filename = String.Format(@"C:\img\grid\{0}_{1}_.png",
(p.X * 1000).ToString("0.0"),
(p.Y * 1000).ToString("0.0"));
Cv2.ImWrite(filename, src);
GC.Collect();
GC.WaitForPendingFinalizers();
GC.Collect();
}
camera.Start();
} catch (SystemException) {
}
}
private void task()
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
CoordManager cm = new CoordManager(parameterManager);
Vector3 InitPoint = mc.GetPoint();
List <Vector2> list_grid_pred = new List <Vector2>();
List <Vector2> list_grid_meas = new List <Vector2>();
List <Vector2> list_grid_part = new List <Vector2>();
List <Vector2> list_grid_meas2 = new List <Vector2>();
/*
* list_grid_pred.Add(new Vector2( 0.0, 0.0));
* list_grid_pred.Add(new Vector2(30.0, 0.0));
* list_grid_pred.Add(new Vector2( 0.0, 30.0));
* list_grid_pred.Add(new Vector2(30.0, 30.0));
*
* list_grid_pred.Add(new Vector2(60.0, 0.0));
* list_grid_pred.Add(new Vector2(0.0, 60.0));
* list_grid_pred.Add(new Vector2(60.0, 60.0));
*
*
*
*
* //list_grid_pred.Add(new Vector2(0.0, 10.0));
* //list_grid_pred.Add(new Vector2(10.0, 0.0));
* //list_grid_pred.Add(new Vector2(10.0, 10.0));
*
* //list_grid_pred.Add(new Vector2(20.0, 0.0));
* //list_grid_pred.Add(new Vector2(0.0, 20.0));
* //list_grid_pred.Add(new Vector2(20.0, 20.0));
*
* //list_grid_pred.Add(new Vector2(20.0, 10.0));
* //list_grid_pred.Add(new Vector2(30.0, 0.0));
* //list_grid_pred.Add(new Vector2(30.0, 10.0));
* //list_grid_pred.Add(new Vector2(10.0, 20.0));
* //list_grid_pred.Add(new Vector2(30.0, 20.0));
* //list_grid_pred.Add(new Vector2(10.0, 30.0));
* //list_grid_pred.Add(new Vector2(20.0, 30.0));
*
* camera.Start();
*
* Affine ap = new Affine(1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
* for (int i = 0; i < list_grid_pred.Count; i++) {
* Vector2 predpoint = ap.Trance(list_grid_pred[i]);
* //mc.MovePoint(predpoint.X, predpoint.Y, InitPoint.Z + 0.030);
* mc.MoveTo(new Vector3(predpoint.X, predpoint.Y, InitPoint.Z + 0.030), new Vector3(0, 0, 0), new Vector3(0, 0, 0));
* mc.Join();
*
* led.AdjustLight(parameterManager);
*
*
* bool flag = true;
* while (flag) {
* mc.MoveDistance(-0.003, VectorId.Z);
* mc.Join();
*
*
* byte[] b = camera.ArrayImage;
* Mat src = new Mat(440, 512, MatType.CV_8U, b);
* Mat mat = src.Clone();
*
* Cv2.GaussianBlur(mat, mat, Cv.Size(3, 3), -1);
* Mat gau = mat.Clone();
* Cv2.GaussianBlur(gau, gau, Cv.Size(7, 7), -1);
* Cv2.Subtract(gau, mat, mat);
* Cv2.Threshold(mat, mat, 4, 1, ThresholdType.Binary);
* int brightness = Cv2.CountNonZero(mat);
*
* if (brightness > 5000 ) flag = false;
* }
*
* Thread.Sleep(100);
*
* mc.SetSpiralCenterPoint();
* flag = true;
* int counter = 0;
* while (flag) {
* if(counter!=0){
* mc.MoveInSpiral(true);
* mc.Join();
* }
* counter++;
*
* Camera c = Camera.GetInstance();
* byte[] b = c.ArrayImage;
* Mat mat = new Mat(440, 512, MatType.CV_8U, b);
* Cv2.GaussianBlur(mat, mat, Cv.Size(7, 7), -1);
*
* Cv2.Threshold(mat, mat, 60, 255, ThresholdType.BinaryInv);
*
* Moments mom = new Moments(mat);
* if (mom.M00 < 1000 * 255) continue;
*
* double cx = mom.M10 / mom.M00;
* double cy = mom.M01 / mom.M00;
* Mat innercir = Mat.Zeros(440, 512, MatType.CV_8UC1);
* Cv2.Circle(innercir, new Point(cx, cy), 10, new Scalar(255, 255, 255), 3);
* int innerpath = Cv2.CountNonZero(innercir);
* Cv2.BitwiseAnd(innercir, mat, innercir);
* int innersum = Cv2.CountNonZero(innercir);
*
* Mat outercir = Mat.Zeros(440, 512, MatType.CV_8UC1);
* Cv2.Circle(outercir, new Point(cx, cy), 100, new Scalar(255, 255, 255), 3);
* int outerpath = Cv2.CountNonZero(outercir);
* Cv2.BitwiseAnd(outercir, mat, outercir);
* int outersum = Cv2.CountNonZero(outercir);
*
* double innerratio = innersum * 1.0 / innerpath * 1.0;
* double outerratio = outersum * 1.0 / outerpath * 1.0;
*
* if (innerratio < 0.8) continue;
* if (outerratio > 0.2) continue;
*
* flag = false;
*
*
* //
* Vector2 grid_meas = cm.TransToEmulsionCoord((int)(cx), (int)(cy));
*
* Vector3 to = new Vector3(grid_meas.X, grid_meas.Y, mc.GetPoint().Z);
* mc.MoveTo(to, new Vector3(0, 0, 0), new Vector3(0, 0, 0));
* mc.Join();
*
* b = c.ArrayImage;
* mat = new Mat(440, 512, MatType.CV_8U, b);
* Cv2.GaussianBlur(mat, mat, Cv.Size(7, 7), -1);
*
* Cv2.Threshold(mat, mat, 60, 255, ThresholdType.BinaryInv);
*
* mom = new Moments(mat);
* if (mom.M00 < 1000 * 255) continue;
*
* cx = mom.M10 / mom.M00;
* cy = mom.M01 / mom.M00;
* innercir = Mat.Zeros(440, 512, MatType.CV_8UC1);
* Cv2.Circle(innercir, new Point(cx, cy), 10, new Scalar(255, 255, 255), 3);
* innerpath = Cv2.CountNonZero(innercir);
* Cv2.BitwiseAnd(innercir, mat, innercir);
* innersum = Cv2.CountNonZero(innercir);
*
* outercir = Mat.Zeros(440, 512, MatType.CV_8UC1);
* Cv2.Circle(outercir, new Point(cx, cy), 100, new Scalar(255, 255, 255), 3);
* outerpath = Cv2.CountNonZero(outercir);
* Cv2.BitwiseAnd(outercir, mat, outercir);
* outersum = Cv2.CountNonZero(outercir);
*
* innerratio = innersum * 1.0 / innerpath * 1.0;
* outerratio = outersum * 1.0 / outerpath * 1.0;
*
* if (innerratio < 0.8) continue;
* if (outerratio > 0.2) continue;
*
* grid_meas = cm.TransToEmulsionCoord((int)(cx), (int)(cy));
* System.Diagnostics.Debug.WriteLine(string.Format("gridmark {0} {1}", grid_meas.X, grid_meas.Y));
*
*
* list_grid_meas.Add(grid_meas);
* list_grid_part.Add(list_grid_pred[i]);
* if (i >= 3) {
* ap = Affine.CreateAffineBy(list_grid_part, list_grid_meas);
* }
* }
*
*
* }//for(int i = 0; i < list_grid_pred.Count; i++)
*/
Affine ap = new Affine(1.00055550, -0.00152264, 0.00174182, 1.00096792, 12.7498, -62.5798);
string txtfileName = string.Format(@"c:\img\grid_g.txt");
StreamWriter twriter = File.CreateText(txtfileName);
for (int x = 0; x < 6; x++)
{
for (int y = 0; y < 6; y++)
{
//if (x == 0 && y == 5) continue;
//if (x == 1 && y == 5) continue;
Vector2 predpoint = ap.Trance(new Vector2(x * 10, y * 10));
mc.MoveTo(new Vector3(predpoint.X, predpoint.Y, InitPoint.Z + 0.030));
mc.Join();
led.AdjustLight(parameterManager);
bool flag = true;
while (flag)
{
mc.MoveDistance(-0.003, VectorId.Z);
mc.Join();
byte[] b = camera.ArrayImage;
Mat src = new Mat(440, 512, MatType.CV_8U, b);
Mat mat = src.Clone();
Cv2.GaussianBlur(mat, mat, Cv.Size(3, 3), -1);
Mat gau = mat.Clone();
Cv2.GaussianBlur(gau, gau, Cv.Size(7, 7), -1);
Cv2.Subtract(gau, mat, mat);
Cv2.Threshold(mat, mat, 4, 1, ThresholdType.Binary);
int brightness = Cv2.CountNonZero(mat);
if (brightness > 5000)
{
flag = false;
}
}
Thread.Sleep(100);
byte[] b2 = camera.ArrayImage;
Mat src2 = new Mat(440, 512, MatType.CV_8U, b2);
Mat mat2 = src2.Clone();
string FileName = string.Format(@"C:\img\x{0}_y{1}.bmp", x, y);
Cv2.ImWrite(FileName, mat2);
Cv2.GaussianBlur(mat2, mat2, Cv.Size(7, 7), -1);
Cv2.Threshold(mat2, mat2, 60, 255, ThresholdType.BinaryInv);
Moments mom2 = new Moments(mat2);
double cx2 = mom2.M10 / mom2.M00;
double cy2 = mom2.M01 / mom2.M00;
/* Mat innercir = Mat.Zeros(440, 512, MatType.CV_8UC1);
* Cv2.Circle(innercir, new Point(cx2, cy2), 10, new Scalar(255, 255, 255), 3);
* int innerpath = Cv2.CountNonZero(innercir);
* Cv2.BitwiseAnd(innercir, mat2, innercir);
* int innersum = Cv2.CountNonZero(innercir);
*
* Mat outercir = Mat.Zeros(440, 512, MatType.CV_8UC1);
* Cv2.Circle(outercir, new Point(cx2, cy2), 100, new Scalar(255, 255, 255), 3);
* int outerpath = Cv2.CountNonZero(outercir);
* Cv2.BitwiseAnd(outercir, mat2, outercir);
* int outersum = Cv2.CountNonZero(outercir);
*
* double innerratio = innersum * 1.0 / innerpath * 1.0;
* double outerratio = outersum * 1.0 / outerpath * 1.0;
*/
Vector2 grid_meas2 = cm.TransToEmulsionCoord((int)(cx2), (int)(cy2));
FileName = string.Format(@"C:\img\after_x{0}_y{1}.bmp", x, y);
Cv2.ImWrite(FileName, mat2);
string stlog = "";
stlog += String.Format("{0} {1} {2:f4} {3:f4} {4:f4} {5:f4}\n",
x * 10,
y * 10,
predpoint.X,
predpoint.Y,
grid_meas2.X,
grid_meas2.Y);
twriter.Write(stlog);
} //for y
} //for x
twriter.Close();
}
private void task()
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
CoordManager cm = new CoordManager(parameterManager);
Vector3 initialpoint = mc.GetPoint();
int pixthre = 500;
for (int bx = -5; bx <= 5; bx++)
{
for (int by = -5; by <= 5; by++)
{
string txtfileName = string.Format(@"E:\img\{0}_{1}.txt", bx, by);
StreamWriter twriter = File.CreateText(txtfileName);
Vector3 blockstartpoint = new Vector3();
blockstartpoint.X = initialpoint.X + bx * 1.0;
blockstartpoint.Y = initialpoint.Y + by * 1.0;
blockstartpoint.Z = initialpoint.Z;
mc.MoveTo(new Vector3(blockstartpoint.X + 0.5, blockstartpoint.Y + 0.5, initialpoint.Z - 0.020));
mc.Join();
int ledbrightness = led.AdjustLight(parameterManager);
camera.Start();
surfrecog(pixthre, 0.003);
camera.Stop();
double surfaceZup = mc.GetPoint().Z;
//上面 ベース中からはじめ、ベース上側を表面認識
//ベース上側からはじめてZ方向正の向きにスキャン
for (int vy = 0; vy < 10; vy++)
{
Vector3 linestartpoint = mc.GetPoint();
linestartpoint.X = blockstartpoint.X;
linestartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY;
linestartpoint.Z = surfaceZup;
for (int vx = 0; vx < 8;)
{
if (vx == 0)
{
Vector3 approachingpoint = mc.GetPoint();
approachingpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX - 0.05;
approachingpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY - 0.05;
approachingpoint.Z = linestartpoint.Z - 0.006;
mc.MoveTo(approachingpoint);
mc.Join();
}
Vector3 viewstartpoint = mc.GetPoint();
viewstartpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX;
viewstartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY;
viewstartpoint.Z = linestartpoint.Z - 0.006;
mc.MoveTo(viewstartpoint);
mc.Join();
Thread.Sleep(100);
Vector3 viewpoint = mc.GetPoint();
List <ImageTaking> lit = new List <ImageTaking>();
mc.Inch(PlusMinus.Plus, 0.15, VectorId.Z);
int viewcounter = 0;
while (viewcounter < 16 + 3)
{
byte[] b = Ipt.CaptureMain();
Vector3 p = mc.GetPoint();
if (viewcounter >= 3)
{
ImageTaking it = new ImageTaking(p, b);
string stlog = "";
stlog += String.Format("{0} {1} {2} {3}\n",
ledbrightness,
p.X,
p.Y,
p.Z);
twriter.Write(stlog);
}
viewcounter++;
}//view
viewcounter = 0;
double endz = mc.GetPoint().Z;
mc.SlowDownStop(VectorId.Z);
mc.Join();
if (endz - viewstartpoint.Z < 0.070)
{
tsparams tsp = new tsparams();
tsp.phthre = 10;
List <microtrack> lm = TrackSelector.Select(lit, tsp);
foreach (microtrack m in lm)
{
double viewx = viewpoint.X;
double viewy = viewpoint.Y;
double pixelx = 135.0 / 512.0;
double pixely = 115.0 / 440.0;
double x = viewx - (m.cx - 256) * pixelx;
double y = viewy + (m.cy - 220) * pixely;
Console.WriteLine(string.Format("{0:0.0} {1:0.0} {2:0.0} {3:0.0} {4:0.0} {5:0.0} {6:0.0} {7:0.0}", m.ph, m.pv, m.ax, m.ay, x, y, m.cx, m.cy));
}
vx++;
}
} //vx
} //vy
//下面 ベース中からはじめ、ベース下側を表面認識
//ベース下側からはじめてZ方向負の向きにスキャン
mc.MoveTo(new Vector3(blockstartpoint.X + 0.5, blockstartpoint.Y + 0.5, initialpoint.Z - 0.140));
mc.Join();
camera.Start();
surfrecog(pixthre, -0.003);
camera.Stop();
double surfaceZdown = mc.GetPoint().Z;
for (int vy = 0; vy < 10; vy++)
{
Vector3 linestartpoint = mc.GetPoint();
linestartpoint.X = blockstartpoint.X;
linestartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY;
linestartpoint.Z = surfaceZdown;
for (int vx = 0; vx < 8;)
{
if (vx == 0)
{
Vector3 approachingpoint = mc.GetPoint();
approachingpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX - 0.05;
approachingpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY - 0.05;
approachingpoint.Z = linestartpoint.Z + 0.006;
mc.MoveTo(approachingpoint);
mc.Join();
}
Vector3 viewstartpoint = mc.GetPoint();
viewstartpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX;
viewstartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY;
viewstartpoint.Z = linestartpoint.Z + 0.006;
mc.MoveTo(viewstartpoint);
mc.Join();
Thread.Sleep(100);
Vector3 viewpoint = mc.GetPoint();
byte[] bb = new byte[440 * 512 * 16];
string datfileName = string.Format(@"E:\img\d_{0}_{1}_{2}_{3}.dat",
(int)(viewpoint.X * 1000),
(int)(viewpoint.Y * 1000),
vx,
vy
);
BinaryWriter writer = new BinaryWriter(File.Open(datfileName, FileMode.Create));
mc.Inch(PlusMinus.Minus, 0.15, VectorId.Z);
int viewcounter = 0;
while (viewcounter < 16 + 3)
{
byte[] b = Ipt.CaptureMain();
Vector3 p = mc.GetPoint();
if (viewcounter >= 3)
{
b.CopyTo(bb, 440 * 512 * (viewcounter - 3));
string stlog = "";
stlog += String.Format("{0} {1} {2} {3}\n",
ledbrightness,
p.X,
p.Y,
p.Z);
twriter.Write(stlog);
}
viewcounter++;
}//view
viewcounter = 0;
double endz = mc.GetPoint().Z;
mc.SlowDownStop(VectorId.Z);
mc.Join();
Thread.Sleep(100);
if (viewstartpoint.Z - endz < 0.070)
{
vx++;
writer.Write(bb);
writer.Flush();
writer.Close();
}
} //vx
} //vy
camera.Stop();
twriter.Close();
} //blocky
} //blockx
} //task
private void task()
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
CoordManager cm = new CoordManager(parameterManager);
Vector3 initialpoint = mc.GetPoint();
int pixthre = 500;
for (int bx = -1; bx <= 1; bx++)
{
for (int by = -1; by <= 1; by++)
{
string txtfileName = string.Format(@"E:\img\{0}_{1}.txt", bx, by);
StreamWriter twriter = File.CreateText(txtfileName);
Vector3 blockstartpoint = new Vector3();
blockstartpoint.X = initialpoint.X + bx * 1.0;
blockstartpoint.Y = initialpoint.Y + by * 1.0;
blockstartpoint.Z = initialpoint.Z;
mc.MoveTo(new Vector3(blockstartpoint.X + 0.5, blockstartpoint.Y + 0.5, initialpoint.Z));
mc.Join();
camera.Start();
int ledbrightness = led.AdjustLight(parameterManager);
camera.Stop();
//初期位置からはじめてZ方向正の向きにスキャン
for (int vy = 0; vy < 10; vy++)
{
Vector3 linestartpoint = mc.GetPoint();
linestartpoint.X = blockstartpoint.X;
linestartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY;
linestartpoint.Z = initialpoint.Z;
for (int vx = 0; vx < 8;)
{
if (vx == 0)
{
Vector3 approachingpoint = mc.GetPoint();
approachingpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX - 0.05;
approachingpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY - 0.05;
approachingpoint.Z = linestartpoint.Z;
mc.MoveTo(approachingpoint);
mc.Join();
}
Vector3 viewstartpoint = mc.GetPoint();
viewstartpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX;
viewstartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY;
viewstartpoint.Z = linestartpoint.Z;
mc.MoveTo(viewstartpoint);
mc.Join();
Thread.Sleep(100);
Vector3 viewpoint = mc.GetPoint();
int npict = 300;
byte[] bb = new byte[440 * 512 * npict];
string datfileName = string.Format(@"E:\img\u_{0}_{1}_{2}_{3}.dat",
(int)(viewpoint.X * 1000),
(int)(viewpoint.Y * 1000),
vx,
vy
);
BinaryWriter writer = new BinaryWriter(File.Open(datfileName, FileMode.Create));
mc.Inch(PlusMinus.Plus, 0.15 / 12, VectorId.Z);
int viewcounter = 0;
while (viewcounter < npict + 3)
{
byte[] b = Ipt.CaptureMain();
Vector3 p = mc.GetPoint();
if (viewcounter >= 3)
{
b.CopyTo(bb, 440 * 512 * (viewcounter - 3));
string stlog = "";
stlog += String.Format("{0} {1} {2} {3}\n",
ledbrightness,
p.X,
p.Y,
p.Z);
twriter.Write(stlog);
}
viewcounter++;
}//view
viewcounter = 0;
double endz = mc.GetPoint().Z;
mc.SlowDownStop(VectorId.Z);
mc.Join();
Thread.Sleep(100);
if (endz - viewstartpoint.Z < 0.15)
{
vx++;
writer.Write(bb);
writer.Flush();
writer.Close();
}
} //vx
} //vy
camera.Stop();
twriter.Close();
} //blocky
} //blockx
} //task
}//BeamDetection
private void task()
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
TracksManager tm = parameterManager.TracksManager;
Track myTrack = tm.GetTrack(tm.TrackingIndex);
Surface surface = Surface.GetInstance(parameterManager);
Stopwatch sw = new Stopwatch();
CoordManager coordManager = new CoordManager(parameterManager);
//CoordManager coordManager;
int mod = parameterManager.ModuleNo;
int pl = parameterManager.PlateNo;
sw.Start();
try
{
//MotorControler mc = MotorControler.GetInstance(parameterManager);
GridMark nearestMark = coordManager.GetTheNearestGridMark(mc.GetPoint());
System.Diagnostics.Debug.WriteLine(String.Format("{0}, {1}", nearestMark.x, nearestMark.y));
mc.MovePointXY(nearestMark.x, nearestMark.y);
mc.Join();
}
catch (GridMarkNotFoundException ex)
{
System.Diagnostics.Debug.WriteLine(String.Format("{0}", ex.ToString()));
}
try
{
// MotorControler mc = MotorControler.GetInstance(parameterManager);
IGridMarkRecognizer GridMarkRecognizer = coordManager;
mc.SetSpiralCenterPoint();
Led led = Led.GetInstance();
Vector2 encoderPoint = new Vector2(-1, -1);
encoderPoint.X = mc.GetPoint().X;
encoderPoint.Y = mc.GetPoint().Y;//おこられたのでしかたなくこうする 吉田20150427
Vector2 viewerPoint = new Vector2(-1, -1);
bool continueFlag = true;
while (continueFlag)
{
led.AdjustLight(parameterManager);
viewerPoint = GridMarkRecognizer.SearchGridMarkx50();
if (viewerPoint.X < 0 || viewerPoint.Y < 0)
{
System.Diagnostics.Debug.WriteLine(String.Format("grid mark not found"));
mc.MoveInSpiral(true);
mc.Join();
continueFlag = (mc.SpiralIndex < 30);
}
else
{
System.Diagnostics.Debug.WriteLine(String.Format("******** {0} {1}", viewerPoint.X, viewerPoint.Y));
encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
mc.MovePointXY(encoderPoint);
mc.Join();
continueFlag = false;
}
} // while
//重心検出と移動を2回繰り返して、グリッドマークを視野中心にもっていく
mc.MovePointXY(encoderPoint);
mc.Join();
viewerPoint = GridMarkRecognizer.SearchGridMarkx50();
encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
mc.MovePointXY(encoderPoint);
mc.Join();
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine("exception");
}
//.........Get value of HFDX and HFDY................//
try
{
Vector3 CurrentCenterPoint = mc.GetPoint();
GridMark nearestMark = coordManager.GetTheNearestGridMark(CurrentCenterPoint);
System.Diagnostics.Debug.WriteLine(String.Format("{0}, {1}", CurrentCenterPoint.X, CurrentCenterPoint.Y));
coordManager.HFDX = CurrentCenterPoint.X - nearestMark.x;
coordManager.HFDY = CurrentCenterPoint.Y - nearestMark.y;
}
catch (EntryPointNotFoundException ex)
{
MessageBox.Show("エントリポイントが見当たりません。 " + ex.Message);
System.Diagnostics.Debug.WriteLine("エントリポイントが見当たりません。 " + ex.Message);
}
//....... Move to track place after revised with shift value.....//
double dstx = myTrack.MsX + coordManager.HFDX;
double dsty = myTrack.MsY + coordManager.HFDY;
mc.MovePointXY(dstx, dsty, delegate {
//stage.WriteLine(Properties.Strings.MovingComplete);
});
mc.Join();
Led led_ = Led.GetInstance();
led_.AdjustLight(parameterManager);
Thread.Sleep(500); //Wait for 5s
//////////////////////////////Surfacerecog/////////////////////////////////////.................................../////////
try
{
if (mc.IsMoving)
{
MessageBoxResult r = MessageBox.Show(
Properties.Strings.SurfaceException01,
Properties.Strings.Abort + "?",
MessageBoxButton.YesNo);
if (r == MessageBoxResult.Yes)
{
mc.AbortMoving();
}
else
{
return;
}
}
//Surface surface = Surface.GetInstance(parameterManager);
if (surface.IsActive)
{
MessageBoxResult r = MessageBox.Show(
Properties.Strings.SurfaceException02,
Properties.Strings.Abort + "?",
MessageBoxButton.YesNo);
if (r == MessageBoxResult.Yes)
{
surface.Abort();
}
else
{
return;
}
}
try
{
surface.Start(true);
}
catch (Exception ex)
{
MessageBox.Show(ex.Message, Properties.Strings.Error);
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message, Properties.Strings.Error);
}
mc.Join();
//...............Beam Pattern Matching..................////////////////
try
{
string datarootdirpath = string.Format(@"C:\test\bpm\{0}", mod);
System.IO.DirectoryInfo mydir = System.IO.Directory.CreateDirectory(datarootdirpath);
string[] sp = myTrack.IdString.Split('-');
string uptxt = string.Format(@"c:\test\bpm\{0}\{1}-{2}-{3}-{4}_up.txt", mod, mod, pl, sp[0], sp[1]);
string dwtxt = string.Format(@"c:\test\bpm\{0}\{1}-{2}-{3}-{4}_dw.txt", mod, mod, pl - 1, sp[0], sp[1]);
BeamDetection(uptxt, true);
BeamPatternMatch bpm = new BeamPatternMatch(8, 200);
bpm.ReadTrackDataTxtFile(dwtxt, false);
bpm.ReadTrackDataTxtFile(uptxt, true);
bpm.DoPatternMatch();
//stage.WriteLine(String.Format("pattern match dx,dy = {0}, {1}", bpm.GetPeakX() * 0.2625 * 0.001, bpm.GetPeakY() * 0.2625 * 0.001));
Vector3 BfPoint = mc.GetPoint();
mc.MoveDistance(bpm.GetPeakX() * 0.2625 * 0.001, VectorId.X);
mc.Join();
mc.MoveDistance(-bpm.GetPeakY() * 0.2625 * 0.001, VectorId.Y);
mc.Join();
Led led = Led.GetInstance();
led.AdjustLight(parameterManager);
Vector3 AfPoint = mc.GetPoint();
//stage.WriteLine(String.Format("Move dx,dy = {0}, {1}", BfPoint.X - AfPoint.X, BfPoint.Y - AfPoint.Y));
}
catch (ArgumentOutOfRangeException)
{
MessageBox.Show("ID numver is not existed。 ");
}
catch (System.Exception)
{
MessageBox.Show("No beam battern。 ");
}
// }
/////////////////////////////SurfaceRecog////////////////////////////////////////////////////////////////////////////
Vector3 initial = mc.GetPoint();///////initialpoint
double Sh = 0.5 / (surface.UpTop - surface.UpBottom);
//ここから角度によって撮影間隔を変更するように書き換える。
double tansi = Math.Sqrt(myTrack.MsDX * myTrack.MsDX + myTrack.MsDY * myTrack.MsDY);
double theta = Math.Atan(tansi);
//絶対値の大きさを入れる。dzはマイナスの値になるようにする。
double dz;
double dz_price_img = (6 * Math.Cos(theta) / Sh) / 1000;
double dz_img = dz_price_img * (-1);
//
string datarootdirpathw = string.Format(@"C:\test\{0}", myTrack.IdString);
System.IO.DirectoryInfo mydir_ = System.IO.Directory.CreateDirectory(datarootdirpathw);
//
//必要なlistをまとめる
//List<ImageTaking> LiITUpTrack = new List<ImageTaking>();
List <OpenCvSharp.CPlusPlus.Point2d> Msdxdy = new List <OpenCvSharp.CPlusPlus.Point2d>();
Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(myTrack.MsDX, myTrack.MsDY));
List <OpenCvSharp.CPlusPlus.Point3d> LStage = new List <OpenCvSharp.CPlusPlus.Point3d>();
List <OpenCvSharp.CPlusPlus.Point> LPeak = new List <OpenCvSharp.CPlusPlus.Point>(); //i番目の画像で実際に見つかったトラックの座標。pixel座標で視野中心からの差分。
List <Point3d> LTrack = new List <Point3d>(); //i番目の画像で実際に見つかったトラックの座標のステージ座標
List <List <ImageTaking> > UpTrackInfo = new List <List <ImageTaking> >();
//エラー防止のために下ゲルの処理の際に必要なListをここに移動した。
double Sh_low;
Sh_low = 0.5 / (surface.LowTop - surface.LowBottom);
List <OpenCvSharp.CPlusPlus.Point2d> Msdxdy_Low = new List <OpenCvSharp.CPlusPlus.Point2d>();
//List<ImageTaking> LiITLowMid = new List<ImageTaking>();
List <OpenCvSharp.CPlusPlus.Point3d> LStage_Low = new List <OpenCvSharp.CPlusPlus.Point3d>();
List <OpenCvSharp.CPlusPlus.Point> LPeak_Low = new List <OpenCvSharp.CPlusPlus.Point>(); //i番目の画像で実際に見つかったトラックの座標。pixel座標で視野中心からの差分。
List <Point3d> LTrack_Low = new List <Point3d>(); //i番目の画像で実際に見つかったトラックの座標のステージ座標
List <List <ImageTaking> > LowTrackInfo = new List <List <ImageTaking> >();
//今までのtrack追跡プログラムとは異なる角度等の使い方をする。
dz_price_img = (6 * Math.Cos(theta) / Sh) / 1000;
dz_img = dz_price_img * (-1);
dz = dz_img;
int gotobase = 0;
int not_detect = 0;
for (int i = 0; gotobase < 1; i++)
{
///////移動して画像処理をしたときに、baseの中に入らないようにする。
Vector3 initialpos = mc.GetPoint();
double moverange = 7 * dz_img;
double predpoint = moverange + initialpos.Z;
if (predpoint < surface.UpBottom)//もしもbaseに入りそうなら、8枚目の画像がちょうどbaseを撮影するようにdzを調整する。
{
gotobase = 1;
dz = surface.UpBottom - initialpos.Z + 7 * dz_price_img;
}
////////
//gotobase = 1のときは、移動して画像を撮影するようにする。
if (i != 0)
{
Vector3 dstpoint = new Vector3(
LTrack[i - 1].X + Msdxdy[i].X * dz * Sh,
LTrack[i - 1].Y + Msdxdy[i].Y * dz * Sh,
LTrack[i - 1].Z + dz
);
mc.MovePoint(dstpoint);
mc.Join();
}
List <ImageTaking> LiITUpMid = TakeSequentialImage(
Msdxdy[i].X * Sh,
Msdxdy[i].Y * Sh,
dz_img,
8);
////画像の保存、座標の保存。
LStage.Add(new OpenCvSharp.CPlusPlus.Point3d(LiITUpMid[7].StageCoord.X, LiITUpMid[7].StageCoord.Y, LiITUpMid[7].StageCoord.Z));
LiITUpMid[7].img.ImWrite(datarootdirpathw + string.Format(@"\img_l_up_{0}.bmp", i));
UpTrackInfo.Add(LiITUpMid);//撮影した8枚の画像と、撮影した位置を記録する。
//撮影した画像をここで処理する。
List <Mat> binimages = new List <Mat>();
for (int t = 0; t <= 7; t++)
{
Mat bin = (Mat)DogContrastBinalize(LiITUpMid[t].img);
double xx = myTrack.MsDX * myTrack.MsDX;
double yy = myTrack.MsDY * myTrack.MsDY;
if (Math.Sqrt(xx + yy) >= 0.4)
{
Cv2.Dilate(bin, bin, new Mat());
}
Cv2.Dilate(bin, bin, new Mat());
binimages.Add(bin);
}
//trackを重ねる処理を入れる。
Point2d pixel_cen = TrackDetection(binimages, 256, 220, 3, 3, 4, 90, 3);//画像の8枚目におけるtrackのpixel座標を算出する。
if (pixel_cen.X == -1 & pixel_cen.Y == -1)
{
//追跡に失敗した時に最後に検出したtrack座標に移動してから、追跡に失敗した地点の画像を撮影するようにする。
//mc.MovePoint(LTrack[i - 1].X, LTrack[i - 1].Y, LTrack[i - 1].Z);
mc.Join();
not_detect = 1;
goto not_detect_track;
}
//検出したpixel座標をstage座標に変換するなどlistに追加する。
LPeak.Add(new OpenCvSharp.CPlusPlus.Point(pixel_cen.X - 256, pixel_cen.Y - 220));
double firstx = LStage[i].X - LPeak[i].X * 0.000267;
double firsty = LStage[i].Y + LPeak[i].Y * 0.000267;
double firstz = LStage[i].Z;
LTrack.Add(new Point3d(firstx, firsty, firstz));
//
//上側乳剤層上面の1回目のtrack検出によって、次のtrackの位置を検出する角度を求める。
//その角度が、1回目のtrack検出の結果によって大きな角度にならないように調整をする。
if (i == 0)
{
Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(myTrack.MsDX, myTrack.MsDY));
}
else if (i == 1)
{
List <Point3d> LTrack_ghost = new List <Point3d>();
double dzPrev = (LStage[0].Z - surface.UpTop) * Sh;
double Lghost_x = LTrack[0].X - Msdxdy[i].X * dzPrev;
double Lghost_y = LTrack[0].Y - Msdxdy[i].Y * dzPrev;
LTrack_ghost.Add(new Point3d(Lghost_x, Lghost_y, surface.UpTop));//上側乳剤層上面にtrackがあるならどの位置にあるかを算出する。
string txtfileName_ltrackghost = datarootdirpathw + string.Format(@"\LTrack_ghost.txt");
StreamWriter twriter_ltrackghost = File.CreateText(txtfileName_ltrackghost);
twriter_ltrackghost.WriteLine("{0} {1} {2}", LTrack_ghost[0].X, LTrack_ghost[0].Y, LTrack_ghost[0].Z);
twriter_ltrackghost.Close();
OpenCvSharp.CPlusPlus.Point2d Tangle = ApproximateStraight(Sh, LTrack_ghost[0], LTrack[0], LTrack[1]);
Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle.X, Tangle.Y));
}
else
{
OpenCvSharp.CPlusPlus.Point2d Tangle = ApproximateStraight(Sh, LTrack[i - 2], LTrack[i - 1], LTrack[i]);
Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle.X, Tangle.Y));
}
}//for i-loop
//baseまたぎ
int ltrack_counter = LTrack.Count();
int msdxdy_counter = Msdxdy.Count();
mc.MovePoint(
LTrack[ltrack_counter - 1].X + Msdxdy[msdxdy_counter - 1].X * (surface.LowTop - surface.UpBottom),
LTrack[ltrack_counter - 1].Y + Msdxdy[msdxdy_counter - 1].Y * (surface.LowTop - surface.UpBottom),
surface.LowTop
);
mc.Join();
//////ここから下gelの処理
Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Msdxdy[msdxdy_counter - 1].X, Msdxdy[msdxdy_counter - 1].Y));
//今までのtrack追跡プログラムとは異なる角度等の使い方をする。
dz_price_img = (6 * Math.Cos(theta) / Sh_low) / 1000;
dz_img = dz_price_img * (-1);
dz = dz_img;
int goto_dgel = 0;
for (int i = 0; goto_dgel < 1; i++)
{
///////移動して画像処理をしたときに、下gelの下に入らないようにする。
Vector3 initialpos = mc.GetPoint();
double moverange = 7 * dz_img;
double predpoint = moverange + initialpos.Z;
if (predpoint < surface.LowBottom)//もしもbaseに入りそうなら、8枚目の画像がちょうど下gelを撮影するようにdzを調整する。
{
goto_dgel = 1;
dz = surface.LowBottom - initialpos.Z + 7 * dz_price_img;
}
////////
//goto_dgel == 1のときは、移動して画像を撮影するようにする。
if (i != 0)
{
Vector3 dstpoint = new Vector3(
LTrack_Low[i - 1].X + Msdxdy_Low[i].X * dz * Sh_low,
LTrack_Low[i - 1].Y + Msdxdy_Low[i].Y * dz * Sh_low,
LTrack_Low[i - 1].Z + dz
);
mc.MovePoint(dstpoint);
mc.Join();
}
//今までのtrack追跡プログラムとは異なる角度等の使い方をする。
List <ImageTaking> LiITLowMid = TakeSequentialImage(
Msdxdy[i].X * Sh_low,
Msdxdy[i].Y * Sh_low,
dz_img,
8);
//画像・座標の記録
LStage_Low.Add(new OpenCvSharp.CPlusPlus.Point3d(LiITLowMid[7].StageCoord.X, LiITLowMid[7].StageCoord.Y, LiITLowMid[7].StageCoord.Z));
LiITLowMid[7].img.ImWrite(datarootdirpathw + string.Format(@"\img_l_low_{0}.bmp", i));
LowTrackInfo.Add(LiITLowMid);//撮影した8枚の画像と、撮影した位置を記録する。
//撮影した画像をここで処理する。
List <Mat> binimages = new List <Mat>();
for (int t = 0; t <= 7; t++)
{
Mat bin = (Mat)DogContrastBinalize(LiITLowMid[t].img);
double xx = myTrack.MsDX * myTrack.MsDX;
double yy = myTrack.MsDY * myTrack.MsDY;
if (Math.Sqrt(xx + yy) >= 0.4)
{
Cv2.Dilate(bin, bin, new Mat());
}
Cv2.Dilate(bin, bin, new Mat());
binimages.Add(bin);
}
//trackを重ねる処理を入れる。
Point2d pixel_cen = TrackDetection(binimages, 256, 220, 3, 3, 4, 90, 3);//画像の8枚目におけるtrackのpixel座標を算出する。
//もし検出に失敗した場合はループを抜ける。
if (pixel_cen.X == -1 & pixel_cen.Y == -1)
{
//mc.MovePoint(LTrack_Low[i - 1].X, LTrack_Low[i - 1].Y, LTrack_Low[i - 1].Z);
mc.Join();
not_detect = 1;
goto not_detect_track;
}
//
//検出したpixel座標をstage座標に変換するなどlistに追加する。
LPeak_Low.Add(new OpenCvSharp.CPlusPlus.Point(pixel_cen.X - 256, pixel_cen.Y - 220));
double firstx = LStage_Low[i].X - LPeak_Low[i].X * 0.000267;
double firsty = LStage_Low[i].Y + LPeak_Low[i].Y * 0.000267;
double firstz = LStage_Low[i].Z;
LTrack_Low.Add(new Point3d(firstx, firsty, firstz));
//
//ここからは、最小二乗法で角度を算出するプログラムである。
//上側乳剤層上面の1回目のtrack検出によって、次のtrackの位置を検出する角度を求める。
//その角度が、1回目のtrack検出の結果によって大きな角度にならないように調整をする。
if (i == 0)
{
OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraightBase(Sh, Sh_low, LTrack[ltrack_counter - 2], LTrack[ltrack_counter - 1], LTrack_Low[i], surface);
Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
}
else if (i == 1)
{
OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraightBase(Sh, Sh_low, LTrack[ltrack_counter - 1], LTrack_Low[i - 1], LTrack_Low[i], surface);
Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
}
else
{
OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraight(Sh_low, LTrack_Low[i - 2], LTrack_Low[i - 1], LTrack_Low[i]);
Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
}
}//i_loop
//
int ltrack_low_count = LTrack_Low.Count();
mc.MovePointXY(LTrack_Low[ltrack_low_count - 1].X, LTrack_Low[ltrack_low_count - 1].Y);
mc.Join();
//検出に失敗した場合は、ループを抜けてここに来る。
not_detect_track :;//検出に失敗したと考えられる地点で画像を取得し、下ゲル下面まで移動する。(現在は下ゲル下面とするが、今後変更する可能性有。)
if (not_detect != 0)
{
//写真撮影
List <ImageTaking> NotDetect = TakeSequentialImage(
Msdxdy[0].X * Sh,
Msdxdy[0].Y * Sh,
0,
1);
string txtfileName_t_not_detect = datarootdirpathw + string.Format(@"\not_detect.txt");
StreamWriter twriter_t_not_detect = File.CreateText(txtfileName_t_not_detect);
for (int i = 0; i < NotDetect.Count; i++)
{
NotDetect[i].img.ImWrite(datarootdirpathw + string.Format(@"\img_t_not_detect.bmp"));
Vector3 p = NotDetect[i].StageCoord;
twriter_t_not_detect.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
}
twriter_t_not_detect.Close();
mc.MovePointZ(surface.LowBottom);
mc.Join();
}
//file write out up_gel
string txtfileName_sh_up = datarootdirpathw + string.Format(@"\Sh_up.txt");
StreamWriter twriter_sh_up = File.CreateText(txtfileName_sh_up);
twriter_sh_up.WriteLine("{0}", Sh);
twriter_sh_up.Close();
//file write out
string txtfileName_t_info_up = datarootdirpathw + string.Format(@"\location_up.txt");
StreamWriter twriter_t_info_up = File.CreateText(txtfileName_t_info_up);
for (int i = 0; i < UpTrackInfo.Count; i++)
{
for (int t = 0; t < UpTrackInfo[i].Count; t++)
{
UpTrackInfo[i][t].img.ImWrite(datarootdirpathw + string.Format(@"\img_t_info_up_{0}-{1}.bmp", i, t));
Vector3 p = UpTrackInfo[i][t].StageCoord;
twriter_t_info_up.WriteLine("{0} {1} {2} {3} {4}", i, t, p.X, p.Y, p.Z);
}
}
twriter_t_info_up.Close();
string txtfileName_lpeak = datarootdirpathw + string.Format(@"\lpeak_up.txt");
StreamWriter twriter_lpeak = File.CreateText(txtfileName_lpeak);
for (int i = 0; i < LPeak.Count(); i++)
{
OpenCvSharp.CPlusPlus.Point p = LPeak[i];
twriter_lpeak.WriteLine("{0} {1} {2}", i, p.X, p.Y);
}
twriter_lpeak.Close();
string txtfileName_ltrack = datarootdirpathw + string.Format(@"\ltrack_up.txt");
StreamWriter twriter_ltrack = File.CreateText(txtfileName_ltrack);
for (int i = 0; i < LTrack.Count(); i++)
{
OpenCvSharp.CPlusPlus.Point3d p = LTrack[i];
twriter_ltrack.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
}
twriter_ltrack.Close();
string txtfileName_msdxdy = datarootdirpathw + string.Format(@"\msdxdy.txt");
StreamWriter twriter_msdxdy = File.CreateText(txtfileName_msdxdy);
for (int i = 0; i < Msdxdy.Count(); i++)
{
OpenCvSharp.CPlusPlus.Point2d p = Msdxdy[i];
twriter_msdxdy.WriteLine("{0} {1} {2}", i, p.X, p.Y);
}
twriter_msdxdy.Close();
//file write out low_gel
string txtfileName_sh_low = datarootdirpathw + string.Format(@"\Sh_low.txt");
StreamWriter twriter_sh_low = File.CreateText(txtfileName_sh_low);
twriter_sh_low.WriteLine("{0}", Sh_low);
twriter_sh_low.Close();
string txtfileName_t_info_low = datarootdirpathw + string.Format(@"\location_low.txt");
StreamWriter twriter_t_info_low = File.CreateText(txtfileName_t_info_low);
for (int i = 0; i < LowTrackInfo.Count; i++)
{
for (int t = 0; t < LowTrackInfo[i].Count; t++)
{
LowTrackInfo[i][t].img.ImWrite(datarootdirpathw + string.Format(@"\img_t_info_low_{0}-{1}.bmp", i, t));
Vector3 p = LowTrackInfo[i][t].StageCoord;
twriter_t_info_low.WriteLine("{0} {1} {2} {3} {4}", i, t, p.X, p.Y, p.Z);
}
}
twriter_t_info_low.Close();
string txtfileName_lpeak_low = datarootdirpathw + string.Format(@"\lpeak_low.txt");
StreamWriter twriter_lpeak_low = File.CreateText(txtfileName_lpeak_low);
for (int i = 0; i < LPeak_Low.Count(); i++)
{
OpenCvSharp.CPlusPlus.Point p = LPeak_Low[i];
twriter_lpeak_low.WriteLine("{0} {1} {2}", i, p.X, p.Y);
}
twriter_lpeak_low.Close();
string txtfileName_ltrack_low = datarootdirpathw + string.Format(@"\ltrack_low.txt");
StreamWriter twriter_ltrack_low = File.CreateText(txtfileName_ltrack_low);
for (int i = 0; i < LTrack_Low.Count(); i++)
{
OpenCvSharp.CPlusPlus.Point3d p = LTrack_Low[i];
twriter_ltrack_low.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
}
twriter_ltrack_low.Close();
string txtfileName_msdxdy_low = datarootdirpathw + string.Format(@"\msdxdy_low.txt");
StreamWriter twriter_msdxdy_low = File.CreateText(txtfileName_msdxdy_low);
for (int i = 0; i < Msdxdy_Low.Count(); i++)
{
OpenCvSharp.CPlusPlus.Point2d p = Msdxdy_Low[i];
twriter_msdxdy_low.WriteLine("{0} {1} {2}", i, p.X, p.Y);
}
twriter_msdxdy_low.Close();//*/
//.................Taking Photo in buttom of down layer.......................//
try
{
string[] sp = myTrack.IdString.Split('-');
string dwtxt = string.Format(@"c:\test\bpm\{0}\{1}-{2}-{3}-{4}_dw.txt", mod, mod, pl, sp[0], sp[1]);
string datarootdirpath = string.Format(@"C:\test\bpm\{0}", mod);
System.IO.DirectoryInfo mydir = System.IO.Directory.CreateDirectory(datarootdirpath);
System.IO.DirectoryInfo mydir2 = System.IO.Directory.CreateDirectory(datarootdirpath);
BeamDetection(dwtxt, false);
}
catch (ArgumentOutOfRangeException)
{
MessageBox.Show("ID is not exist ");
}
//...............Move to top surfacr of upper layer........................//
try
{
Vector3 cc = mc.GetPoint();
double Zp = surface.UpTop;
mc.MoveTo(new Vector3(cc.X, cc.Y, Zp));
mc.Join();
}
catch (ArgumentOutOfRangeException)
{
MessageBox.Show("Cannot move to top surface of upperlayer ");
}
// .......................................................................//
sw.Stop();
string[] sp1 = myTrack.IdString.Split('-');
string logtxt_ = string.Format(@"c:\test\bpm\{0}\{1}-{2}_log_.txt", mod, mod, pl);
//string log_ = string.Format("{0} \n", sw.Elapsed);
string log_ = string.Format("{0} {1} {2} \n", sp1[0], sp1[1], sw.Elapsed);
StreamWriter swr = new StreamWriter(logtxt_, true, Encoding.ASCII);
swr.Write(log_);
swr.Close();
}
private void task()
{
try {
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
CoordManager cm = new CoordManager(parameterManager);
Vector3 InitPoint = mc.GetPoint();
Vector3 SurfPoint = mc.GetPoint();
Vector3 p = new Vector3();
int viewcounter = 0;
//string txtfileName = string.Format(@"{0}\{1}.txt",
// direcotryPath, System.DateTime.Now.ToString("yyyyMMdd_HHmmss_ffff"));
//StreamWriter twriter = File.CreateText(txtfileName);
List <pointscan> PSList = new List <pointscan>();
var reader = new StreamReader(File.OpenRead(@"C:\test\list.txt"));
bool headerflag = true;
while (!reader.EndOfStream)
{
var line = reader.ReadLine();
string[] delimiter = { " " };
var values = line.Split(delimiter, StringSplitOptions.RemoveEmptyEntries);
pointscan ps = new pointscan();
ps.id = int.Parse(values[1]);
ps.stagecoord = new Vector3(
double.Parse(values[12]),
double.Parse(values[13]),
double.Parse(values[14])
);
PSList.Add(ps);
}
int nshot = 26;
byte[] bb = new byte[440 * 512 * nshot];
camera.Stop();
for (int pp = 0; pp < PSList.Count(); pp++)
{
viewcounter = 0;
if (pp % 1 == 0)
{
Vector3 surfrecogpoint = PSList[pp].stagecoord;
surfrecogpoint.Z = InitPoint.Z + 0.06;
mc.MoveTo(surfrecogpoint);
mc.Join();
bool flag = true;
while (flag)
{
mc.MoveDistance(-0.003, VectorId.Z);
mc.Join();
byte[] b = Ipt.CaptureMain();
int brightness;
using (Mat src = new Mat(440, 512, MatType.CV_8U, b))
using (Mat mat = src.Clone()) {
Cv2.GaussianBlur(mat, mat, Cv.Size(3, 3), -1);
using (Mat gau = mat.Clone()) {
Cv2.GaussianBlur(gau, gau, Cv.Size(31, 31), -1);
Cv2.Subtract(gau, mat, mat);
Cv2.Threshold(mat, mat, 10, 1, ThresholdType.Binary);
brightness = Cv2.CountNonZero(mat);
}//using gau
}//using src and mat
viewcounter++;
if (brightness > 5000 || viewcounter > 40)
{
flag = false;
}
}
SurfPoint = mc.GetPoint();
}
Vector3 CandPoint = PSList[pp].stagecoord;
CandPoint.Z = SurfPoint.Z + CandPoint.Z - 0.044;
mc.MoveTo(CandPoint);
mc.Join();
camera.Start();
led.AdjustLight(parameterManager);
camera.Stop();
p = mc.GetPoint();
double prev_z = p.Z;
DateTime starttime = System.DateTime.Now;
string datfileName = string.Format(@"{0}\{1:00000}_x{2}_y{3}.dat",
direcotryPath,
PSList[pp].id,
(int)(p.X * 1000),
(int)(p.Y * 1000));
BinaryWriter writer = new BinaryWriter(File.Open(datfileName, FileMode.Create));
string stlog = "";
viewcounter = 0;
while (viewcounter < nshot)
{
mc.MoveDistance(0.003, VectorId.Z);
mc.Join();
byte[] b = Ipt.CaptureMain();
p = mc.GetPoint();
TimeSpan ts = System.DateTime.Now - starttime;
stlog += String.Format("{0} {1} {2} {3} {4} {5} {6} {7}\n",
pp,
System.DateTime.Now.ToString("HHmmss\\.fff"),
ts.ToString("s\\.fff"),
(p.X * 1000).ToString("0.0"),
(p.Y * 1000).ToString("0.0"),
(p.Z * 1000).ToString("0.0"),
(prev_z * 1000 - p.Z * 1000).ToString("0.0"),
viewcounter);
b.CopyTo(bb, 440 * 512 * viewcounter);
viewcounter++;
}
//twriter.Write(stlog);
writer.Write(bb);
writer.Flush();
writer.Close();
GC.Collect();
GC.WaitForPendingFinalizers();
GC.Collect();
}
camera.Start();
//twriter.Close();
} catch (Exception ex) {
System.Windows.Forms.MessageBox.Show(ex.Message);
}
}
private void task()
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
CoordManager cm = new CoordManager(parameterManager);
Vector3 InitPoint = mc.GetPoint();
List <Mat> image_set = new List <Mat>();
Vector3 ggg = mc.GetPoint();
string txtfileName = string.Format(@"c:\img\bbbbbb.txt");
StreamWriter twriter = File.CreateText(txtfileName);
// twriter.Write("line95\n");
Vector3 initialpoint = mc.GetPoint();
List <Vector2> list_lateralmark43 = new List <Vector2>();//in PL#43
list_lateralmark43.Add(new Vector2(81.706, 89.124));
list_lateralmark43.Add(new Vector2(-89.715, 83.350));
list_lateralmark43.Add(new Vector2(-86.969, -87.964));
list_lateralmark43.Add(new Vector2(83.521, -86.169));
List <Vector2> list_scanregion43 = new List <Vector2>();
list_scanregion43.Add(list_lateralmark43[0] - new Vector2(-5, -5));
list_scanregion43.Add(list_lateralmark43[1] - new Vector2(+5, -5));
list_scanregion43.Add(list_lateralmark43[2] - new Vector2(+5, +5));
list_scanregion43.Add(list_lateralmark43[3] - new Vector2(-5, +5));
List <Vector2> list_lateralmark42 = new List <Vector2>();//in PL#42
list_lateralmark42.Add(new Vector2(81.750, 89.099));
list_lateralmark42.Add(new Vector2(-89.526, 83.417));
list_lateralmark42.Add(new Vector2(-86.754, -87.727));
list_lateralmark42.Add(new Vector2(83.612, -85.831));
List <Vector2> list_scanregion42 = new List <Vector2>();
list_scanregion42.Add(list_lateralmark42[0] - new Vector2(-5, -5));
list_scanregion42.Add(list_lateralmark42[1] - new Vector2(+5, -5));
list_scanregion42.Add(list_lateralmark42[2] - new Vector2(+5, +5));
list_scanregion42.Add(list_lateralmark42[3] - new Vector2(-5, +5));
Affine ap_s43_s42 = Affine.CreateAffineBy(list_lateralmark43, list_lateralmark42);
List <Vector2> list_scanpoint_g = new List <Vector2>();
for (int x = 0; x < 11; x++)
{
for (int y = 0; y < 11; y++)
{
list_scanpoint_g.Add(new Vector2(x * 1.0, y * 1.0));
}
}
List <Vector2> list_scancorner_g = new List <Vector2>();
list_scancorner_g.Add(new Vector2(10.0, 10.0));
list_scancorner_g.Add(new Vector2(0.0, 10.0));
list_scancorner_g.Add(new Vector2(0.0, 0.0));
list_scancorner_g.Add(new Vector2(10.0, 0.0));
Affine ap_g43_s43 = Affine.CreateAffineBy(list_scancorner_g, list_scanregion43);
camera.Start();
for (int d = 0; d < list_scanpoint_g.Count; d++)
{
// Vector2 predpoint41 = ap_g41_s41.Trance(list_scanpoint_g[d]);
Vector2 predpoint43 = ap_g43_s43.Trance(list_scanpoint_g[d]);
Vector2 predpoint42 = ap_s43_s42.Trance(predpoint43);
mc.MoveTo(new Vector3(predpoint42.X, predpoint42.Y, initialpoint.Z - 0.100));
mc.Join();
Thread.Sleep(4000);
int ledbrightness = led.AdjustLight(parameterManager);
int viewcounter = 0;
bool flag = true;
while (flag)
{
mc.MoveDistance(+0.003, VectorId.Z);
mc.Join();
byte[] b = camera.ArrayImage;
Mat src = new Mat(440, 512, MatType.CV_8U, b);
Mat mat = src.Clone();
Cv2.GaussianBlur(mat, mat, Cv.Size(3, 3), -1);
Mat gau = mat.Clone();
Cv2.GaussianBlur(gau, gau, Cv.Size(7, 7), -1);
Cv2.Subtract(gau, mat, mat);
Cv2.Threshold(mat, mat, 4, 1, ThresholdType.Binary);
int brightness = Cv2.CountNonZero(mat);
viewcounter++;
if (brightness > 2000 || viewcounter > 100)
{
flag = false;
}
}//while-loop surface detection
Vector3 surfacepoint = mc.GetPoint();
for (int vy = 0; vy < 2; vy++)
{
for (int vx = 0; vx < 2; vx++)
{
mc.MoveTo
(new Vector3(
predpoint42.X - 0.125 / 2.0 + 0.125 * vx,
predpoint42.Y - 0.105 / 2.0 + 0.105 * vy,
surfacepoint.Z));
mc.Join();
for (int q = 0; q < 8; q++)
{
mc.MovePointZ(surfacepoint.Z + 0.003 + (0.003 * q));
mc.Join();
Vector3 currentpoint = mc.GetPoint();
byte[] b = camera.ArrayImage;
Mat image = new Mat(440, 512, MatType.CV_8U, b);
Mat clone_image = image.Clone();
//clone_image.ImWrite(String.Format(@"c:\img\{0}_{1:00}.bmp", d, q));
clone_image.ImWrite(String.Format(@"c:\img\{0}_{1}_{2}_{3:00}.bmp", d, vx, vy, q));
string stlog = "";
stlog += String.Format("{0} {1} {2} {3}\n",
ledbrightness,
currentpoint.X,
currentpoint.Y,
currentpoint.Z);
twriter.Write(stlog);
// twriter.Write("line198\n");
twriter.Flush();
} //for-loop q pictureID
} //for-loop vx view0,1
} //for-loop vy view0,1
} //for-loop d-th scanning point
camera.Stop();
twriter.Close();
}
private void task()
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
CoordManager cm = new CoordManager(parameterManager);
Vector3 InitPoint = mc.GetPoint();
List <Vector2> src = new List <Vector2>();
List <Vector2> dst = new List <Vector2>();
src.Add(new Vector2(0, 0));
dst.Add(new Vector2(0, 0));
src.Add(new Vector2(-140.0, 150.0));
dst.Add(new Vector2(-138.770, 151.485));
src.Add(new Vector2(+140.0, 160.0));
dst.Add(new Vector2(+141.581, 158.903));
src.Add(new Vector2(+150.0, -140.0));
dst.Add(new Vector2(+148.880, -141.600));
src.Add(new Vector2(-150.0, -140.0));
dst.Add(new Vector2(-151.297, -138.670));
Affine ap = Affine.CreateAffineBy(src, dst);
string txtfileName = string.Format(@"c:\img\_grid_g.txt");
StreamWriter twriter = File.CreateText(txtfileName);
for (int x = -14; x < 14; x += 2)
{
for (int y = -14; y < 14; y += 2)
{
Vector2 predpoint = ap.Trance(new Vector2(x * 10, y * 10));
mc.MoveTo(new Vector3(predpoint.X, predpoint.Y, InitPoint.Z));
mc.Join();
led.AdjustLight(parameterManager);
byte[] b2 = camera.ArrayImage;
Mat src2 = new Mat(440, 512, MatType.CV_8U, b2);
Mat mat2 = src2.Clone();
string FileName = string.Format(@"C:\img\grid_x{0}_y{1}.png", x, y);
Cv2.ImWrite(FileName, mat2);
string stlog = "";
stlog += String.Format("{0} {1} {2:f4} {3:f4}\n",
x * 10,
y * 10,
predpoint.X,
predpoint.Y);
twriter.Write(stlog);
} //for y
} //for x
twriter.Close();
}//task()
