/// <summary> /// 计算单工位角度 /// </summary> /// <param name="index">工位号</param> /// <param name="rc">旋转中心</param> /// <param name="cameraNo">相机编号</param> /// <param name="disMark">mark间距</param> /// <param name="srcAngle">起始角度</param> /// <param name="dstAngle">终点角度</param> /// <param name="botAngle">相机朝上,铭牌和机器人Y轴的夹角(逆时针)</param> /// <returns></returns> public Point3D Calc(int index, Point2D rc, int cameraNo, double disMark, double srcAngle, double dstAngle, double botAngle) { Point3D pt3Result = new Point3D(); //数组索引从0开始 --index; try { //用于计算结果的工具类 FunCalibRotate funCalibRotate = new FunCalibRotate(); //计算当前位置与标定位置的角度差 double deltar = CalibDataMngr.instance.CurAngle(disMark, cameraNo) - CalibDataMngr.instance.CalibPos_L[index].DblValue4; //计算旋转之后的mark位置 Point2D dblMark1AfterR = funCalibRotate.GetPoint_AfterRotation(deltar / 180 * Math.PI, rc, CalibDataMngr.instance.pt2Mark1); //计算xy和标定位置的偏差 double dblDeltaX = dblMark1AfterR.DblValue1 - CalibDataMngr.instance.CalibPos_L[index].DblValue1; double dblDeltaY = dblMark1AfterR.DblValue2 - CalibDataMngr.instance.CalibPos_L[index].DblValue2; //根据当前角度和目标位置角度计算实际偏差 Point2D delta = TransDelta(new Point2D(dblDeltaX * AMP, dblDeltaY * AMP), srcAngle, dstAngle); //给计算结果赋值 pt3Result.DblValue1 = delta.DblValue1; pt3Result.DblValue2 = delta.DblValue2; pt3Result.DblValue3 = deltar; } catch (Exception ex) { Log.L_I.WriteError(NameClass, ex); } return(pt3Result); }
public bool CalibRotateCenter(string cellName, Point2D pt2Mark1, Point2D pt2Mark2, double rotateAngle, BaseParComprehensive parComprehensive) { try { //获取旋转中心cell BaseParCalibrate baseParComprehensive = parComprehensive.GetCellParCalibrate(cellName); //获取旋转中心算子 ParCalibRotate parCalibRotate = (ParCalibRotate)baseParComprehensive; //根据参数求旋转中心 Point2D rc = new FunCalibRotate().GetOriginPoint(rotateAngle, pt2Mark1, pt2Mark2); //把旋转中心存入算子 parCalibRotate.XRC = rc.DblValue1; parCalibRotate.YRC = rc.DblValue2; //讲计算结果写入xml parComprehensive.WriteXmlDoc(cellName); //将参数保存到结果类 new FunCalibRotate().SaveParToResult(HtResult_Cam1, parCalibRotate); //输出计算结果 string info = string.Format("相机{0}旋转中心标定完成,X:{1},Y:{2}", g_NoCamera, parCalibRotate.XRC.ToString(), parCalibRotate.YRC.ToString()); ShowState(info); return(true); } catch (Exception ex) { Log.L_I.WriteError(NameClass, ex); return(false); } }
/// <summary> /// 偏差计算,取deltaX进行角度计算 /// </summary> /// <param name="mark1">旋转起始点</param> /// <param name="mark2">旋转终止点</param> /// <param name="ratio">相机系数</param> /// <param name="disMark">mark间距</param> /// <param name="cellName">算子序号</param> /// <param name="index">对应的基准值索引</param> /// <param name="baseParComprehensive">相机综合设置算子序列</param> /// <param name="pt3Result">计算结果</param> /// <returns></returns> public static bool CalcDeviationY(Point2D mark1, Point2D mark2, double ratio, double disMark, string cellName, int index, BaseParComprehensive baseParComprehensive, out double[] pt3Result) { pt3Result = new double[3]; try { //用于计算结果的工具类 FunCalibRotate funCalibRotate = new FunCalibRotate(); //计算当前位置与标定位的角度 double deltar = GetCurAngleByY(disMark, ratio, mark1, mark2); //算上基准值的角度,本机只用到pos0/1 Point4D calibPos = CalibDataMngr.instance.CalibPos_L[index]; deltar -= calibPos.DblValue4; //计算旋转之后的mark位置,此处用第二点计算 if (!GetPtAfterRotate(mark2, -deltar, cellName, baseParComprehensive, out Point2D result)) { return(false); } pt3Result = new double[] { Math.Round((calibPos.DblValue1 - result.DblValue1) * ratio, 4), Math.Round((calibPos.DblValue2 - result.DblValue2) * ratio, 4), Math.Round(deltar, 4) }; } catch (Exception ex) { Log.L_I.WriteError(ClassName, ex); return(false); } return(true); }
public void DualLocation(int index) { if (Camera1Done & Camera2Done) { ShowState("开始计算工位" + index + "偏差"); BaseParCalibrate baseParComprehensive = ParComprehensive2.P_I.GetCellParCalibrate(Camera2RC); ParCalibRotate parCalibRotate = (ParCalibRotate)baseParComprehensive; double angle = Math.Atan( (Pt2Mark2.DblValue2 - Pt2Mark1.DblValue2) * AMP / Protocols.confDisMark) * 180 / Math.PI - StationDataMngr.CalibPos_L[index - 1].DblValue4; ShowState("工位" + index + "逆时针角度偏差: " + angle); FunCalibRotate fcr = new FunCalibRotate(); Point2D MarkAfterRotate = fcr.GetPoint_AfterRotation( angle / 180 * Math.PI, parCalibRotate.PointRC, Pt2Mark2); double deltaY = MarkAfterRotate.DblValue1 - StationDataMngr.CalibPos_L[index - 1].DblValue1; double deltaX = MarkAfterRotate.DblValue2 - StationDataMngr.CalibPos_L[index - 1].DblValue2; deltaX *= AMP; deltaY *= -AMP; Point2D delta = TransDelta(new Point2D(deltaX, deltaY), Protocols.ConfPlaceAngle, Protocols.ConfPreciseAngle); ShowState(string.Format("工位{0}X方向补偿{1},Y方向补偿{2}", index, delta.DblValue1.ToString(ReserveDigits), delta.DblValue2.ToString(ReserveDigits))); int num = (StationNum - 1) * 2 + index; Point4D pos = new Point4D { DblValue1 = delta.DblValue1 + StationDataMngr.PlacePos_L[index - 1].DblValue1 + ParAdjust.Value1("adj" + num), DblValue2 = delta.DblValue2 + StationDataMngr.PlacePos_L[index - 1].DblValue2 + ParAdjust.Value2("adj" + num), DblValue3 = StationDataMngr.PlacePos_L[index - 1].DblValue3, DblValue4 = Protocols.ConfPlaceAngle + angle + ParAdjust.Value3("adj" + num) }; LogicRobot.L_I.WriteRobotCMD(pos, Protocols.BotCmd_StationPos); ShowState("发送机器人放片坐标:" + pos.DblValue1.ToString("f3") + @"/" + pos.DblValue2.ToString("f3")); } }
public static bool GetPtAfterRotate(Point2D pt, double r, string cellName, BaseParComprehensive parComprehensive, out Point2D result) { result = new Point2D(); try { if (!GetRcFromPar(cellName, parComprehensive, out Point2D rc)) { return(false); } result = new FunCalibRotate().GetPoint_AfterRotation(-r / 180 * Math.PI, rc, pt); } catch (Exception ex) { Log.L_I.WriteError(ClassName, ex); return(false); } return(true); }
/// <summary> /// 背光一次定位偏差计算 /// </summary> /// <param name="pt2Src">像素坐标,精定位处获取的计算结果</param> /// <param name="pt2Rc">旋转中心</param> /// <param name="r">角度补偿,即需要旋转的角度</param> /// <param name="ratio">相机系数</param> /// <param name="type">背光方向</param> /// <param name="srcAngle">精定位处玻璃角度</param> /// <param name="dstAngle">目标点玻璃角度</param> /// <param name="botAngle">机器人放置角度</param> /// <param name="displayAngle">画面显示角度</param> /// <param name="blAngle">背光角度</param> public static void Verify(Point2D pt2Src, Point2D pt2Rc, double r, double ratio, BackLightDisplay_Enum type, double srcAngle, double dstAngle, double botAngle, double displayAngle, double blAngle) { try { r = GetDeltaR(r, type); Point2D pt2AfterR = new FunCalibRotate().GetPoint_AfterRotation(r / 180 * Math.PI, pt2Rc, pt2Src); Point2D delta = pt2AfterR - pt2Rc; delta = GetDeviationForBot(delta, botAngle, displayAngle, blAngle, srcAngle, dstAngle); delta = new Point2D(delta.DblValue1 * ratio, delta.DblValue2 * ratio); ShowState(string.Format("一次拍照计算的偏差结果:X:{0}/Y:{1}", delta.DblValue1.ToString(ReservDigits), delta.DblValue2.ToString(ReservDigits))); } catch (Exception ex) { Log.L_I.WriteError(ClassName, ex); } }
public void CalibRC() { Thread.Sleep(500); //double curR= ModuleBase.GetCurAngleByY(Protocols.GlassXInPresize, AMP, Pt2Mark1, Pt2MarkRC); BaseParCalibrate baseParComprehensive = ParComprehensive2.P_I.GetCellParCalibrate(Camera2RC); ParCalibRotate parCalibRotate = (ParCalibRotate)baseParComprehensive; Point2D orgPoint = new FunCalibRotate().GetOriginPoint(Protocols.BotRCCalibAngle, Pt2Mark2, Pt2MarkRC); ShowState("计算旋转中心X:" + orgPoint.DblValue1 + ",Y:" + orgPoint.DblValue2); Point2D offset = new Point2D(Protocols.GlassXInPresize / 2 / AMP, Protocols.GlassYInPresize / 2 / AMP); double r = ModuleBase.GetCurAngleByY(Protocols.GlassXInPresize, AMP, Pt2Mark1, Pt2Mark2); offset = TransDelta(offset, r + 0.5, 0); Point2D rc = new Point2D(offset.DblValue1 + Pt2Mark2.DblValue1, offset.DblValue2 + Pt2Mark2.DblValue2); ShowState("理论旋转中心X:" + rc.DblValue1 + ",Y:" + rc.DblValue2); if (Math.Abs(orgPoint.DblValue1 - rc.DblValue1) < 1000 && Math.Abs(orgPoint.DblValue2 - rc.DblValue2) < 1000) { ShowState("使用计算旋转中心"); parCalibRotate.XRC = orgPoint.DblValue1; parCalibRotate.YRC = orgPoint.DblValue2; } else { ShowState("使用理论旋转中心"); parCalibRotate.XRC = rc.DblValue1; // orgPoint.DblValue1; parCalibRotate.YRC = rc.DblValue2; // orgPoint.DblValue2; } ParComprehensive2.P_I.WriteXmlDoc(Camera2RC); //将参数保存到结果类 new FunCalibRotate().SaveParToResult(HtResult_Cam2, parCalibRotate); ShowState(string.Format("旋转中心标定完成,X_{0},Y_{1}", parCalibRotate.XRC.ToString(), parCalibRotate.YRC.ToString())); LogicRobot.L_I.WriteRobotCMD(Protocols.BotCmd_CalibRC); }
/// <summary> /// 将视觉偏差转换为最终偏差 /// </summary> /// <returns></returns> public Point2D GetRealDelta() { try { FunCalibRotate fun = new FunCalibRotate(); //角度分两块 1 精确定位到残边平台 //2 残边平台在转移到机器人坐标系 //相当于将一组偏差转两次,设置好对应关系后相加即可 //先得到实际坐标系偏差 DeltaReal = fun.GetPoint_AfterRotation( AnglePreciToITOPlat / 180 * Math.PI, new Point2D(0, 0), DeltaVision); DeltaRobot = fun.GetPoint_AfterRotation( ((double)ParSTDArrange.P_I.TypeRobotCoor_E) / 180 * Math.PI, new Point2D(0, 0), DeltaReal); //认为机器人的偏差位最终偏差 return(DeltaReal); } catch (Exception ex) { Log.L_I.WriteError("MainSTDArrangeProd.GetRealDelta", ex); return(new Point2D()); } }
/// <summary> /// 自动校准旋转中心,1401表示位置1第一次拍照, /// </summary> /// <param name="pos">算子序号,不表示拍照位置</param> /// <param name="index"></param> /// <param name="htResult">哈希表结果</param> /// <returns></returns> bool CalibRotate(int pos, int index, out Hashtable htResult) { htResult = null; Stopwatch sw = new Stopwatch(); try { //获取该算子所有的单元格 List <string> nameCell_L = g_BaseParComprehensive.GetAllNameCellByType("旋转中心变换"); if (nameCell_L.Count < pos) { WinMsgBox.ShowMsgBox("算子个数小于序号!"); return(false); } string nameCell = nameCell_L[pos - 1];//单元格名称 ParCalibRotate par = g_BaseParComprehensive.GetCellParCalibrate(nameCell) as ParCalibRotate; int indexCell = int.Parse(nameCell.Replace("C", ""));//单元格索引 ParCellExeReference parCellExeReference = par.g_ParCellExecuteReference; ParGetResultFromCell parGetResultFromCell = par.g_ParGetResultFromCellForRC;//用来计算旋转中心 //调用执行 g_BaseDealComprehensive.DealComprehensivePosCell_ForTestRun(g_UCDisplayCamera, parCellExeReference, par.g_ParCellExecuteReference.g_CellExecute_L, out htResult);//执行算法且显示 string cellError = ""; if (!GetErrorCell(htResult, pos, out cellError)) { WinMsgBox.ShowMsgBox(string.Format("相机{0}位置{1},单元格{2}校准计算错误", g_NoCamera.ToString(), pos.ToString(), par.NameCell)); return(false); } FunCellDataReferenc fun = new FunCellDataReferenc(); fun.GetResultValue(htResult, parGetResultFromCell, index - 1); string info = string.Format("相机{0}位置{1},单元格{2}", g_NoCamera.ToString(), pos.ToString(), par.NameCell); if (index == 1) { info += "旋转中心校准第一次拍照"; ShowState(info); SaveStdImage(par, "旋转中心位置1"); } else if (index == 2) { info += "旋转中心校准第二次拍照"; ShowState(info); SaveStdImage(par, "旋转中心位置2"); FunCalibRotate funCalibRotate = new FunCalibRotate(); Point2D point = funCalibRotate.GetOriginPoint(par); par.XRC = point.DblValue1; par.YRC = point.DblValue2; //显示求取的旋转中心 info = string.Format("旋转中心:X{0},Y{1}", par.XRC.ToString(), par.YRC.ToString()); ShowState(info); //保存此单元格 bool blSave = g_BaseParComprehensive.WriteXmlDoc(par.NameCell); if (blSave) { ShowState(string.Format("将参数旋转中心校准{0}保存到本地成功", par.NameCell)); } else { ShowAlarm(string.Format("将参数旋转中心校准{0}保存到本地失败", par.NameCell)); return(false); } //将参数保存到结果中 bool blSaveResult = g_BaseDealComprehensive.InitCalibRotate(indexCell); if (blSaveResult) { ShowState(string.Format("将参数旋转中心校准{0}保存到结果成功", par.NameCell)); } else { ShowAlarm(string.Format("将参数旋转中心校准{0}保存到结果失败", par.NameCell)); return(false); } } return(true); } catch (Exception ex) { ShowAlarm(string.Format("相机{0}旋转中心计算进入Catch", g_NoCamera.ToString())); Log.L_I.WriteError(NameClass, ex); return(false); } }