/// <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); }
/// <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()); } }
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")); } }