GRBL AutoLevel 对于平面调整补偿的一些小理解

写这段的目的是为了满足对更高平面的测量，从开源CNC控制器GBRL上做了一点小探究。

在这段代码中，ApplyHeightMap 扩展方法将高度图（Height Map）应用到 G-Code 命令序列上，通过调整每个移动命令的 Z 轴坐标来实现。

具体来说，对于每个 G-Code 命令（无论是直线移动 Straight 还是圆弧移动 Arc），代码会根据命令中点的 X 和 Y 坐标来查询高度图，获取相应的 Z 轴偏移量，然后将这个偏移量加到当前点的 Z 坐标上。

以下是具体步骤：

1. 确定命令类型：首先检查当前的 G-Code 命令是否为 OtherCode 类型，如果是，则不做修改直接返回该命令。

2. 对于移动命令：如果命令是 Movement 类型（包括 Straight 和 Arc），则进一步处理。

3. 计算分割数：对于每个移动命令，根据移动的长度和高度图的格子大小来计算需要分割成多少段。

4. 直线移动（Straight）：

   • 如果是快速移动（Rapid），直接在终点的 Z 轴上加上高度图在该点的高度值。

   • 如果不是快速移动，将移动分割成多个小段，每段的终点 Z 轴坐标都加上高度图在该点的高度值。

5. 圆弧移动（Arc）：

   • 类似于直线移动，将圆弧分割成多个小段，每段的终点 Z 轴坐标都加上高度图在该点的高度值。

6. 查询高度图：使用 map.GetHeightAt(end.X, end.Y) 方法查询高度图中给定点的高度值。这个方法通过双线性插值或其他插值方法来估算出在指定 X 和 Y 坐标上的高度值。

7. 应用高度偏移：将查询到的高度值加到当前点的 Z 坐标上，从而得到新的 Z 坐标。

8. 生成新的命令：使用修改后的坐标生成新的 G-Code 命令，并将这些命令作为序列的一部分返回。

9. 返回新的命令序列：使用 yield return 语句逐个返回修改后的命令，最终形成一个完整的修改后的 G-Code 命令序列。

通过这种方式，高度图的高度信息被“映射”到了 G-Code 命令的 Z 轴坐标上，使得机器在执行这些命令时能够根据地形的变化调整其高度，实现更加精确的 3D 打印或雕刻。

    public static IEnumerable<GCodeCommand> ApplyHeightMap(this IEnumerable<GCodeCommand> commands, HeightMap map)

        {

            foreach (GCodeCommand command in commands)

            {

                if (command is OtherCode)

                {

                    yield return command;

                    continue;

                }

                else

                {

                    Movement m = (Movement)command;



                    int divisions = (int)Math.Ceiling(m.Length / map.GridSize);



                    if (m is Straight)

                    {

                        Straight s = (Straight)m;



                        if (s.Rapid)

                        {

                            Vector3 newEnd = s.End;

                            newEnd.Z += map.GetHeightAt(s.End.X, s.End.Y);



                            yield return new Straight(s.Start, newEnd, true);

                        }

                        else

                        {

                            Vector3 pos = s.Start;



                            for (int x = 1; x <= divisions; x++)

                            {

                                Vector3 end = s.Start.Interpolate(s.End, (float)x / (float)divisions);

                                end.Z += map.GetHeightAt(end.X, end.Y);

                                Straight st = new Straight(pos, end, false);

                                if (x == 1)

                                    st.FeedRate = s.FeedRate;

                                yield return st;

                                pos = end;

                            }

                        }

                    }

                    if (m is Arc)

                    {

                        Arc a = (Arc)m;



                        Vector3 pos = a.Start;



                        float stretch = a.StartAngle - a.EndAngle;



                        if (stretch <= 0)

                            stretch += 2 * (float)Math.PI;



                        if (a.Direction == ArcDirection.CCW)

                        {

                            stretch = 2 * (float)Math.PI - stretch;

                        }



                        if (stretch <= 0)

                            stretch += 2 * (float)Math.PI;



                        for (int x = 1; x <= divisions; x++)

                        {

                            Vector3 end = new Vector3(a.Radius, 0, 0);



                            if (a.Direction != ArcDirection.CW)

                                end.Roll(a.StartAngle + stretch * (float)x / (float)divisions);

                            else

                                end.Roll(a.StartAngle - stretch * (float)x / (float)divisions);



                            end += a.Center;



                            end.Z = a.Start.Z + (a.End.Z - a.Start.Z) * (float)x / (float)divisions;



                            end.Z += map.GetHeightAt(end.X, end.Y);



                            Arc arc = new Arc(pos, end, a.Center, a.Direction);



                            if (x == 1)

                                arc.FeedRate = a.FeedRate;



                            yield return arc;

                            pos = end;

                        }

                    }

                }

            }



            yield break;

        }