public override void UninitCalib()
        {
            do_gt2_cy?.SetDo(this, false, ignoreOrWaringOrError: null);

            if (Platform1 != null)
            {
                Platform1.EnterAuto(this).Home(2);
                Platform1.EnterAuto(this).MoveAbs("Wait", checkLimit: false);

                Platform1.ExitAuto();
            }


            if (Platform2 != null)
            {
                Platform2.EnterAuto(this).Home(2);
                Platform2.EnterAuto(this).MoveAbs("Wait", checkLimit: false);

                Platform2.ExitAuto();
            }


            PlatformCarrier?.EnterAuto(this).MoveAbs("Wait");

            do_clampy_cy.SetDo(this, false);

            PlatformCarrier?.ExitAuto();
        }
示例#2
0
        public override void UninitCalib()
        {
            do_clampy_cy?.SetDo(this, false);
            do_gt2_cy?.SetDo(this, false);

            Platform1?.EnterAuto(this).Jump("Wait", JumpHeight1);
            Platform2?.EnterAuto(this).Jump("Wait", JumpHeight2);

            PlatformCarrier?.EnterAuto(this).MoveAbs("Wait");
        }
示例#3
0
        public override void DoCalib()
        {
            Platform1?.EnterAuto(this).MoveAbs("Wait");


            //上工站点位数据采集
            Log($"上平台Align点位标定\n------------------------------------------------------");
            List <PosXYZ> upAlignPos = new List <PosXYZ>();
            bool          isFirst    = true;

            foreach (var pos in AlignPosUp)
            {
                if (isFirst)
                {
                    isFirst = false;
                    Platform1?.EnterAuto(this).Jump(pos, 0);
                }
                else
                {
                    Platform1?.EnterAuto(this).Jump(pos, JumpHeight1);
                }

                upAlignPos.Add(pos);
                DataList.Add(pos.ToString());
                Log($"CurPos {pos.ToString()}", LogLevel.Info);
            }
            //复位平台1
            Platform1?.EnterAuto(this).Jump("Wait", JumpHeight1);
            Log($"上平台Align点位标定 完成\n------------------------------------------------------");
            OnCalibProgress(50);



            Platform2?.EnterAuto(this).MoveAbs("Wait");
            //下工站点位 数据采集
            Log($"下平台Align点位标定\n------------------------------------------------------");
            List <PosXYZ> downAlingPos = new List <PosXYZ>();

            isFirst = true;
            foreach (var pos in AlignPosDown)
            {
                if (isFirst)
                {
                    isFirst = false;
                    Platform2?.EnterAuto(this).Jump(pos, 0);
                }
                else
                {
                    Platform2?.EnterAuto(this).Jump(pos, JumpHeight2);
                }

                downAlingPos.Add(pos);
                DataList.Add(pos.ToString());
                Log($"CurPos {pos.ToString()}", LogLevel.Info);
            }

            Log($"下平台Align点位标定 完成\n------------------------------------------------------");


            //计算下平台GT2偏移
            Log($"下平台GT1GT2偏移标定\n------------------------------------------------------");
            if (Platform2GtOffsetCalibGT1 != null && Platform2GtOffsetCalibGT2 != null)
            {
                Platform2?.EnterAuto(this).Jump(Platform2GtOffsetCalibGT1, JumpHeight1);
                Platform2?.EnterAuto(this).Jump(Platform2GtOffsetCalibGT2, JumpHeight1);

                var gt1 = Platform2GtOffsetCalibGT1 as PosXYZ;
                var gt2 = Platform2GtOffsetCalibGT2 as PosXYZ;
                OutputPlatform2GtOffset   = gt2 - gt1;
                OutputPlatform2GtOffset.Z = 0;
            }


            //复位平台2
            Platform2?.EnterAuto(this).Jump("Wait", JumpHeight2);
            Log($"下平台GT1GT2偏移标定 完成\n------------------------------------------------------");


            //复位治具
            PlatformCarrier?.EnterAuto(this).MoveAbs("Wait");
            OnCalibProgress(100);


            //计算上下平台坐标转换
            {
                var ret = XyzPlarformCalibration.CalcAffineTransform(
                    upAlignPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList(),
                    downAlingPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList());

                Log($"计算上下平台转换矩阵:\r\n"
                    + $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
                    + $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
                    + $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
                    + $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n", LogLevel.Info);

                Log($"计算上下平台转换误差:\r\n{ret.Item2:F2}\r\n", LogLevel.Info);

                OutputTransForm = new TransformParams(ret.Item1);
            }

            Log($"GT1GT2偏移:\r\n{OutputPlatform2GtOffset}\r\n", LogLevel.Info);
        }
        public override void DoCalib()
        {
            OutputStandardHeight = StandardHeight;

            Log("上平台GT高度标定\n----------------------------------------------------------");
            {
                //GT标定高度
                OutputGTCalibPos = Platform1GTCalibPos;
                DataList.Add(OutputGTCalibPos.ToString());

                //标准平面标定
                if (Platform1GTPlaneCalibPos != null)
                {
                    //update z
                    if (Platform1 != null && GtController != null)
                    {
                        bool isFirst = true;

                        foreach (var calibPos in Platform1GTPlaneCalibPos)
                        {
                            //product to up platform
                            var pos = new PosXYZ(calibPos.Data())
                            {
                                Z = Platform1GTCalibPos.Z
                            };
                            if (isFirst)
                            {
                                isFirst = false;
                                Platform1?.EnterAuto(this).Jump(Platform1.GetPos("P->UP", pos.Data()), 0);
                            }
                            else
                            {
                                Platform1?.EnterAuto(this).Jump(Platform1.GetPos("P->UP", pos.Data()), JumpHeight1);
                            }
                            Thread.Sleep(1000);
                            calibPos.Z = GtController.ReadData()[0];
                            DataList.Add(calibPos.ToString());
                        }
                    }

                    var fitplane = PlaneParams.FitPlane(Platform1GTPlaneCalibPos);
                    OutputUpStandardPlane = new PlaneParams()
                    {
                        Normal = fitplane.Normal, Origin = fitplane.Origin
                    };
                    DataList.Add(OutputUpStandardPlane.ToString());
                }
            }

            //复位上平台
            Platform1?.EnterAuto(this).Jump("Wait", JumpHeight1);
            Log("上平台GT高度标定 完成\n----------------------------------------------------------");


            //下GT高度标定

            //复位下平台Z轴
            if (Platform2 != null)
            {
                Platform2.EnterAuto(this).Home(2);
                Platform2.EnterAuto(this).MoveAbs(2, "Wait", checkLimit: false);
            }

            Log("下平台GT1高度标定\n----------------------------------------------------------");
            {
                do_gt2_cy?.SetDo(this, true, ignoreOrWaringOrError: null);
                {
                    //GT1GT2 高度差标定
                    OutputGT1CalibPos = Platform2GT1CalibPos;
                    //update z
                    if (Platform2 != null && GtController != null)
                    {
                        Platform2.EnterAuto(this).Jump(Platform2GT1CalibPos, 0);
                        Thread.Sleep(1000);
                        OutputGT1CalibPos.OffsetZ = GtController.ReadData()[1];
                    }

                    //GT1GT2 高度差标定
                    OutputGT2CalibPos = Platform2GT2CalibPos;
                    //update z
                    if (Platform2 != null && GtController != null)
                    {
                        Platform2.EnterAuto(this).Jump(Platform2GT2CalibPos, JumpHeight2);
                        Thread.Sleep(1000);
                        OutputGT2CalibPos.OffsetZ = GtController.ReadData()[2];
                    }


                    //GT1标准平面标定
                    if (Platform2GT1PlaneCalibPos != null)
                    {
                        //update z
                        if (Platform2 != null && GtController != null)
                        {
                            foreach (var calibPos in Platform2GT1PlaneCalibPos)
                            {
                                //product to up platform
                                var pos = new PosXYZ(calibPos.Data())
                                {
                                    Z = Platform2GT1CalibPos.Z
                                };
                                Platform2.EnterAuto(this).Jump(Platform2.GetPos("P->DOWN1", pos.Data()), JumpHeight2);
                                Thread.Sleep(1000);
                                calibPos.Z = GtController.ReadData()[1];
                                DataList.Add(calibPos.ToString());
                            }
                        }

                        var fitplane = PlaneParams.FitPlane(Platform2GT1PlaneCalibPos);
                        OutputDownStandardPlane = new PlaneParams()
                        {
                            Normal = fitplane.Normal, Origin = fitplane.Origin
                        };
                        DataList.Add(OutputDownStandardPlane.ToString());
                    }

                    //GT2标准平面标定
                    if (Platform2GT2PlaneCalibPos != null)
                    {
                        //update z
                        if (Platform2 != null && GtController != null)
                        {
                            foreach (var calibPos in Platform2GT2PlaneCalibPos)
                            {
                                //product to up platform
                                var pos = new PosXYZ(calibPos.Data())
                                {
                                    Z = Platform2GT2CalibPos.Z
                                };
                                Platform2.EnterAuto(this).Jump(Platform2.GetPos("P->DOWN2", pos.Data()), JumpHeight2);
                                Thread.Sleep(1000);
                                calibPos.Z = GtController.ReadData()[2];
                                DataList.Add(calibPos.ToString());
                            }
                        }

                        var fitplane = PlaneParams.FitPlane(Platform2GT2PlaneCalibPos);
                        OutputDownStandardPlane2 = new PlaneParams()
                        {
                            Normal = fitplane.Normal, Origin = fitplane.Origin
                        };
                        DataList.Add(OutputDownStandardPlane2.ToString());
                    }
                }
                do_gt2_cy?.SetDo(this, false, ignoreOrWaringOrError: null);
            }


            {
                //复位下平台
                //move platform2 wait
                if (Platform2 != null)
                {
                    Platform2.EnterAuto(this).Home(2);
                    Platform2.EnterAuto(this).MoveAbs("Wait", checkLimit: false);
                }
                Log("下平台GT1/2高度标定  完成\n----------------------------------------------------------");
            }

            //output
            DataList.Add(OutputGT1CalibPos.ToString());
            DataList.Add(OutputGT2CalibPos.ToString());
            DataList.Add(OutputStandardHeight.ToString());

            DataList.Add(OutputUpStandardPlane.ToString());
            DataList.Add(OutputDownStandardPlane.ToString());
        }