public static PosXYZ AffineInverseTransform(TransformParams trans, PosXYZ pos) { return(new PosXYZ(RigidAlign.AffineInverseTransform(pos.Data(), trans.ToDoubles())) { Z = pos.Z }); }
public static PosXYZ AlignTransform(PosXYZ pos, double[,] trans) { return(new PosXYZ(RigidAlign.Transform(pos.Data(), trans)) { Z = pos.Z }); }
public static PosXYZ AffineTransform(PosXYZ pos, TransformParams trans) { return(new PosXYZ(CoordAlignHelper.AffineTransform(pos.Data(), trans.ToDoubles())) { Z = pos.Z }); }
public newRightMeasureDown(int id, string name, Station station) : base(id, name, station) { DoGTCylinder = Machine.Ins.Find <ICylinderEx>("RGTCylinder"); VioTransInp = Machine.Ins.Find <IVioEx>("RTransInp"); VioBarcodeFinish = Machine.Ins.Find <IVioEx>("RBarcodeFinish"); VioTransFinish = Machine.Ins.Find <IVioEx>("RTransFinishDown"); VioMeasureFinish = Machine.Ins.Find <IVioEx>("RMeasureFinishDown"); Platform = Machine.Ins.Find <PlatformEx>("RightDown"); Platform.SafeChecks.Clear(); Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.AutoHome)); Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.Auto)); Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.ManualHome)); Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.Manual)); var pToDown1 = new Func <double[], double[]>(d => { var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightUpTransform); pnew = XyzPlarformCalibration.AffineTransform(pnew, Machine.Ins.Settings.Calibration.RightTransform); return(pnew.Data()); }); var pToDown2 = new Func <double[], double[]>(d => { var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightUpTransform); pnew = XyzPlarformCalibration.AffineTransform(pnew, Machine.Ins.Settings.Calibration.RightTransform); pnew = pnew + Machine.Ins.Settings.Calibration.RightGtOffset; return(pnew.Data()); }); var down1ToP = new Func <double[], double[]>(d => { var pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, new PosXYZ(d)); pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightUpTransform, pnew); return(pnew.Data()); }); var down2ToP = new Func <double[], double[]>(d => { var pnew = new PosXYZ(d) - Machine.Ins.Settings.Calibration.RightGtOffset; pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, pnew); pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightUpTransform, pnew); return(pnew.Data()); }); var upToDown1 = new Func <double[], double[]>(d => { var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightTransform); return(pnew.Data()); }); var down1ToUp = new Func <double[], double[]>(d => { var pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, new PosXYZ(d)); return(pnew.Data()); }); var upToDown2 = new Func <double[], double[]>(d => { var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightTransform); pnew = pnew + Machine.Ins.Settings.Calibration.RightGtOffset; return(pnew.Data()); }); var down2ToUp = new Func <double[], double[]>(d => { var pnew = new PosXYZ(d) - Machine.Ins.Settings.Calibration.RightGtOffset; pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, pnew); return(pnew.Data()); }); Platform.PosConvertFuncs.Add("P->DOWN1", pToDown1); Platform.PosConvertFuncs.Add("P->DOWN2", pToDown2); Platform.PosConvertFuncs.Add("DOWN1->P", down1ToP); Platform.PosConvertFuncs.Add("DOWN2->P", down2ToP); Platform.PosConvertFuncs.Add("UP->DOWN1", upToDown1); Platform.PosConvertFuncs.Add("DOWN1->UP", down1ToUp); Platform.PosConvertFuncs.Add("UP->DOWN2", upToDown2); Platform.PosConvertFuncs.Add("DOWN2->UP", down2ToUp); }
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()); }