/// <summary>
/// 与えられたエンコーダ座標系の地点から最も近いグリッドマーク情報を取得します。
/// </summary>
/// <param name="point">エンコーダ座標系の座標</param>
/// <returns>最も近いグッドマークをエンコーダ座標系で</returns>
public GridMark GetTheNearestGridMark(Vector3 encoderPoint)
{
GridParameter gridParam = parameterManager.GridParameter;
if (gridParam.LoadedGridOriginalFine == false)
{
throw new Exception("null");
}
GridMark retval = new GridMark();
Vector2 pmover = new Vector2();
//Vector2 gmover = new Vector2();
Vector2 gstage = new Vector2();
try {
double[,] gridOriginalFineX = gridParam.GridOriginalFineX;
double[,] gridOriginalFineY = gridParam.GridOriginalFineY;
double minDistance = 99999999.9;
for (int ix = 0; ix < gridOriginalFineX.GetLength(0); ++ix)
{
for (int iy = 0; iy < gridOriginalFineX.GetLength(1); ++iy)
{
double distanceX = gridOriginalFineX[ix, iy] - encoderPoint.X;
double distanceY = gridOriginalFineY[ix, iy] - encoderPoint.Y;
double distance = Math.Sqrt(distanceX * distanceX + distanceY * distanceY);
if (distance < minDistance)
{
minDistance = distance;
retval.x = gridOriginalFineX[ix, iy];
retval.y = gridOriginalFineY[ix, iy];
}
}
}
} catch {
throw new Exception("null");
}
//Ipt.MtoG(0, gmover.X, gmover.Y, ref gstage.X, ref gstage.Y);
Ipt.GToM("p", retval.x, retval.y, ref gstage.X, ref gstage.Y);
retval.x = gstage.X;
retval.y = gstage.Y;
return(retval);
}
public void readMagTheta()
{
string datarootdirpath = string.Format(@"C:\test");
string line;
System.IO.StreamReader file = new System.IO.StreamReader(datarootdirpath + @"\mag_theta.txt");
line = file.ReadLine();
string[] data = line.Split(' ');
magnitOfGrid = double.Parse(data[0]);
angleOfGrid = double.Parse(data[1]);
file.Close();
Ipt.SetGridLocal(
(short)parameterManager.PlateNo,
magnitOfGrid,
angleOfGrid,
parameterManager.EmulsionIndexUp,
parameterManager.EmulsionIndexDown);
System.Diagnostics.Debug.WriteLine(String.Format("mag: {0}, theta: {1}", magnitOfGrid, angleOfGrid));
}
/// <summary>
/// 座標の補正値を算出して，返します．
/// </summary>
/// <returns>座標の補正値</returns>
private Vector2 calcOffset()
{
Vector2 offset = new Vector2();
Vector2Int iId = new Vector2Int();
double emX = new double();
double emY = new double();
// 定義されたグリッドマーク数が少ない場合は例外を返す
int num = DefinedGridMarkNum;
if (num < 2)
{
throw new Exception("Grid mark data is few(" + num.ToString() + ").");
}
if (gridMarks[(int)GridMarkPoint.CenterMiddle].Existed)
{
offset.X = gridMarks[(int)GridMarkPoint.CenterMiddle].x;
offset.Y = gridMarks[(int)GridMarkPoint.CenterMiddle].y;
}
else
{
for (int i = 1; i < AllGridMarksNum; ++i)
{
if (gridMarks[i].Existed)
{
iId = getXYGridId(i);
Ipt.GToM("0", gridOrgX[iId.X, iId.Y], gridOrgY[iId.X, iId.Y], ref emX, ref emY);
offset.X += gridMarks[i].x - emX;
offset.Y += gridMarks[i].y - emY;
}
}
offset.X /= DefinedGridMarkNum;
offset.Y /= DefinedGridMarkNum;
}
return(offset);
}
/// <summary>
/// グリッドの大きさと角度を定義されたグリッドマークから算出します．
/// </summary>
/// <exception cref="System.Exception">定義されたグリッドマーク数が少ない場合</exception>
private void setAngleAndMagnitOfGrid()
{
double distOrigin = 0, distNow = 0;
double thetaOrigin = 0, thetaNow = 0, theta = 0;
double dx, dy;
Vector2Int iId, jId;
int combinationNum = 0;
// 定義されたグリッドマーク数が少ない場合は例外を返す
int num = DefinedGridMarkNum;
if (num < 2)
{
throw new Exception("Grid mark data is few(" + num.ToString() + ").");
}
for (int i = 0; i < AllGridMarksNum; ++i)
{
for (int j = i + 1; j < AllGridMarksNum; ++j)
{
if (gridMarks[i].Existed)
{
dx = gridMarks[i].x - gridMarks[j].x;
dy = gridMarks[i].y - gridMarks[j].y;
distNow += Math.Sqrt(dx * dx + dy * dy);
thetaNow = (Math.Abs(dx) > 5 ? Math.Atan(dy / dx) : -Math.Atan(dx / dy));
iId = getXYGridId(i);
jId = getXYGridId(j);
dx = gridOrgX[iId.X, iId.Y] - gridOrgX[jId.X, jId.Y];
dy = gridOrgY[iId.X, iId.Y] - gridOrgY[jId.X, jId.Y];
distOrigin += Math.Sqrt(dx * dx + dy * dy);
thetaOrigin = (Math.Abs(dx) > 5 ? Math.Atan(dy / dx) : -Math.Atan(dx / dy));
theta += (thetaNow - thetaOrigin);
++combinationNum;
} // if END
} // for j END
} // for i END
magnitOfGrid = distNow / distOrigin;
angleOfGrid = theta / combinationNum;
Ipt.SetGridLocal(
(short)parameterManager.PlateNo,
magnitOfGrid,
angleOfGrid,
parameterManager.EmulsionIndexUp,
parameterManager.EmulsionIndexDown);
System.Diagnostics.Debug.WriteLine(String.Format("mag: {0}, theta: {1}", magnitOfGrid, angleOfGrid));
string datarootdirpath = string.Format(@"C:\test");
System.IO.DirectoryInfo mydir = System.IO.Directory.CreateDirectory(datarootdirpath);
string txtfileName_grid = datarootdirpath + string.Format(@"\mag_theta.txt");
StreamWriter twriter_grid = File.CreateText(txtfileName_grid);
twriter_grid.WriteLine("{0} {1}", magnitOfGrid, angleOfGrid);
twriter_grid.Close();
string txtfileName_grid3x3 = datarootdirpath + string.Format(@"\stagecoord_grid3x3.txt");
StreamWriter twriter_grid3x3 = File.CreateText(txtfileName_grid3x3);
for (int i = 0; i < AllGridMarksNum; i++)
{
twriter_grid3x3.WriteLine("{0} {1}", gridMarks[i].x, gridMarks[i].y);
}
twriter_grid3x3.Close();
}
/// <summary>
/// グリッドマークを基準に座標系を作成します．
/// </summary>
/// <exception cref="Exception">定義されたグリッドマーク数が少ない場合</exception>
public void CreateCoordSystem()
{
double x = new double();
double y = new double();
double emX, emY;
Vector2Int iId;
Vector2 offset;
try {
setAngleAndMagnitOfGrid();
offset = calcOffset();
} catch (Exception ex) {
throw ex;
}
// 原点を設定
gridOffsetX[1, 1] = 0;
gridOffsetY[1, 1] = 0;
for (int i = 1; i < AllGridMarksNum; ++i)
{
iId = getXYGridId(i);
if (gridMarks[i].Existed)
{
emX = gridMarks[i].x - offset.X;
emY = gridMarks[i].y - offset.Y;
Ipt.MtoG(0, emX, emY, ref x, ref y);
iId.X = (int)(((x + 100) / 100) + 0.5);
iId.Y = (int)(((y + 100) / 100) + 0.5);
gridOffsetX[iId.X, iId.Y] = x - gridOrgX[iId.X, iId.Y];
gridOffsetY[iId.X, iId.Y] = y - gridOrgY[iId.X, iId.Y];
}
else
{
gridOffsetX[iId.X, iId.Y] = 0;
gridOffsetX[iId.X, iId.Y] = 0;
}
}
/* 新しい原点のためにモータコントロールボードを初期化する．*/
// 新しい原点に移動を開始し，別スレッドで移動完了を感知したら，
// モータコントロールボードを初期化し原点を再設定する
MotorControler mc = MotorControler.GetInstance();
mc.MovePointXY(offset.X, offset.Y);
Thread thread = new Thread(new ParameterizedThreadStart(delegate(object o) {
MotorControler motor = MotorControler.GetInstance();
while (motor.IsMoving)
{
// モータの移動中は待機
Thread.Sleep(10);
}
// モータコントロールボードを初期化し，原点を設定する
//motor.InitializeMotorControlBoard(MechaAxisAddress.XAddress);
//motor.InitializeMotorControlBoard(MechaAxisAddress.YAddress);
//motor.InitializeMotorControlBoard(MechaAxisAddress.ZAddress);
coordDefined = true;
}));
try {
thread.Start();
} catch (Exception ex) {
throw ex;
}
}