AutoPay Login Invoice Login About Us Software AutoPay FerrousCNC Moneylender Store

The code that runs in my controller software to probe the PCB height. As requested by Greg's Stone Yard

Feel free to review/copy/use this code for whatever you want - even commercial purposes. No attribution required.

Part 1 of 2 - Collecting a heightmap of the surface of the copper clad

This class is responsible for scanning across the PCB and reading the probe heights from the router. The pcbHeightResponses function gets called each time the router replies to a command, and the argument newLine is the line sent back by the router. With GRBL, the probe command will respond with a [PRB####] line, and we're looking for this to pull out the value of the z-axis (the third number from the probe response)

namespace CNC_Command {
  public class PcbHeightMapperVM: ViewModelBase {
    //PCB properties.
    public
    const int SampleSpacing = 5; //Mapping points are 5mm apart.
    public
    const int BoardWidth = 70; //65mm sample width
    public
    const int BoardHeight = 100; //85mm sample height
    private
    const int ProbingSpeed = 60; //mm/min

    public
    const int MaxX = BoardWidth / SampleSpacing - 1; //5 through 65
    public
    const int MaxY = BoardHeight / SampleSpacing - 1; //5 through 95

    public int X {
      get {
        return _X;
      }
      set {
        SetNotifyingProperty(() => X, ref _X, value);
        CurrentPosition = new Thickness(X * SampleSpacing, MaxY * SampleSpacing - Y * SampleSpacing, 0, 0);
      }
    }
    private int _X;

    public int Y {
      get {
        return _Y;
      }
      set {
        SetNotifyingProperty(() => Y, ref _Y, value);
        CurrentPosition = new Thickness(X * SampleSpacing, MaxY * SampleSpacing - Y * SampleSpacing, 0, 0);
      }
    }
    private int _Y;

    public Thickness CurrentPosition {
      get {
        return _CurrentPosition;
      }
      set {
        SetNotifyingProperty(() => CurrentPosition, ref _CurrentPosition, value);
      }
    }
    private Thickness _CurrentPosition;

    public double[, ] HeightMap; //Heighs of points on the PCB

    private GrblMachine Machine; //Ref to the CNC router.

    public event EventHandler HeightMapComplete;

    public PcbHeightMapperVM() {}

    public PcbHeightMapperVM(GrblMachine Machine) {
      this.Machine = Machine; //Localize ref to machine.
      HeightMap = new double[MaxX, MaxY]; //Initialize the heigh map.
      X = 1; //Initialize the height sample points.
      Y = 1;

      //Hook up to the machine's output.
      Machine.LogUpdated += pcbHeightResponses;

      //First, probe to Z0 at 0,0.
      Machine.Commands.AddCommand("G90"); //Absolute positioning
      Machine.Commands.AddCommand("G0 X" + SampleSpacing + " Y" + SampleSpacing); //Move to the point of the first space.

      ////Testing image map.
      //for (int x = 0; x < MaxX; x++)
      //{
      //    for (int y = 0; y < MaxY; y++)
      //    {
      //        HeightMap[x, y] = 25 + x + y;
      //    }
      //}
      //return;

      //Take the first sample.
      //sampleNextPoint();
    }

    private void pcbHeightResponses(string newLine) {
      //Probing returns [PRB:0.000,0.000,0.000,0.000,0.000:0]
      if (newLine.StartsWith("[PRB:")) {
        //Number after the second comma.
        int pos = newLine.IndexOf(","); //First comma
        pos = newLine.IndexOf(",", pos + 1) + 1; //We're at the start of the third number.
        int len = newLine.IndexOfAny(new char[] {
          ':',
          ',',
          ']',
          '\r',
          '\n'
        }, pos) - pos;

        HeightMap[X - 1, Y - 1] = double.Parse(newLine.Substring(pos, len));

        //Note that this will be the zero point for the subsequent trace milling operation.
        if (X == 1 && Y == 1) Machine.Commands.AddCommand("G92 Z0");

        //Advance to next item.
        X++;

        //Already completed this row?
        if (X > MaxX) {
          //go to the next row.
          X = 1;
          Y++;

          //Hit the end of the board.
          if (Y > MaxY) {
            //Unhook from the log updated event.
            Machine.LogUpdated -= pcbHeightResponses;

            Machine.Commands.AddCommand("G0 Z10"); //Move to a safe height;
            Machine.Commands.AddCommand("G0 X35 Y50"); //Move to PCB center;

            //Notify the completion of the height map probing.
            HeightMapComplete?.Invoke(this, EventArgs.Empty);

            return;
          }
        }

        //Send the commands to prove the next point.
        sampleNextPoint();
      }
    }

    public void sampleNextPoint() {
      #if DEBUG
      //Spoof a probe response on the log entries.
      pcbHeightResponses("[PRB:0,0," + ((2 * X + Y) / 3 + 15) + ",0.00]");
      #else
      //Move to a safe height.
      Machine.Commands.AddCommand("G0 Z2");

      //Rapid to next coordinates
      Machine.Commands.AddCommand("G0 X" + (X * SampleSpacing) + " Y" + (Y * SampleSpacing));

      //Probe this point.
      Machine.Commands.AddCommand("G38.2 z-5 f" + ProbingSpeed);
      #endif
    }
  }
}

Part 2 of 2 - Applying the probed heights to the gcode of the milling job

The result of the probing operation is an array of Z values across a variety of X,Y points. Then to apply the heightmap to the actual milling job, we adjust the z value on all G1 moves by a bicubic interpolation of the surrounding points from the probe grid. If there's a long run in a straight line, we'll also split up that run into smaller segments so the bit can be made to better trace the curvature of the stock material.

A MaybePoint3D is a point in 3D space where some axes might not be explicitly specified - like when you move to 5,5 and then tell the router to move horizontally to 10, you just send a G1 x10 command. The MaybePoint would be 10,?, and we can fill in the 5 from the previously known machine position.

        private double[,] HeightMap;
        private double HeightAtZero;
        private void applyHeightMap()
        {
            //Scan the gcode.
            MaybePoint3D machinePos = new MaybePoint3D() { X = 0, Y = 0, Z = 0 };
            HeightAtZero = HeightMap[0, 0];

            StringBuilder result = new StringBuilder();
            foreach (var line in FileGCodes.Split(new char[] { '\r', '\n' }, StringSplitOptions.RemoveEmptyEntries))
            {
                GCodeCommand cmd = new GCodeCommand(line);
                if (cmd.CommandType != CommandType.G1 || !cmd.Coords.HasPosition())
                {
                    if (cmd.CommandType == CommandType.G0)
                    {
                        //Update the machine position to the result of this G0 move.
                        if (cmd.Coords.X.HasValue) machinePos.X = cmd.Coords.X.Value; //Move the the specified X
                        if (cmd.Coords.Y.HasValue) machinePos.Y = cmd.Coords.Y.Value; //Move the the specified Y
                        if (cmd.Coords.Z.HasValue) machinePos.Z = cmd.Coords.Z.Value; //Move the the specified Z
                    }

                    //Not cutting.
                    result.AppendLine(cmd.Command);
                    continue;
                }


                //How far are we about to cut?
                double dist = machinePos.DistanceTo(cmd.Coords);

                if (dist > 5)
                {
                    //More than 5mm, break it up into smaller lengths.
                    double split = Math.Ceiling(dist / 5.0d);

                    double diffX = cmd.Coords.X.HasValue ? cmd.Coords.X.Value - machinePos.X.Value : 0; //If X present, distance from current pos to new pos.
                    double diffY = cmd.Coords.Y.HasValue ? cmd.Coords.Y.Value - machinePos.Y.Value : 0; //If Y present, distance from current pos to new pos.
                    double diffZ = cmd.Coords.Z.HasValue ? cmd.Coords.Z.Value - machinePos.Z.Value : 0; //If Z present, distance from current to target Z height.

                    //Split into this many items.
                    for (double segs = 1; segs < split; segs++)
                    {
                        //Move towards the destination proportionally along the line that joins the machine to the target.
                        double partialX = machinePos.X.Value + segs * diffX / split; //Moving X in the direction of the destination.
                        double partialY = machinePos.Y.Value + segs * diffY / split; //Moving Y in the direction of the destination.
                        double partialZ = machinePos.Z.Value + segs * diffZ / split; //Maybe sliding up or down along the line as well.
                        partialZ = partialZ + GetZModifier(partialX, partialY); //Adjust Z to the hPCB surface height.

                        //Compile the segment line.
                        string partialres = "G1 X" + partialX.ToString("0.000") + " Y" + partialY.ToString("0.000") + " Z" + partialZ.ToString("0.000");
                        if (cmd.Coords.F.HasValue) partialres += " F" + cmd.Coords.F.Value; //Add the feed rate if present in the original command.

                        //Write the segment to the gcode output.
                        result.AppendLine(partialres);
                    }
                }

                //No splitting needed, or we're done splitting, advance the machine to the target point and adjust the Z offset.
                if (cmd.Coords.X.HasValue) machinePos.X = cmd.Coords.X.Value; //Move the the specified X
                if (cmd.Coords.Y.HasValue) machinePos.Y = cmd.Coords.Y.Value; //Move the the specified Y
                if (cmd.Coords.Z.HasValue) machinePos.Z = cmd.Coords.Z.Value; //Move the the specified Z

                //Adjust the z based on X and Y position.
                double tempZ = machinePos.Z.Value + GetZModifier(machinePos.X.Value, machinePos.Y.Value);

                //Compile the adjusted line of gcode
                string res = "G1 X" + machinePos.X.Value.ToString("0.000") + " Y" + machinePos.Y.Value.ToString("0.000") + " Z" + tempZ.ToString("0.000");
                if (cmd.Coords.F.HasValue) res += " F" + cmd.Coords.F.Value; //Add the feed rate if present in the original command.

                //Write the line of GCode to the output.
                result.AppendLine(res);
            }

            //Replace the file gcode with the adjusted gcode.
            FileGCodes = result.ToString();
            loadFromText();
        }

        private double GetZModifier(double X, double Y)
        {
            //What are the four salient points?
            int lowX = (int)Math.Floor(X / PcbHeightMapperVM.SampleSpacing);
            int highX = (int)Math.Ceiling(X / PcbHeightMapperVM.SampleSpacing);
            int lowY = (int)Math.Floor(Y / PcbHeightMapperVM.SampleSpacing);
            int highY = (int)Math.Ceiling(Y / PcbHeightMapperVM.SampleSpacing);

            //Make sure we're adjusting within the bounds of the probed area.
            if (lowX >= PcbHeightMapperVM.MaxX) lowX = PcbHeightMapperVM.MaxX - 1;
            if (highX >= PcbHeightMapperVM.MaxX) highX = PcbHeightMapperVM.MaxX - 1;
            if (lowY >= PcbHeightMapperVM.MaxY) lowY = PcbHeightMapperVM.MaxY - 1;
            if (highY >= PcbHeightMapperVM.MaxY) highY = PcbHeightMapperVM.MaxY - 1;

            //Get the heights in those corners.
            double ll = HeightMap[lowX, lowY],
                lr = HeightMap[highX, lowY],
                ul = HeightMap[lowX, highY],
                ur = HeightMap[highX, highY];

            double lowXc = lowX * PcbHeightMapperVM.SampleSpacing;
            double lowYc = lowY * PcbHeightMapperVM.SampleSpacing;
            double highXc = highX * PcbHeightMapperVM.SampleSpacing;
            double highYc = highY * PcbHeightMapperVM.SampleSpacing;

            //Bilinear interpolation, start by interpolating the upper and lower edges.
            double dxl;
            double dxu;
            if (X <= lowXc) //Pegged to the left edge.
            {
                dxl = ll;
                dxu = ul;
            }
            else if (X >= highXc) //Pegged all the way to the right edge.
            {
                dxl = lr;
                dxu = ur;
            }
            else //Floating between the left and right sides.
            {
                dxl = (highXc - X) / (PcbHeightMapperVM.SampleSpacing) * ll + (X - lowXc) / PcbHeightMapperVM.SampleSpacing * lr; //Interpolate the bottom edge.
                dxu = (highXc - X) / PcbHeightMapperVM.SampleSpacing * ul + (X - lowXc) / PcbHeightMapperVM.SampleSpacing * ur; //Interpolate the top edge.
            }

            //Interpolat the y direction from the interpolated values.
            double result;
            if (Y <= lowYc) result = dxl; //Below the bottom.
            else if (Y >= highYc) result = dxu; //Above the top.
            else
            {
                //Interpolate between the previous values.
                result = (highYc - Y) / PcbHeightMapperVM.SampleSpacing * dxl + (Y - lowYc) / PcbHeightMapperVM.SampleSpacing * dxu; //Interpolated between the interpolated values.
            }
            return result - HeightAtZero; //Subtract the base height to get the height offset at the specified point.
        }

        private double hmDist(double X, double Y, int XX, int YY)
        {
            return Math.Pow(Math.Pow(X - XX * PcbHeightMapperVM.SampleSpacing, 2) + Math.Pow(Y - YY * PcbHeightMapperVM.SampleSpacing, 2), 0.5) + 0.001;
        }