Пример #1
0
        /// <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);
        }
Пример #2
0
        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));
        }
Пример #3
0
        /// <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);
        }
Пример #4
0
        /// <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();
        }
Пример #5
0
        /// <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;
            }
        }