void KeyHandleWorkspace_KeyUp(object sender, System.Windows.Input.KeyEventArgs e)
{
// モータの移動処理を停止する．
MotorControler mc = MotorControler.GetInstance(parameterManager);
if (e.Key == Key.A || e.Key == Key.D)
{
mc.SlowDownStop(VectorId.X);
}
if (e.Key == Key.W || e.Key == Key.X)
{
mc.SlowDownStop(VectorId.Y);
}
if (e.Key == Key.Q || e.Key == Key.E)
{
mc.SlowDownStop(VectorId.Z);
}
}
private void task()
{
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
Vector3 InitPoint = mc.GetPoint();
Vector3 p = new Vector3();
double emthickness = sur.UpTop - sur.UpBottom;
int nshot = (int)(emthickness / 0.003);
int blockXCounter = 0;
int blockYCounter = 0;
while (blockYCounter < nyView)
{
while (blockXCounter < nxView)
{
string txtfileName = string.Format(@"{0}\{1}.txt"
, direcotryPath
, System.DateTime.Now.ToString("yyyyMMdd_HHmmss_ffff")
);
StreamWriter twriter = File.CreateText(txtfileName);
//Vector3 InitPointofThisBlock = new Vector3(
// InitPoint.X + blockXCounter * 4.350,
// InitPoint.Y + blockYCounter * 4.390,
// InitPoint.Z
// );
//Vector3 SurfPointofThisBlock = new Vector3(
// InitPointofThisBlock.X + 2.200,
// InitPointofThisBlock.Y + 2.200,
// InitPoint.Z
// );
Vector3 InitPointofThisBlock = new Vector3(
InitPoint.X + (double)(blockXCounter) * ((0.210 - 0.01) * 10 - 0.030), //if x40 -> 2.150,
InitPoint.Y - (double)(blockYCounter) * ((0.180 - 0.01) * 10 - 0.030), //if x40 -> 2.170,
InitPoint.Z
);
Vector3 SurfPointofThisBlock = new Vector3(
InitPointofThisBlock.X + 1.000,
InitPointofThisBlock.Y - 1.000,
InitPoint.Z
);
//go to surface measurement
mc.MovePoint(SurfPointofThisBlock.X, SurfPointofThisBlock.Y, sur.UpTop + 0.050);//above 50micron
mc.Join();
//surface landing
bool flag = true;
int layercounter = 0;
camera.Start();
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(31, 31), -1);
Cv2.Subtract(gau, mat, mat);
Cv2.Threshold(mat, mat, 10, 1, ThresholdType.Binary);
int brightness = Cv2.CountNonZero(mat);
layercounter++;
if (brightness > 10000 || layercounter > 30)
{
flag = false;
}
}
led.AdjustLight(parameterManager);
camera.Stop();
//surface
double surfacetopz = mc.GetPoint().Z;
double surfacebottomz = surfacetopz - emthickness;
//data taking
int rowcounter = 0;
int colcounter = 0;
//while (rowcounter < 24) {
// while (colcounter < 20) {
while (rowcounter < 12)
{
while (colcounter < 10)
{
string stlog = "";
byte[] bb = new byte[440 * 512 * nshot];
double startZ = 0.0;
PlusMinus plusminus;
if (colcounter % 2 == 0)
{
//camera.Start();
//led.AdjustLight(parameterManager);
//camera.Stop();
startZ = surfacetopz + 0.012;
plusminus = PlusMinus.Minus;
}
else
{
startZ = surfacebottomz - 0.009;
plusminus = PlusMinus.Plus;
}
double prev_z = startZ;
mc.MovePoint(
InitPointofThisBlock.X + (0.210 - 0.01) * colcounter, //x40, 0.230-0.01 //parameterManager.SpiralShiftX
InitPointofThisBlock.Y - (0.180 - 0.01) * rowcounter, //x40, 0.195-0.01 //parameterManager.SpiralShiftY
startZ);
mc.Join();
p = mc.GetPoint();
DateTime starttime = System.DateTime.Now;
string datfileName = string.Format(@"{0}\{1}_x{2}_y{3}_xi{4}_yi{5}.dat",
direcotryPath,
starttime.ToString("yyyyMMdd_HHmmss"),
(int)(p.X * 1000),
(int)(p.Y * 1000),
colcounter,
rowcounter
);
BinaryWriter writer = new BinaryWriter(File.Open(datfileName, FileMode.Create));
mc.Inch(plusminus, 0.15, VectorId.Z);
int viewcounter = 0;
while (viewcounter < nshot + 3)
{
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",
colcounter % 2,
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);
prev_z = p.Z;
if (viewcounter >= 3)
{
b.CopyTo(bb, 440 * 512 * (viewcounter - 3));
}
viewcounter++;
}//view
viewcounter = 0;
mc.SlowDownStop(VectorId.Z);
twriter.Write(stlog);
writer.Write(bb);
writer.Flush();
writer.Close();
colcounter++;
}//col
colcounter = 0;
rowcounter++;
}//row
rowcounter = 0;
twriter.Close();
blockXCounter++;
}//blockX
blockXCounter = 0;
blockYCounter++;
}//blockY
blockYCounter = 0;
camera.Start();
}//end of task()
/// <summary>
/// 撮影処理を行います．
/// <para>別スレッドのタスクとして実行してください．直接呼び出さないでください．</para>
/// </summary>
private void shootingThreadTask()
{
if (Started != null)
{
Started(this, new EventArgs());
}
imagesUri = new List <string>();
shotPoint = new List <double>();
MotorControler mc = MotorControler.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
camera.Start();
numOfShot = 0;
// 撮影開始地点へ移動する
mc.MovePointZ(startPoint);
mc.Join();
StreamWriter writer = new StreamWriter(string.Format(@"{0}\{1}.txt", filepath, filenameprefix));
// 撮影終了地点に移動しながら画像を確保する
double pnInterval = (startPoint > endPoint ? -interval : interval);
double presentPoint = mc.GetPoint().Z;
while (isShootContinue(presentPoint))
{
mc.MoveDistance(pnInterval, VectorId.Z);
mc.Join();
presentPoint = mc.GetPoint().Z;
byte[] b = camera.ArrayImage;
Mat src = new Mat(440, 512, MatType.CV_8U, b);
string pngfileName = string.Format(@"{0}\{1}{2:000}.png", filepath, filenameprefix, numOfShot);
src.ImWrite(pngfileName);
shotPoint.Add(presentPoint);
writer.WriteLine(presentPoint.ToString());
++numOfShot;
if (OnShort != null)
{
ActivityEventArgs e = new ActivityEventArgs();
e.ZValue = presentPoint;
OnShort(this, e);
}
#if false//20140225 yokoyama,jyoshida
mc.MoveDistance(pnInterval, VectorId.Z, delegate {
presentPoint = mc.GetPoint().Z;
Camera c = Camera.GetInstance();
saveTemp(c.Image);
shotPoint.Add(presentPoint);
++numOfShot;
if (OnShort != null)
{
ActivityEventArgs e = new ActivityEventArgs();
e.ZValue = presentPoint;
OnShort(this, e);
}
});
#endif
}//while
writer.Close();
mc.SlowDownStop(VectorId.Z);
}
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
/// <summary>
/// recogThreadの行う処理です．このメソッドを直接呼び出さないでください．
/// </summary>
private void recogThread_Task()
{
App.logger.Info("Surface.recogThread_Task() start");
if (Started != null)
{
Started(this, new EventArgs());
}
Camera camera = Camera.GetInstance();
camera.Start();
/* Z軸をSpeed1(低速)でマイナス方向に動かして，最下点もしくは下降距離分だけ移動させる．*/
MotorControler mc = MotorControler.GetInstance(parameterManager);
speedBeforeStart = mc.Speed;
bool flag;
double temp_z;
mc.Inch(PlusMinus.Minus, motorSpeed, VectorId.Z);
flag = true;
while (flag)
{
int brightness = CountHitPixels();
if (brightness > 8000)
{
flag = false;
}
//if (brightness < 10000) flag = false;//
}
/*
* UpTopRecognized(this, new eventArgs());
* LowBottomRecognized(this, new eventArgs());
* LowTopRecognized(this, new eventArgs());
* UpBottomRecognized(this, new eventArgs());
*/
mc.SlowDownStop(VectorId.Z);
surfaces[0] = mc.GetPoint().Z;
System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
Thread.Sleep(100);
mc.Move(new Vector3(0.0, 0.0, -0.2));
mc.Join();
Thread.Sleep(100);
mc.Inch(PlusMinus.Minus, motorSpeed, VectorId.Z);
flag = true;
while (flag)
{
int brightness = CountHitPixels();
if (brightness < 5000)
{
flag = false; //
}
// if (brightness < 4000) flag = false;
}
surfaces[1] = mc.GetPoint().Z;//
// surfaces[2] = mc.GetPoint().Z;
System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
flag = true;
while (flag)
{
int brightness = CountHitPixels();
//if (brightness > 4000) flag = false;
if (brightness > 5000)
{
flag = false;
}
}
mc.SlowDownStop(VectorId.Z);
// surfaces[1] = mc.GetPoint().Z;
surfaces[2] = mc.GetPoint().Z;//
System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
Thread.Sleep(100);
// Thread.Sleep(150);//
// mc.Move(new Vector3(0.0, 0.0, -0.2));
mc.Move(new Vector3(0.0, 0.0, -0.23));//
mc.Join();
Thread.Sleep(100);
//Thread.Sleep(150);//
mc.Inch(PlusMinus.Minus, motorSpeed, VectorId.Z);
flag = true;
while (flag)
{
int brightness = CountHitPixels();
// if (brightness < 2000) flag = false;
if (brightness < 3000)
{
flag = false; //
}
}
mc.SlowDownStop(VectorId.Z);
surfaces[3] = mc.GetPoint().Z;
System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
Thread.Sleep(100);
// Thread.Sleep(150);//
// temp_z = (surfaces[0] - 0.05) - mc.GetPoint().Z;
temp_z = (surfaces[0] + 0.02) - mc.GetPoint().Z;//
mc.Move(new Vector3(0.0, 0.0, temp_z));
mc.Join();
Thread.Sleep(100);
//Thread.Sleep(150);//
/*
* // モータZ軸をマイナス方向(下方向)に稼働させる
* try {
* // 下降の開始
* mc.SetMotorSpeed(IO.MotorSpeed.Speed3);
* mc.Inch(MechaAxisAddress.ZAddress, PlusMinus.Plus);
* } catch (MotorAxisException) {
* // すでに最下点に位置していた場合は，catchのスコープが実行される．
* isOnBottomLimit = true;
* }
*
* // 最下点もしくは下降距離分に下降したかを監視する．
* double startPoint = mc.GetPoint().Z;
* while (!isOnBottomLimit) {
* // 移動距離の確認
* if (loweringDistance != 0) {
* isOnBottomLimit = (loweringDistance <= Math.Abs(mc.GetPoint().Z - startPoint));
* }
*
* // モータの移動可能最下点到達か否かの確認
* isOnBottomLimit |=
* (mc.GetAbnomalState(MechaAxisAddress.ZAddress)
* == MotorAbnomalState.AxisLimitPlus);
* }
* // 最下点に到達したため，モータの移動を止める．
* // 最下点に到達時のイベントを発生させる
* mc.StopInching(MechaAxisAddress.ZAddress);
* if (OnMotorBottomLimited != null) {
* OnMotorBottomLimited(this, new EventArgs());
* }
*
* // 遅延を行わないとモータドライバが動作不良を起こす場合がある.
* Thread.Sleep(300);
*
* */
/* -------------------- 下降の完了 ---------------- */
/* 上昇しながら撮影中の画像を分析する */
// 最下点からZ軸をプラス方向に動かす．
// 撮影中の画像がゲルの中か否かを判定する．
// ベース及びその他の場所と，ゲルの中の境界である座標を記録する．
/*
*
*
* mc.Inch(PlusMinus.Plus, motorSpeed, VectorId.Z);
* startPoint = mc.GetPoint().Z;
*
*
* // bool flag = true;
*
* while (flag) {
* //Thread.Sleep(delayTime);
*
* 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(31, 31), -1);
* Cv2.Subtract(gau, mat, mat);
* Cv2.Threshold(mat, mat, 10, 1, ThresholdType.Binary);
* int brightness = Cv2.CountNonZero(mat);
*
* if (brightness > 3000) flag = false;
* }
*
* mc.StopInching(MechaAxisAddress.ZAddress);
* surfaces[3] = mc.GetPoint().Z;
* System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
*
*
*
* // Vector3 speed = new Vector3(30, 30, 0.4);//つかわないけど
* // Vector3 tolerance = new Vector3(0.001, 0.001, 0.001);//つかわないけど
* // Vector3 distance = new Vector3(0, 0, 0.3);
* // mc.Move(distance, speed, tolerance);
*
* Thread.Sleep(100);
*
*
*/
/*
*
*
* mc.Inch(PlusMinus.Plus, MotorSpeed, VectorId.Z);
* double currentPoint = mc.GetPoint().Z;
*
* bool dice = true;
*
* while (dice) {
*
* double nowPoint = mc.GetPoint().Z;
*
* double renge = nowPoint - currentPoint;
*
* if (renge > 0.20) dice = false;
*
*
*
*
* }
* mc.StopInching(MechaAxisAddress.ZAddress);
*
* Thread.Sleep(100);
*
*
* /
*
*
* // mc.MoveDistance(0.2, VectorId.Z);
* // mc.Join();
* // Thread.Sleep(2000);
* // System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
* /*
*
* mc.Inch(PlusMinus.Plus, motorSpeed, VectorId.Z);
* flag = true;
* while (flag) {
*
* 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(31, 31), -1);
* Cv2.Subtract(gau, mat, mat);
* Cv2.Threshold(mat, mat, 10, 1, ThresholdType.Binary);
* int brightness = Cv2.CountNonZero(mat);
*
* if (brightness < 4000) flag = false;
* }
*
*
* surfaces[2] = mc.GetPoint().Z;
* System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
*
*/
/*
*
*
* flag = true;
* while (flag) {
*
* 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(31, 31), -1);
* Cv2.Subtract(gau, mat, mat);
* Cv2.Threshold(mat, mat, 10, 1, ThresholdType.Binary);
* int brightness = Cv2.CountNonZero(mat);
*
* if (brightness > 4000) flag = false;
* }
*
* mc.StopInching(MechaAxisAddress.ZAddress);
* surfaces[1] = mc.GetPoint().Z;
* System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
*
*
* //mc.Move(distance, speed, tolerance);
*
*
*
*
*
*
*
* // 遅延を行わないとモータドライバが動作不良を起こす場合がある.
* Thread.Sleep(100);
*
*
*/
/*
*
* mc.Inch(PlusMinus.Plus, MotorSpeed, VectorId.Z);
* double current2Point = mc.GetPoint().Z;
*
* bool dice2 = true;
*
* while (dice2) {
*
* double nowPoint = mc.GetPoint().Z;
*
* double renge = nowPoint - current2Point;
*
* if (renge > 0.20) dice2 = false;
*
*
*
*
* }
*
* mc.StopInching(MechaAxisAddress.ZAddress);
*
* Thread.Sleep(100);
*
*/
// mc.MoveDistance(0.2, VectorId.Z);
// mc.Join();
// Thread.Sleep(2000);
// System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
/* mc.Inch(PlusMinus.Plus, motorSpeed, VectorId.Z);
* flag = true;
* while (flag) {
*
* 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(31, 31), -1);
* Cv2.Subtract(gau, mat, mat);
* Cv2.Threshold(mat, mat, 10, 1, ThresholdType.Binary);
* int brightness = Cv2.CountNonZero(mat);
*
* if (brightness < 12000) flag = false;
* }
* mc.StopInching(MechaAxisAddress.ZAddress);
* surfaces[0] = mc.GetPoint().Z;
* System.Diagnostics.Debug.WriteLine(string.Format("{0}", mc.GetPoint().Z));
*
*
*
*/
//////////////
// mc.AAAAAA();
/*
* stopWatch = new System.Diagnostics.Stopwatch();
* stopWatch.Start();
*
* int idNo = 0;
* int index = 3;
#if false
* bool[] votes = new bool[numOfVoting];
* int[] values = new int[numOfVoting];
* double[] points = new double[numOfVoting];
*
* Thread.Sleep(delayTime);
*
* // 今回の入力画像の演算結果を多数決配列の最後尾に設定する．
* // 同時に今回の判定値やモータの座標もそれぞれ保持する．
* votes[votes.Length - 1] = IsInGel();
* values[values.Length - 1] = brightness;
* points[points.Length - 1] = mc.GetPoint().Z;
*
* // 今回と過去一定回数分の結果で多数決をとる．
* presentResult = Vote(votes);
*
* // 多数決の結果配列を一つずつシフトする
* for (int i = 0; i < values.Length; ++i) {
* votes[i] = (i == votes.Length - 1 ? false : votes[i + 1]);
* values[i] = (i == values.Length - 1 ? 0 : values[i + 1]);
* points[i] = (i == points.Length - 1 ? 0 : points[i + 1]);
* }
#endif
*
* double previousZVal, presentZVal;
* int previousBrightness , presentBrightness;
* bool previousResult = false, presentResult = false;
* while (index >= 0) {
* Thread.Sleep(delayTime);
* presentZVal = Math.Round(mc.GetPoint().Z, 5);
* presentResult = IsInGel();
* bool isBorder = isBoudarySurface(previousResult, presentResult, index);
*
* // 撮像・入力画像の確認のイベントを発生させる．
* SurfaceEventArgs eventArgs = new SurfaceEventArgs();
* eventArgs.Id = idNo;
#if false
* eventArgs.ZValue = points[0];
* eventArgs.Brightness = values[0];
* eventArgs.Distance = Math.Abs(points[0] - startPoint);
#endif
*
* presentBrightness = brightness;
* eventArgs.ZValue = presentZVal;
* eventArgs.Brightness = presentBrightness;
* eventArgs.Distance = Math.Abs(presentZVal - startPoint);
* eventArgs.IsInGel = presentResult;
* eventArgs.IsBoundary = isBorder;
* eventArgs.Second = Time;
++idNo;
* if (Shooting != null) {
* Thread t = new Thread(new ThreadStart(delegate() {
* Shooting(this, eventArgs);
* }));
* t.IsBackground = true;
* t.Start();
* }
*
* if (isBorder) {
* // 境界面であった場合，多数決に用いたZ座標の値の内，最も古い値を境界面の座標として採用する．
* // ここでは境界面として，その座標値を保持している．
* //surfaces[index] = points[0];
* surfaces[index] = presentZVal;
*
* // 境界面認識時のイベントを発生させる．
* switch (index) {
* case 3:
* if (LowBottomRecognized != null) {
* LowBottomRecognized(this, eventArgs);
* }
* break;
* case 2:
* if (LowTopRecognized != null) {
* LowTopRecognized(this, eventArgs);
* }
* break;
* case 1:
* if (UpBottomRecognized != null) {
* UpBottomRecognized(this, eventArgs);
* }
* break;
* case 0:
* if (UpTopRecognized != null) {
* UpTopRecognized(this, eventArgs);
* }
* break;
* }
*
* --index;
* }
* previousResult = presentResult;
* previousBrightness = presentBrightness;
* previousZVal = presentZVal;
*
* // 距離によるエラー終了
* if (mc.GetPoint().Z - startPoint > distanceOut) {
* mc.StopInching(MechaAxisAddress.ZAddress);
* throw new SurfaceFailedException("Distance limit out");
* }
* }
*
* mc.StopInching(MechaAxisAddress.ZAddress);
* stopWatch.Stop();
* dateTime = DateTime.Now;
* latestCoord = new Vector2(mc.GetPoint().X, mc.GetPoint().Y);
*/
}
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
