LitePlacer.com forums

Code: Select all

	public List<NeedlePoint> CalibrationPoints = new List<NeedlePoint>();

Code: Select all

	public List<NeedlePoint> CalibrationPoints = new List<NeedlePoint>();
	public NeedlePoint CalibrationPointsCenter = new NeedlePoint();

Code: Select all

            Calibrated = true;
            return true;

Code: Select all

            /*********************************************************************************
             * Here we are going to calculate the center of the calibration points.  It will
             * be the center of the line connecting each two points that are 180 degrees apart.
             * We are assuming that the points are on even spacing, cover 360 degrees and that
             * there are an even number of points.  For example, 16 points with 22.5 degree
             * spacing will work.  In the ideal world all of the centers will be at the same
             * point.  But to account for some variation we will take the average of all the
             * centers.  To do that we will sum all of the points in the following loop and
             * divide the sum by the loop count
             * 
             * A little more discussion here to validate my thinking for my present self and to
             * let my future self and other programmers know what is happening.  The coordinate
             * system is setup by the camera and is relative to the camera.  But the working
             * part of the PNP is the needle.  We need a precise reference from the camera
             * position to the needle position.  The needle position wobbles as it is rotated.
             * We have calculated the center of that wobble and will be setting our needle
             * correction relative to that center.  The idea being that even a new needle will
             * wobble about that same point. Now the most important consideration is the sign
             * of the correction.  The present logic is that the correction factors are added
             * to the move.  Subtracting the center from the points gives us the right polarity
             * for a correction factor that will be added to compensate.  Then when we set the 
             * needle to camera offset we need to subtract the correction factor from the 
             * needle position to give us the point to be used in the offset calculations
             ********************************************************************************/
            // There are half as many lines as there are points
            int HalfCount = CalibrationPoints.Count / 2;
            CalibrationPointsCenter.X = 0;
            CalibrationPointsCenter.Y = 0;
            for (int i = 0; i < HalfCount; i++)
            {
                //The following block could be used if one is anal about validating the situation
                //if(CalibrationPoints[i + HalfCount].Angle - CalibrationPoints[i ].Angle != 180)
                //{
                //    Calibrated = false;
                //    return false;
                //}
                CalibrationPointsCenter.X += (CalibrationPoints[i].X + CalibrationPoints[i + HalfCount].X) / 2;
                CalibrationPointsCenter.Y += (CalibrationPoints[i].Y + CalibrationPoints[i + HalfCount].Y) / 2;
            }
            CalibrationPointsCenter.X /= HalfCount;
            CalibrationPointsCenter.Y /= HalfCount;
            /**********************************************************************************
             * The previous technique was calculating the correction based on the needle being
             * near the center of the screen at rotation 0.  Effectively we are normalizing to
             * a point in the center of the distribution of points.  Instead of changing the
             * CorrectedPosition_m routine, we can normalize the values here and leave that
             * routine alone.
             **********************************************************************************/
            for (int i = 0; i < CalibrationPoints.Count; i++)
            {
                NeedlePoint Point = CalibrationPoints[i];
                Point.X -= CalibrationPointsCenter.X;
                Point.Y -= CalibrationPointsCenter.Y;
            }
            Calibrated = true;
            return true;

Code: Select all

        /**********************************************************************
         * This function sets the needle to camera offset and wobble correction.
         * Previously the offset was calculated and then the wobble correction.
         * That order does not give the best results.  Doing the wobble correction
         * first will give the desired results
         *********************************************************************/
        private void Offset2Method_button_Click(object sender, EventArgs e)
        {
            ZGuardOff();
            SelectCamera(DownCamera);
            SetCurrentCameraParameters();
            switch (SetNeedleOffset_stage)
            {
                case 0:
                    ShowMessageBox(
                        "Jog the needle above the up camera,\n\rtake needle down, jog it to the image center\n\rand set Up Camera location",
                        "Setup for Wobble Calibration",
                        MessageBoxButtons.OK);
                    SelectCamera(UpCamera);
                    //SetNeedleMeasurement();
                    NeedleOffset_label.Visible = true;
                    NeedleOffset_label.Text = "Click \"Next\" when needle is in position for wobble calibration.";
                    SetNeedleOffset_stage = 1;
                    Offset2Method_button.Text = "Next";
                    break;

                case 1:
                    Properties.Settings.Default.UpCam_PositionX = Cnc.CurrentX;
                    Properties.Settings.Default.UpCam_PositionY = Cnc.CurrentY;
                    if (!CalibrateNeedle_m())
                    {
                        SetNeedleOffset_stage = 0;
                        NeedleOffset_label.Visible = false;
                        NeedleOffset_label.Text = "   ";
                        Offset2Method_button.Text = "Start";
                        ZGuardOn();
                        break;
                    }
                    SetNeedleOffset_stage = 2;
                    NeedleOffset_label.Visible = true;
                    NeedleOffset_label.Text = "Click \"Next\" when wobble calibration is complete.";
                    break;

                case 2:
                    SetNeedleOffset_stage = 3;
                    CNC_A_m(0.0);
                    NeedleOffset_label.Visible = true;
                    NeedleOffset_label.Text = "Jog needle to a point on a PCB, then click \"Next\"";
                    break;

                case 3:
                    double dX;
                    double dY;
                    SetNeedleOffset_stage = 4;
                    Needle.CorrectedPosition_m(Cnc.CurrentA, out dX, out dY);
                    NeedleOffsetMarkX = Cnc.CurrentX - dX;
                    NeedleOffsetMarkY = Cnc.CurrentY - dY;
                    CNC_Z_m(0);
                    CNC_XY_m(Cnc.CurrentX - 75.0, Cnc.CurrentY - 29.0);
                    DownCamera.DrawCross = true;
                    NeedleOffset_label.Text = "Jog camera above the same point, \n\rthen click \"Next\"";
                    break;

                case 4:
                    SetNeedleOffset_stage = 0;
                    Properties.Settings.Default.DownCam_NeedleOffsetX = NeedleOffsetMarkX - Cnc.CurrentX;
                    Properties.Settings.Default.DownCam_NeedleOffsetY = NeedleOffsetMarkY - Cnc.CurrentY;
                    NeedleOffsetX_textBox.Text = Properties.Settings.Default.DownCam_NeedleOffsetX.ToString("0.00", CultureInfo.InvariantCulture);
                    NeedleOffsetY_textBox.Text = Properties.Settings.Default.DownCam_NeedleOffsetY.ToString("0.00", CultureInfo.InvariantCulture);
                    NeedleOffset_label.Visible = false;
                    NeedleOffset_label.Text = "   ";
                    Offset2Method_button.Text = "Start";
                    CNC_Z_m(0.0);
                    ZGuardOn();
                    break;
            }
        }

Code: Select all

            // take needle to camera
            result &= CNC_XY_m(Properties.Settings.Default.UpCam_PositionX, Properties.Settings.Default.UpCam_PositionY);
            result &= CNC_Z_m(Properties.Settings.Default.General_ZtoPCB - 1.0); // Average small component height 1mm (?)
            int iTries = 0;

            //Used in place of Do While because this form allows the exit in the middle of block
            //We should have the greatest accuracy if the camera reads the positions of the needle close to the
            //center of the video.  This block will make up to two passes moving the needle so that the needle wobble
            //is centered on the display
            while (true)
            {
                // measure the values
                SetNeedleMeasurement();
                result &= Needle.Calibrate(4.0 / Properties.Settings.Default.UpCam_XmmPerPixel);  // have to find the tip within 4mm of center
                double dCenterDist = Math.Sqrt(Math.Pow(Needle.CalibrationPointsCenter.X, 2) + Math.Pow(Needle.CalibrationPointsCenter.Y, 2));
                if (dCenterDist <= 0.2) break;
                iTries++;
                if (iTries >= 2) break;
                //Setup for another pass through the loop, recalculate UpCam_Position to center wobble
                Properties.Settings.Default.UpCam_PositionX += Needle.CalibrationPointsCenter.X;
                Properties.Settings.Default.UpCam_PositionY += Needle.CalibrationPointsCenter.Y;
                ZGuardOff();
                result &= CNC_XY_m(Properties.Settings.Default.UpCam_PositionX, Properties.Settings.Default.UpCam_PositionY);
                ZGuardOn();
            }


            // take needle up
            result &= CNC_Z_m(0.0);

Code: Select all

  
 /// <summary>
        /// Compute the inetrsection point of two lines A and B defined by their start and end loactions
        /// </summary>
        /// <param name="AStart"></param>
        /// <param name="AEnd"></param>
        /// <param name="BStart"></param>
        /// <param name="BEnd"></param>
        /// <returns></returns>
        PartLocation LineIntersectionPoint(PartLocation AStart, PartLocation AEnd, PartLocation BStart, PartLocation BEnd)
        {
            // Get A,B,C of first line - points : ps1 to pe1
            double A1 = AEnd.Y - AStart.Y;
            double B1 = AStart.X - AEnd.X;
            double C1 = A1 * AStart.X + B1 * AStart.Y;

            // Get A,B,C of second line - points : ps2 to pe2
            double A2 = BEnd.Y - BStart.Y;
            double B2 = BStart.X - BEnd.X;
            double C2 = A2 * BStart.X + B2 * BStart.Y;

            // Get delta and check if the lines are parallel
            double delta = A1 * B2 - A2 * B1;

            if (delta == 0)
                throw new Exception("Lines are parallel");

            // now return the Vector2 intersection point
            return new PartLocation(
                (B2 * C1 - B1 * C2) / delta,
                (A1 * C2 - A2 * C1) / delta
            );
        }
        /// Computes the center intersection point of a nozzle calibration
        /// list. This list contains 16 point measured in 22.5° steps.
        /// Therefore we have 4 pairs of ortogonal intersecting lines
        /// </summary>
        public PartLocation ZCenterOffset
        {
            get
            {
                List<PartLocation> resList = new List<PartLocation>();

                for (int i = 0; i < 4; i++)
                {
                    PartLocation pResult = LineIntersectionPoint(AbsCalibrationPoints[i], AbsCalibrationPoints[i + 8], AbsCalibrationPoints[i + 4], AbsCalibrationPoints[i + 12]);
                    resList.Add(pResult);
                }

                return PartLocation.Average(resList);
            }
        }

        /// <summary>
        /// Calibrates the needle by first computing the real A axis center
        /// and then the wobble offset
        /// </summary>
        /// <returns>true on success</returns>
        public bool Calibrate()
        {
            // first we evaluate the true A-axis center point
            if (Calibrate(4.0 / Config.Instance.UpCamXmmPerPixel))
            {
                // and we save it
                Location currentCamCenter = LocationSet.FindLocation("UPCAMERA");

                LocationSet.FindLocation("UPCAMERA").SetTo(currentCamCenter - ZCenterOffset);
                CNC.MoveToLocation("UPCAMERA");

                if (Calibrate(4.0 / Config.Instance.UpCamXmmPerPixel))
                {
                    for (int i = 0; i < CalibrationPoints.Count; i++)
                        DashBoard.sLogText(2, CalibrationPoints[i].ToString(), Color.Purple);

                    return true;
                }
            }

            MainForm.sShowError("Nozzle Calibration Failed!");
            IsCalibrated = false;
            return false;
        }

  /// <summary>
        /// Add the specific nozzle offset and the runoff/wobble 
        /// induced by the current nozzle
        /// 
        /// </summary>
        /// <param name="location"></param>
        /// <returns></returns>
        public SpaceLocation AddOffset(SpaceLocation location, bool subtract = false)
        {
            double dX = 0;
            double dY = 0;

            if (GetCorrectedPosition(location.A, out dX, out dY))
            { 
                var wobble = new PartLocation(dX, dY, 0);

                if (subtract)
                {
                    location += wobble;
                    location -= Offset;
                }
                else
                {
                    location -= wobble;
                    location += Offset;
                }
            }

            return location;
        }

Code: Select all

 
         /// <summary>
        /// When we move the machine to a head based location the designated location
        /// in down-camera view is offset by the calibrated head + nozzle offsets.
        /// </summary>
        /// <param name="location">the space loaction X,Y,Z and Angle</param>
        /// <returns></returns>
        public SpaceLocation AddOffset(SpaceLocation location, bool subtract = false)
        {
            if (subtract)
                location -= Offset;
            else
                location += Offset;

            if (Nozzle != null)
                location = Nozzle.AddOffset(location, subtract);   

            return location;
        }

Code: Select all

  public static bool MoveToXYAZ(double? x, double? y, double? angle = null, double? z = null, int? speed = null, OffsetStyle offsetStyle = OffsetStyle.Use_Default)
        {
            if (_ErrorOccured || !Connected)
            {
                LogText(2, string.Format("CNC.MoveToXYZA: move canceled because {0}", _ErrorOccured ? "Error has occured!" : "CNC not connected!"));
                return false;
            }

            if (!CNC.JoggingBusy)
            {
                // if not set as parameter use global offset type
                if (offsetStyle == OffsetStyle.Use_Default)
                    offsetStyle = OffsetType;

                // depending on what we are intending to use add the appropriate offset 
                if (offsetStyle != OffsetStyle.Camera)
                {
                    if (offsetStyle == OffsetStyle.Last_Head)
                        offsetStyle = _LastPnPHead;

                    SpaceLocation offset = NozzleSet.GetHeadByIndex((int)offsetStyle).AddOffset(new SpaceLocation(x, y, z, angle));

                    if (x != null) x = offset.X;
                    if (y != null) y = offset.Y;
                    if (z != null) z = offset.Z;
                }
            }

             LogText(2, string.Format("MoveToXYZA: X:{0:#0.00} Y:{1:#0.00} Z:{2:#0.00} A:{3:#0.00}", x, y, z, angle), Color.DarkMagenta);

            if (!_NoSafeTest && !CanMoveSafeTo(x, y, z))
            {
                LogText(2, "CNC.MoveToXYZA: ignored because cannot move safe!", Color.Red);
                return false;
            }

            _BlockingWriteDone = false;

            Thread t = new Thread(() => BlockingMoveThread(x, y, z, angle, speed))
            {
                Name = "CNC_BlockingMove",
                IsBackground = true
            };

            t.Start();
            int i = _MoveTimeout;

            while (!_BlockingWriteDone && !_ErrorOccured)
            {
                Thread.Sleep(2);
                Application.DoEvents();
                i--;

                if (i <= 0)
                    _readyEvent.Set();   // causes CNC_Blocking_thread to exit
            }

            _BlockingWriteDone = true;

            if (((i > _MoveTimeout) || _ErrorOccured) && Connected)
            {
                MainForm.sShowError("CNC.MoveToXYZA: Timeout ");
                //Close();
                MainForm.sUpdateCNCStatus();
            }

            return (Connected);