Проблемы с моей системой рисования рук

Question

В настоящее время я создаю проект, который позволяет пользователю ввести изображение, а затем нарисовать его, используя руку из сервоприводов. Используемый язык - C#. Программа работает так, что пользователь загружает изображение, затем изображение обрабатывается с использованием алгоритма обнаружения краев Собела, и оно печатает новое изображение в графическом окне. Мне удалось получить значения X и Y на картинке, и заставить каждый сервопривод в руке перемещаться вдоль этих точек, рисуя картинку на чистом листе бумаги, но, как работает Собел, нарисовать изображение практически невозможно без использования оси Z, чтобы знать, когда нажимать ручку на бумаге, чтобы рисовать, а когда нет. Вот код, который я использую. Я хотел бы знать, есть ли что-то более плавное, например, рисование эскизов, и нарисовать все это за один раунд, не перемещая ручку вверх и вниз по оси Z.

int oldi = 0;
int oldj=0;
private void sobel()
{   



      Bitmap  originalImage = new Bitmap(ResizeImage(pictureBox1.Image, 70, 70));   
    Bitmap sobelImage = new Bitmap(originalImage);
    int width = originalImage.Width;
    int height = originalImage.Height;

    int[,] gx = new int[,] { { -1, 0, 1 }, { -2, 0, 2 }, { -1, 0, 1 } };
    int[,] gy = new int[,] { { 1, 2, 1 }, { 0, 0, 0 }, { -1, -2, -1 } };

    int[,] allPixR = new int[width, height];
    int[,] allPixG = new int[width, height];
    int[,] allPixB = new int[width, height];

    int limit = 150 * 150;


        for (int i = 0; i < width; i++)
        {
            for (int j = 0; j < height; j++)
            {
                allPixR[i, j] = originalImage.GetPixel(i, j).R;
                allPixG[i, j] = originalImage.GetPixel(i, j).G;
                allPixB[i, j] = originalImage.GetPixel(i, j).B;
            }
        }

        int new_rx = 0, new_ry = 0;
        int new_gx = 0, new_gy = 0;
        int new_bx = 0, new_by = 0;
        int rc, gc, bc;
        for (int i = 1; i < originalImage.Width - 1; i++)
        {
            for (int j = 1; j < originalImage.Height - 1; j++)
            {
                new_rx = 0;
                new_ry = 0;
                new_gx = 0;


         new_gy = 0;
                    new_bx = 0;
                    new_by = 0;
                    rc = 0;
                    gc = 0;
                    bc = 0;

                    for (int wi = -1; wi < 2; wi++)
                    {
                        for (int hw = -1; hw < 2; hw++)
                        {
                            rc = allPixR[i + hw, j + wi];
                            new_rx += gx[wi + 1, hw + 1] * rc;
                            new_ry += gy[wi + 1, hw + 1] * rc;

                            gc = allPixG[i + hw, j + wi];
                            new_gx += gx[wi + 1, hw + 1] * gc;
                            new_gy += gy[wi + 1, hw + 1] * gc;

                            bc = allPixB[i + hw, j + wi];
                            new_bx += gx[wi + 1, hw + 1] * bc;
                            new_by += gy[wi + 1, hw + 1] * bc;
                        }
                    }

                    if (new_rx * new_rx + new_ry * new_ry > limit || new_gx * new_gx + new_gy *                    new_gy > limit || new_bx * new_bx + new_by * new_by > limit)
                    {
    // Draws the edges of the picture on picture box 2                 

       sobelImage.SetPixel(i, j, Color.Black);





                                if (oldi != i)
                                {
                                    oldi = i;

    // move along the X axis   

                          ezB_Connect1.EZB.Servo.SetServoPosition(EZ_B.Servo.ServoPortEnum.D0, i);
                                     Thread.Sleep(500);


                                }




                                    if (oldj != j)
                                    {
                                       // move along the Y axis 

ezB_Connect1.EZB.Servo.SetServoPosition (EZ_B.Servo.ServoPortEnum.D1, j);

                                        oldj = j;



                                    }



                            }

                               pictureBox2.Image = sobelImage;
                            }




                        else
                        {
    // draws white "fills the insides of edges" 
                              sobelImage.SetPixel(i, j, Color.White);



                        }



                }

            }