Beispiel #1
0
        public override XYUPoint GetObjPonit(XYUPoint objVisoionpoint, XYUPoint SanpMachinePoint)
        {
            XYUPoint ObjPoint = new XYUPoint();
            HTuple   qx, qy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Dutr, objVisoionpoint.x, objVisoionpoint.y, out qx, out qy);
            ObjPoint.x = SanpMachinePoint.x - qx[0].D;
            ObjPoint.y = SanpMachinePoint.y - qy[0].D;
            return(ObjPoint);
        }
Beispiel #2
0
        public XYUPoint GetDispOrLaserPoint(XYUPoint OldMachineModlePos, XYUPoint NowMachineModlePos, XYUPoint OldPinPos)
        {
            HOperatorSet.VectorAngleToRigid(OldMachineModlePos.x, OldMachineModlePos.y, 0, NowMachineModlePos.x, NowMachineModlePos.y, NowMachineModlePos.u, out HTuple hom2d);
            HTuple qx, qy;

            HOperatorSet.AffineTransPoint2d(hom2d, OldPinPos.x, OldPinPos.y, out qx, out qy);
            XYUPoint NowPoint = new XYUPoint(qx[0].D, qy[0].D, NowMachineModlePos.u);

            return(NowPoint);
        }
Beispiel #3
0
        public override XYUPoint GetObjPonit(XYUPoint objVisoionpoint, XYUPoint SanpMachinePoint)
        {
            HTuple qx, qy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Dutr, objVisoionpoint.x, objVisoionpoint.y, out qx, out qy);
            XYUPoint dstpoint = new XYUPoint();

            dstpoint.x = SanpMachinePoint.x - qx + AidPoint.x;
            dstpoint.y = SanpMachinePoint.y - qy + AidPoint.y;
            return(dstpoint);
        }
Beispiel #4
0
        public override XYUPoint GetObjPonit(XYUPoint objVisoionpoint, XYUPoint SanpMachinePoint)
        {
            XYUPoint objMachinePos = new XYUPoint();
            HTuple   hTuplex, hTupley;
            HTuple   qx, qy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, objVisoionpoint.x, objVisoionpoint.y, out qx, out qy);
            objMachinePos.x = qx[0].D;
            objMachinePos.y = qy[0].D;
            return(objMachinePos);
        }
Beispiel #5
0
 public void Init()
 {
     offSetX = 0;
     offSetY = 0;
     offSetU = 0;
     highZ   = 0;
     NozzleCalibVisionCoor = new Point2d(0, 0);
     //吸嘴标定Robot坐标
     NozzleCalibRobotCoor        = new XYUPoint(0, 0, 0);
     IsCounterclockwise_Rotation = true;
     u0degModle = 0;
 }
Beispiel #6
0
        public override XYUPoint GetDstPonit(XYUPoint dstVisoionpoint, XYUPoint SanpMachinePoint)
        {
            XYUPoint dstMachinePos = new XYUPoint();
            HTuple   hTuplex, hTupley;
            HTuple   qx, qy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Dutd, dstVisoionpoint.x, dstVisoionpoint.y, out hTuplex, out hTupley);
            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, hTuplex, hTupley, out qx, out qy);
            XYUPoint vec = new XYUPoint();

            vec.x = SanpMachinePoint.x - CalibPoint.x;
            vec.y = SanpMachinePoint.y - CalibPoint.y;

            dstMachinePos.x = qx[0].D + vec.x;
            dstMachinePos.y = qy[0].D + vec.y;
            return(dstMachinePos);
        }
Beispiel #7
0
        /// <summary>
        /// 获取对应Laser点和Pin的 现况下的位置
        /// </summary>
        /// <param name="OldVisoionModlePos">模板视觉坐标位置</param>
        /// <param name="NowVisionModlePos">现在匹配模板视觉坐标位置</param>
        /// <param name="OldSnapModlePos">模板拍照机械位置</param>
        /// <param name="OldPinMachinePos">开始设置的Pin机械位置</param>
        /// <param name=""></param>
        /// <returns></returns>

        public XYUPoint GetDispOrLaserPoint2(XYUPoint OldVisoionModlePos, XYUPoint NowVisionModlePos, XYUPoint OldSnapModlePos, XYUPoint OldPinMachinePos, XYUPoint NowSnapMchinePos)
        {
            HOperatorSet.VectorAngleToRigid(OldVisoionModlePos.x, OldVisoionModlePos.y, OldVisoionModlePos.u, NowVisionModlePos.x, NowVisionModlePos.y, NowVisionModlePos.u, out HTuple hom2d);

            HTuple   vx, vy;
            XYUPoint machineXY = new XYUPoint(OldSnapModlePos.x - OldPinMachinePos.x, OldSnapModlePos.y - OldPinMachinePos.y, 0);

            HOperatorSet.AffineTransPoint2d(m_Hom2Drtu, machineXY.x, machineXY.y, out vx, out vy);

            HTuple qvx, qvy;

            HOperatorSet.AffineTransPixel(hom2d, vx, vy, out qvx, out qvy);

            XYUPoint NowPinMachinePoint = GetDstPonit(new XYUPoint(qvx, qvy, 0), NowSnapMchinePos);


            return(NowPinMachinePoint);
        }
Beispiel #8
0
        /// <summary>
        /// 获取 吸嘴中新对准点
        /// </summary>
        /// <param name="snappos"></param>
        /// <param name="u0degModle"></param>
        /// <param name="Point2dVisionCoor"></param>
        /// <returns></returns>
        public XYUPoint GetPosWhenUpCameraSnap(int nIndex, XYUPoint snappos, XYUPoint Point2dVisionCoor)
        {
            XYUPoint posull = new XYUPoint(double.MaxValue, double.MaxValue, double.MaxValue);

            if (nIndex > resultCalibMulitNozzle.nozzleSturts.Count || nIndex < 0)
            {
                return(posull);
            }

            XYUPoint NozzleCalibRobotCoor        = resultCalibMulitNozzle.nozzleSturts[nIndex].NozzleCalibRobotCoor;
            Point2d  NozzleCalibVisionCoor       = resultCalibMulitNozzle.nozzleSturts[nIndex].NozzleCalibVisionCoor;
            double   u0degModle                  = resultCalibMulitNozzle.nozzleSturts[nIndex].u0degModle;
            bool     IsCounterclockwise_Rotation = resultCalibMulitNozzle.nozzleSturts[nIndex].IsCounterclockwise_Rotation;
            // u0degModle 0 度模板(做模板) u的角度
            HTuple qx, qy;

            HTuple dsqx, dsqy;
            HTuple dcqx, dcqy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, snappos.x, snappos.y, out dsqx, out dsqy);
            HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, CalibPos.x, CalibPos.y, out dcqx, out dcqy);

            HTuple calbx, calby;

            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, dcqx, dcqy, out calbx, out calby);
            HOperatorSet.AffineTransPoint2d(m_Hom2Dutd, Point2dVisionCoor.x, Point2dVisionCoor.y, out qx, out qy);

            HTuple vecx, vecy;

            vecx = dsqx - dcqx;
            vecy = dsqy - dcqy;


            //mark 坐标
            HTuple dmarkx, dmarky;

            dmarkx = qx + vecx;
            dmarky = qy + vecy;
            HTuple rmarkx, rmarky;

            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, dmarkx, dmarky, out rmarkx, out rmarky);
            if (Point2dVisionCoor.u > Math.PI)
            {
                Point2dVisionCoor.u = Point2dVisionCoor.u - 2 * Math.PI;
            }
            else if (Point2dVisionCoor.u < -Math.PI)
            {
                Point2dVisionCoor.u = Point2dVisionCoor.u + 2 * Math.PI;
            }
            //旋转轴的物理 角度
            HTuple tupleAngle = 0;

            if (!IsCounterclockwise_Rotation)// 旋转轴 正向 逆时针 IsCounterclockwise_Rotation=ture
            {
                tupleAngle = u0degModle - 180.000 * Point2dVisionCoor.u / Math.PI;
            }
            else
            {
                tupleAngle = u0degModle + 180.000 * Point2dVisionCoor.u / Math.PI;
            }
            HTuple rqx, rqy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, qx, qy, out rqx, out rqy);
            XYUPoint post = new XYUPoint();

            post.x = rqx + snappos.x - CalibPos.x;
            post.y = rqy + snappos.y - CalibPos.y;

            HTuple vecNx, vecNy;
            HTuple dcnqx, dcnqy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, NozzleCalibRobotCoor.x, NozzleCalibRobotCoor.y, out dcnqx, out dcnqy);
            vecNx = NozzleCalibVisionCoor.x - dcnqx;
            vecNy = NozzleCalibVisionCoor.y - dcnqy;
            HTuple lens = Math.Sqrt(vecNx.D * vecNx.D + vecNy.D * vecNy.D);

            //需要旋转的角度
            HTuple AngleRote = tupleAngle - snappos.u;

            HTuple tupleAnglecalib = 0;

            HOperatorSet.AngleLx(dcnqy, dcnqx, NozzleCalibVisionCoor.y, NozzleCalibVisionCoor.x, out tupleAnglecalib);
            //  HOperatorSet.AngleLl(0,0,0,100,dcnqy, dcnqx, NozzleCalibVisionCoor.y, NozzleCalibVisionCoor.x, out tupleAnglecalib);
            HTuple destangleandR = 0;

            if (!IsCounterclockwise_Rotation)
            {
                destangleandR = tupleAnglecalib - Math.PI * (tupleAngle - NozzleCalibRobotCoor.u) / 180.0000;
            }
            else
            {
                destangleandR = tupleAnglecalib + Math.PI * (tupleAngle - NozzleCalibRobotCoor.u) / 180.0000;
            }
            HTuple Destnozzledx = dsqx + Math.Cos(destangleandR.D) * lens;
            HTuple Destnozzledy = dsqy - Math.Sin(destangleandR.D) * lens;
            HTuple rnozzlex, rnozzley;

            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, Destnozzledx, Destnozzledy, out rnozzlex, out rnozzley);


            HTuple offsetx = rmarkx - rnozzlex;
            HTuple offsety = rmarky - rnozzley;

            XYUPoint pos = new XYUPoint(snappos.x + offsetx.D, snappos.y + offsety.D, tupleAngle);

            return(pos);
        }
Beispiel #9
0
        /// <summary>
        ///   上相机看目标 下相机看对象,机械手拿料移动组装
        /// </summary>
        /// <param name="snappos"></param>
        /// <param name="Point2dVisionCoorDst"></param>
        /// <param name="Point2dVisionCoorObj"></param>
        /// <returns></returns>
        public XYUPoint GetPosWhenUpDownSnap(XYUPoint snappos, XYUPoint Point2dVisionCoorDst, XYUPoint Point2dVisionCoorObj)
        {
            XYUPoint pos = new XYUPoint(snappos.x, snappos.y, 0);

            return(pos);
        }
Beispiel #10
0
        /// <summary>
        /// 获取 吸嘴中新对准点  上相机拍照  获取吸嘴对应的点视觉坐标 转换得出 机械坐标点(带旋转的机械手  适用 上相机 拍照拾取)
        /// </summary>带旋转的机械手  适用 上相机 拍照拾取
        /// <param name="snappos"> 拍照点 机械手坐标</param>
        /// <param name="u0degModle"> 0 度模板对应的 机械手 机械坐标</param>
        /// <param name="Point2dVisionCoor">视觉mark的视觉坐标</param>
        /// <returns></returns>
        public XYUPoint GetPosWhenUpCameraSnap(XYUPoint snappos, double u0degModle, XYUPoint Point2dVisionCoor)
        {
            // u0degModle 0 度模板(做模板) u的角度
            HTuple qx, qy;

            HTuple dsqx, dsqy;
            HTuple dcqx, dcqy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, snappos.x, snappos.y, out dsqx, out dsqy);
            HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, CalibPos.x, CalibPos.y, out dcqx, out dcqy);

            HTuple calbx, calby;

            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, dcqx, dcqy, out calbx, out calby);
            HOperatorSet.AffineTransPoint2d(m_Hom2Dutd, Point2dVisionCoor.x, Point2dVisionCoor.y, out qx, out qy);

            HTuple vecx, vecy;

            vecx = dsqx - dcqx;
            vecy = dsqy - dcqy;


            //mark 坐标
            HTuple dmarkx, dmarky;

            dmarkx = qx + vecx;
            dmarky = qy + vecy;
            HTuple rmarkx, rmarky;

            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, dmarkx, dmarky, out rmarkx, out rmarky);
            if (Point2dVisionCoor.u > Math.PI)
            {
                Point2dVisionCoor.u = Point2dVisionCoor.u - 2 * Math.PI;
            }
            else if (Point2dVisionCoor.u < -Math.PI)
            {
                Point2dVisionCoor.u = Point2dVisionCoor.u + 2 * Math.PI;
            }
            HTuple tupleAngle = u0degModle - 180.000 * Point2dVisionCoor.u / Math.PI;
            HTuple rqx, rqy;
            //HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, qx, qy, out rqx, out rqy);
            //XYUPoint post = new XYUPoint();
            //post.x = rqx + snappos.x - CalibPos.x;
            //post.y = rqy + snappos.y - CalibPos.y;

            HTuple vecNx, vecNy;
            HTuple dcnqx, dcnqy;

            HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, NozzleCalibRobotCoor.x, NozzleCalibRobotCoor.y, out dcnqx, out dcnqy);
            vecNx = NozzleCalibVisionCoor.x - dcnqx;
            vecNy = NozzleCalibVisionCoor.y - dcnqy;
            HTuple lens = Math.Sqrt(vecNx.D * vecNx.D + vecNy.D * vecNy.D);

            ////旋转后到的角度
            // HTuple tupleAngle = u0degModle+180.000* Point2dVisionCoor.u/Math.PI;
            //需要旋转的角度
            HTuple AngleRote = tupleAngle - snappos.u;

            HTuple tupleAnglecalib = 0;

            HOperatorSet.AngleLx(dcnqy, dcnqx, NozzleCalibVisionCoor.y, NozzleCalibVisionCoor.x, out tupleAnglecalib);
            //  HOperatorSet.AngleLl(0,0,0,100,dcnqy, dcnqx, NozzleCalibVisionCoor.y, NozzleCalibVisionCoor.x, out tupleAnglecalib);
            HTuple destangleandR = tupleAnglecalib - Math.PI * (tupleAngle - NozzleCalibRobotCoor.u) / 180.0000;
            HTuple Destnozzledx = dsqx + Math.Cos(destangleandR.D) * lens;
            HTuple Destnozzledy = dsqy - Math.Sin(destangleandR.D) * lens;
            HTuple rnozzlex, rnozzley;

            HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, Destnozzledx, Destnozzledy, out rnozzlex, out rnozzley);


            HTuple offsetx = rmarkx - rnozzlex;
            HTuple offsety = rmarky - rnozzley;

            XYUPoint pos = new XYUPoint(snappos.x + offsetx.D, snappos.y + offsety.D, tupleAngle);

            return(pos);
        }
Beispiel #11
0
 public XYUPoint GetModleMachiePos(XYUPoint dstVisoionpoint, XYUPoint SanpMachinePoint)
 {
     return(GetDstPonit(dstVisoionpoint, SanpMachinePoint));
 }
Beispiel #12
0
 /// <summary>
 /// 获取对象的点位
 /// </summary>
 /// <param name="dstVisoionpoint"> 对象视觉坐标</param>
 /// <param name="SanpPoint">对象的拍照机械位置</param>
 /// <returns>返回对象机械坐标</returns>
 public abstract XYUPoint GetObjPonit(XYUPoint objVisoionpoint, XYUPoint SanpMachinePoint);
Beispiel #13
0
 /// <summary>
 /// 获取目标的点位
 /// </summary>
 /// <param name="dstVisoionpoint"> 目标视觉坐标</param>
 /// <param name="SanpPoint">目标的拍照机械位置</param>
 /// <returns>返回目标机械坐标</returns>
 public abstract XYUPoint GetDstPonit(XYUPoint dstVisoionpoint, XYUPoint SanpMachinePoint);