/// <summary>
/// 撮影処理を行うループです．
/// <para>Startメソッドで開始し，Stopメソッドで停止させてください．</para>
/// <para>画像撮影のみを行います．モータ制御動作を廃止しましした．</para>
/// </summary>
private void captureLoop()
{
while (flagBreakCaptureLoop)
{
// 撮影画像を取得し，描画する．
#if !_NoHardWare
try {
captruredBuffer = Ipt.CaptureMain();
} catch (Exception ex) {
System.Console.WriteLine(ex.Message);
}
#else
// ハードウェアがないデバッグ時は適当な映像を描写
for (int i = 0; i < captruredBuffer.Length; ++i)
{
captruredBuffer[i] = (byte)(random.Next(255));
}
Thread.Sleep(10);
#endif
if (action != null)
{
actionThread = new Thread(new ThreadStart(action));
actionThread.IsBackground = true;
actionThread.Start();
actionThread.Join();
}
Thread.Sleep(10);
}
}
/// <summary>
/// 設定ファイルを読み込み，パラメータを(再)定義します．
/// </summary>
/// <exception cref="System.IO.FileNotFoundException">設定ファイルが見つからなかった場合</exception>
public void Load()
{
xmlDocument.Load(ConfigureFile);
parseCcdXml();
Magnification = 10;
Ipt.SetCcdResolution(CcdResolutionX, CcdResolutionY);
}
public Track(int _index, short[] _id, short _direction, double _x, double _y, double dx, double dy, string _comment = null)
{
this.index = _index;
this.id = _id;
this.direction = _direction;
this.x = _x;
this.y = _y;
this.comment = _comment;
Ipt.GToM("p", x, y, ref this.msx, ref this.msy);
Ipt.GToM("a", dx, dy, ref this.msdx, ref this.msdy);
this.msTan = Math.Sqrt(msdx * msdx + msdy * msdy);
}
public int MarkCenter(ref double x, ref double y, int size)
{
Camera camera = Camera.GetInstance();
bool isStart = camera.IsRunning;
camera.Stop();
int retval = Ipt.MarkCenter(ref x, ref y, size);
if (isStart)
{
camera.Start();
}
return(retval);
}
/// <summary>
/// Prediction, ScanData, Vertexを記録するファイルを開きます．
/// 更に，出力先ファイルを設定します．
/// </summary>
/// <param name="mode">ファイルの種類</param>
/// <param name="inputFilePath">入力元ファイルへのパス</param>
/// <param name="outputFilePath">出力先ファイルへのパス(エラーの出力先)</param>
/// <param name="append">
/// 出力ファイルが存在していた場合にアペンドするかどうか，true=アペンド, false=上書き
/// </param>
public void OpenInFile(OpenInFileMode mode, string inputFilePath, string outputFilePath, Boolean append = true)
{
int returnCode = Ipt.OpenFile((int)mode, inputFilePath, outputFilePath, (append ? "a" : "w"));
if (returnCode == -1)
{
throw new Exception(
string.Format(
"ipt.dll OpenFile is failed. input is {0}, output is {1}, mode is {2}",
inputFilePath,
outputFilePath,
mode.ToString()
));
}
openedInFileMode = mode;
tracksManager.Initialize();
}
/// <summary>
/// (*.grid)ファイルを読み込みます
/// </summary>
/// <param name="filePath"></param>
public void ReadGrid(string filePath)
{
if (!File.Exists(filePath))
{
throw new FileNotFoundException(string.Format("{0} is not found.", filePath));
}
int ret = Ipt.ReadWriteGridData("r", filePath);
if (ret != 0)
{
throw new FormatException(string.Format("Format of {0} is not correct", filePath));
}
loadedGrid = true;
definedBeamCo = true;
definedGGDat = true;
}
/// <summary>
/// (*.gofs)ファイルを読み込みます．
/// </summary>
/// <param name="filePath">読み込むファイルへのパス</param>
/// <param name="readAnyTime"></param>
/// <exception cref="FormatException">読み込みに失敗した場合</exception>
/// <exception cref="System.IO.FileNotFoundException">ファイルが存在しなかった場合</exception>
public void ReadGridFineParameter(string filePath, Boolean readAnyTime)
{
if (!File.Exists(filePath))
{
throw new FileNotFoundException(filePath);
}
EmulsionParameter emParam = parameterManager.EmulsionParameter;
double emulsionThickness = new double();
int ret = Ipt.ReadWriteGridFine("r", filePath, ref emulsionThickness);
if (ret == -1)
{
throw new FormatException(string.Format("ThisGelThickness can not input from {0}", filePath));
}
thisGelThickness = (emulsionThickness - emParam.BaseThick) * 0.5;
definedGridFine = true;
}
/// <summary>追跡するtrackを更新し，次のtrackにします．</summary>
/// <param name="mode">モード(0 or 1)</param>
/// <exception cref="System.Exception">You need to open a scan file.</exception>
/// <exception cref="System.IO.IOException">Invalid data.</exception>
/// <exception cref="TrackNotExistException">全てのtrackを完了し，次のtrackがない場合</exception>
/// <exception cref="IOException">データが不正であった場合</exception>
private void readNextTrack(int mode = 1)
{
short[] id = new short[Track.NumOfPlate];
short direction = new short();
char[] comment = new char[256];
double gx0 = new double(), gy0 = new double();
double gdxdz0 = new double(), gdydz0 = new double();
int status = 0;
double deltaZ = -emParameter.GelThick;
if (parameterManager.OpenedInFileMode == OpenInFileMode.None)
{
throw new Exception("You need to open a scan file.");
}
if (mode == 0)
{
status = Ipt.GetNextTrack(deltaZ, ref id[0], ref direction, ref gx0, ref gdxdz0, ref gy0, ref gdydz0, comment[0]);
}
else if (mode == 1)
{
status = Ipt.GetTrackOfId(deltaZ, ref id[0], ref direction, ref gx0, ref gdxdz0, ref gy0, ref gdydz0, comment[0]);
}
switch (status)
{
case -1: // Error
throw new IOException("Invalid data.");
case 0: // Scan finished.
Ipt.SendMessage("Whole scanning End!!");
string str = (mode == 0 ? "Scanning finished at all." : "Scan data dose not exists.");
throw new TrackNotExistException(str);
default:
Track t = new Track(numOfTracks, id, direction, gx0, gy0, gdxdz0, gdydz0, new String(comment));
tracks.Add(t);
presentTrackIndex = tracks.Count - 1;
break;
}
}
/// <summary>
/// LEDの明るさを自動調整します
/// </summary>
/// <param name="parameterManager">ParameterManagerのインスタンス</param>
/// <returns>調整後の明るさ</returns>
/// <exception cref="System.ArgumentException"></exception>
/// <exception cref="System.Exception"></exception>
public int AdjustLight(ParameterManager parameterManager)
{
//double xGrid = new double(), yGrid = new double();
int brightness = 0;
int brightness0 = parameterManager.EmulsionParameter.BasicBrightness;
int lightStlength = 0, goodLightStlength = 0, goodDiffrence = 255;
Camera camera = Camera.GetInstance();
camera.Stop();
try {
for (int i = 7; i >= 0; --i)
{
lightStlength += (brightness < brightness0 ? (int)Math.Pow(2, i) : -(int)Math.Pow(2, i));
DAout(lightStlength, parameterManager);
brightness = Ipt.GetAver();
if (Math.Abs(brightness - brightness0) < goodDiffrence)
{
goodLightStlength = lightStlength;
goodDiffrence = Math.Abs(brightness - brightness0);
}
}
if (goodDiffrence > 5)
{
//MotorControler mc = MotorControler.GetInstance();
//Vector3 stage = mc.GetPoint();
//Ipt.MtoG(1, stage.X, stage.Y, ref xGrid, ref yGrid);
App.logger.Error("Failed to LightAdjust");
}
DAout(goodLightStlength, parameterManager);
} catch (Exception ex) {
throw ex;
}
camera.Action = null;
camera.Start();
return(goodLightStlength);
}
/// <summary>
/// 初期化します
/// </summary>
/// <exception cref="System.Exception">ボードの初期化に失敗した場合</exception>
public void Initialize()
{
#if !_NoHardWare
try {
deviceId = 0;
int retCode = new int();
retCode = Ipt.Initialize(deviceId, ref retCode);
if (retCode == -1)
{
throw new Exception("Initializing VP910 was failed.");
}
Ipt.SetCameraType((int)Camera.CameraType);
deviceId = Ipt.GetDeviceId();
VP910Define.SelectCamera(deviceId, 0, (int)Camera.CameraType);
VP910Define.SetTrigerMode(deviceId, 2);
} catch (Exception) {
throw new Exception("Initializing VP910 was failed.");
}
#endif
}
private void task()
{
try {
MotorControler mc = MotorControler.GetInstance(parameterManager);
Surface sur = Surface.GetInstance(parameterManager);
Camera camera = Camera.GetInstance();
Led led = Led.GetInstance();
Vector3 initialpoint = mc.GetPoint();
camera.Stop();
string txtfileName = string.Format(@"C:\img\{0}_{1}.txt",
initialpoint.X,
initialpoint.Y);
StreamWriter twriter = File.CreateText(txtfileName);
double thickness = sur.UpTop - initialpoint.Z;
int npict = (int)(thickness / 0.002) + 3;
byte[] bb = new byte[440 * 512 * npict];
string datfileName = string.Format(@"C:\img\{0}_{1}.dat",
(int)(initialpoint.X),
(int)(initialpoint.Y)
);
BinaryWriter writer = new BinaryWriter(File.Open(datfileName, FileMode.Create));
string stlog = "";
int viewcounter = 0;
Vector3 p = new Vector3();
while (viewcounter < npict)
{
mc.MoveDistance(0.002, VectorId.Z);
mc.Join();
byte[] b = Ipt.CaptureMain();
p = mc.GetPoint();
stlog += String.Format("{0} {1} {2} {3} {4}\n",
System.DateTime.Now.ToString("HHmmss\\.fff"),
(p.X * 1000).ToString("0.0"),
(p.Y * 1000).ToString("0.0"),
(p.Z * 1000).ToString("0.0"),
viewcounter);
b.CopyTo(bb, 440 * 512 * viewcounter);
viewcounter++;
}
twriter.Write(stlog);
twriter.Close();
writer.Write(bb);
writer.Flush();
writer.Close();
GC.Collect();
GC.WaitForPendingFinalizers();
GC.Collect();
camera.Start();
} catch (SystemException) {
}
}//task
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()
{
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();
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>
/// ファイルから読み込んでエマルションのパラメータを初期化します．
/// </summary>
/// <param name="path">INIファイルへのパス</param>
/// <exception cref="System.IO.FileNotFoundException"></exception>
/// <exception cref="System.IO.FileLoadException"></exception>
/// <exception cref="System.Exception"></exception>
public void Load(string path)
{
string line;
string[] args;
try {
StreamReader sr = File.OpenText(path);
char[] delimiterChars = { ' ', '\t' };
_IniFile = path;
sr.ReadLine();
sr.ReadLine();
for (int i = 0; i < 2; ++i)
{
line = sr.ReadLine();
args = line.Split(delimiterChars, StringSplitOptions.RemoveEmptyEntries);
_BaseThick[i] = double.Parse(args[1]);
_GelThick[i] = double.Parse(args[2]);
_Brightness[i] = int.Parse(args[3]);
}
sr.ReadLine();
_DistanceBundleEm = double.Parse(sr.ReadLine());
sr.ReadLine();
line = sr.ReadLine();
args = line.Split(delimiterChars, StringSplitOptions.RemoveEmptyEntries);
_AutoScanAreaX = double.Parse(args[0]);
_AutoScanAreaY = double.Parse(args[1]);
_AutoScanAngleRange = double.Parse(args[2]);
sr.ReadLine();
sr.ReadLine();
line = sr.ReadLine();
args = line.Split(delimiterChars, StringSplitOptions.RemoveEmptyEntries);
_ThresholdBrightnessHit = int.Parse(args[0]);
_ThresholdBrightnessDust = int.Parse(args[1]);
_ThresholdDust[0] = int.Parse(args[2]);
_ThresholdDust[1] = int.Parse(args[3]);
_ThresholdSurface[0] = int.Parse(args[4]);
_ThresholdSurface[1] = int.Parse(args[5]);
_ThresholdGridBrightness = double.Parse(args[6]);
_BaseThresholdTrack[0] = int.Parse(args[7]);
_BaseThresholdTrack[1] = int.Parse(args[8]);
sr.ReadLine();
sr.ReadLine();
line = sr.ReadLine();
args = line.Split(delimiterChars, StringSplitOptions.RemoveEmptyEntries);
_GridOriginalX0 = double.Parse(args[0]);
_GridOriginalY0 = double.Parse(args[1]);
Ipt.SetThreshold(_ThresholdBrightnessHit, _ThresholdBrightnessDust,
_ThresholdDust[0], _ThresholdDust[1]);
// グリッドマーク関係のパラメータを設定
double[] markx = new double[9];
double[] marky = new double[9];
double distance = 0;
_GridOriginalSita = 0;
for (int i = 1; i < 9; ++i)
{
line = sr.ReadLine();
args = line.Split(delimiterChars, StringSplitOptions.RemoveEmptyEntries);
markx[i] = double.Parse(args[0]) - _GridOriginalX0;
marky[i] = double.Parse(args[1]) - _GridOriginalY0;
distance += Math.Sqrt(markx[i] * markx[i] + marky[i] * marky[i]);
_GridOriginalSita += (Math.Abs(markx[i]) > 5 ?
Math.Atan(marky[i] / markx[i]) : -Math.Atan(markx[i] / marky[i]));
}
// Grid mark間の距離を200とする
_GridOrigianlMag = distance / (4 * 100 * (1 + 1.41421356));
_GridOriginalSita /= 8;
_GridOriginalX[1, 1] = 0;
_GridOriginalY[1, 1] = 0;
double cosGridOrgSita = Math.Cos(_GridOriginalSita);
double sinGridOrgSita = Math.Sin(_GridOriginalSita);
for (int i = 1; i < 9; ++i)
{
double x = (markx[i] * cosGridOrgSita + marky[i] * sinGridOrgSita) / _GridOrigianlMag;
double y = (-markx[i] * sinGridOrgSita + marky[i] * cosGridOrgSita) / _GridOrigianlMag;
int ix = (int)(((x + 100) / 100) + 0.5);
int iy = (int)(((y + 100) / 100) + 0.5);
_GridOriginalX[ix, iy] = x;
_GridOriginalY[ix, iy] = y;
}
sr.Close();
_IniFile = path;
} catch (Exception ex) {
throw ex;
}
}
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
