List <ImageTaking> TakeSequentialImage(double ax, double ay, double dz, int nimage)
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Vector3 initialpos = mc.GetPoint();
List <ImageTaking> lit = new List <ImageTaking>();
for (int i = 0; i < nimage; i++)
{
Vector3 dstpoint = new Vector3(
initialpos.X + ax * dz * i,
initialpos.Y + ay * dz * i,
initialpos.Z + dz * i
);
mc.MovePoint(dstpoint);
mc.Join();
byte[] b = camera.ArrayImage;
Mat image = new Mat(440, 512, MatType.CV_8U, b);
ImageTaking it = new ImageTaking();
it.img = image.Clone();
it.StageCoord = mc.GetPoint();
lit.Add(it);
//image.Release();
//imagec.Release();
}
return(lit);
}
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
