使用固高运动卡在圆形里走倒T型运动

 private void RunZ_Click(object sender, EventArgs e)
        {
            
            //搜索点
            double  radius = Convert.ToDouble(Radius.Text);
            double  stepLength = Convert.ToDouble(StepLength.Text);

            double StartLineY = Convert.ToDouble((radius * 2) % stepLength);

            int InterSpaceCount =int.Parse( ((radius*2) / stepLength).ToString() );

            double  y;
            double bottomLength;

            List<double[]> XYPoint = new List<double[]>();
            //  List<PointF> pt = new List<PointF>();


            //运动控制，采用点位运动的形式
            Axis = 1;
            rtn = GT_ClrSts(carNumber, Axis, 8);//清除1到8轴状态

            TTrapPrm trap = new TTrapPrm();

            //获取参数
            double acc = Convert.ToDouble(Acceleration.Text);
            double dec = Convert.ToDouble(Deceleration.Text);
            short SmoothTime = Convert.ToInt16(SmoothingTime.Text);

            //设置参数
            trap.acc = acc;
            trap.dec = dec;
            trap.smoothTime = SmoothTime;
            trap.velStart = 10;

            // X轴
            rtn = GT_SetTrapPrm(carNumber, Axis, ref trap);
            CommandPrint("GT_SetTrapPrm", rtn);

            // Y轴
            rtn = GT_SetTrapPrm(carNumber, (short)(Axis+1), ref trap);
            CommandPrint("GT_SetTrapPrm", rtn);

            //设置XY轴目标速度
            double vel = Convert.ToDouble(Speed.Text);
            rtn = GT_SetVel(carNumber, Axis, vel);
            CommandPrint("GT_SetVel", rtn);

            rtn = GT_SetVel(carNumber, (short)(Axis + 1), vel);
            CommandPrint("GT_SetVel", rtn);


          

            //搜索点
            for (int i = 0; i < InterSpaceCount; i++)
            {
                double[] site = new double[2];
                double[] sites = new double[2];
                if (StartLineY == 0)
                {
                    site[0] = 0;
                    site[1] = 0;
                    XYPoint.Add(site);
                    // StartLineY += InterSpace;
                    StartLineY += stepLength;
                }
                else
                {
                    if (StartLineY < stepLength)
                    {
                        StartLineY = stepLength;
                    }
                    y = StartLineY - radius;
                    bottomLength = Math.Sqrt((radius * radius) - (y * y));
                    bottomLength = Math.Round(bottomLength, 3);
                    Console.WriteLine($"bottomLength: {bottomLength}");

                    site[0] =  bottomLength;
                    site[1] =  y;
                    XYPoint.Add(site);

                    sites[0] = bottomLength;
                    sites[1] = y;
                    XYPoint.Add(sites);

                  //  StartLineY += InterSpace;
                    StartLineY += stepLength;
                    if (i % 2 == 0)
                    {
                        Console.WriteLine($"i: {i}");
                        //位置2，X轴
                        rtn = GT_SetPos(carNumber, Axis,(int)XYPoint[XYPoint.Count - 1][0]);
                        Console.WriteLine($"X轴移动到位置2: {(int)XYPoint[XYPoint.Count - 1][0]}");
                        //启动点位运动
                        rtn = GT_Update(carNumber, 1 << (Axis - 1));
                        CommandPrint("X轴GT_Update", rtn);

                        //位置2，Y轴
                        rtn = GT_SetPos(carNumber, (short)(Axis + 1), (int)XYPoint[XYPoint.Count - 1][1]);
                        Console.WriteLine($"Y轴移动到位置2: {(int)XYPoint[XYPoint.Count - 1][1]}");
                        rtn = GT_Update(carNumber, 1 << (Axis));
                        CommandPrint("Y轴GT_Update", rtn);
                        MoveDetect((int)XYPoint[XYPoint.Count - 1][0], (int)XYPoint[XYPoint.Count - 1][1]);


                        //位置1，X轴
                        rtn = GT_SetPos(carNumber, Axis, (int)XYPoint[XYPoint.Count - 2][0]);
                        Console.WriteLine($"X轴移动到位置1: {(int)XYPoint[XYPoint.Count - 2][0]}");
                        rtn = GT_Update(carNumber, 1 << (Axis - 1));


                        //位置1，Y轴
                        rtn = GT_SetPos(carNumber, (short)(Axis + 1), (int)XYPoint[XYPoint.Count - 2][1]);
                        Console.WriteLine($"Y轴移动到位置1: {(int)XYPoint[XYPoint.Count - 2][1]}");
                        rtn = GT_Update(carNumber, 1 << (Axis));
                        CommandPrint("Y轴GT_Update", rtn);
                        MoveDetect((int)XYPoint[XYPoint.Count - 2][0], (int)XYPoint[XYPoint.Count - 2][1]);
                  
                    }
                    else
                    {
                        Console.WriteLine($"i: {i}");
                        //位置1，X轴
                        rtn = GT_SetPos(carNumber, Axis, (int)XYPoint[XYPoint.Count - 2][0]);
                        Console.WriteLine($"X轴移动到位置1: {(int)XYPoint[XYPoint.Count - 2][0]}");
                        //启动点位运动
                        rtn = GT_Update(carNumber, 1 << (Axis - 1));
                        CommandPrint("X轴GT_Update", rtn);

                        //位置1，Y轴
                        rtn = GT_SetPos(carNumber, (short)(Axis + 1), (int)XYPoint[XYPoint.Count - 2][1]);
                        Console.WriteLine($"Y轴移动到位置1: {(int)XYPoint[XYPoint.Count - 2][1]}");
                        rtn = GT_Update(carNumber, 1 << (Axis));
                        CommandPrint("Y轴GT_Update", rtn);
                        MoveDetect((int)XYPoint[XYPoint.Count - 2][0], (int)XYPoint[XYPoint.Count - 2][1]);

                        

                        //位置2，X轴
                        rtn = GT_SetPos(carNumber, Axis, (int)XYPoint[XYPoint.Count - 1][0]);
                        Console.WriteLine($"X轴移动到位置2: { (int)XYPoint[XYPoint.Count - 1][0]}");
                        //启动点位运动
                        rtn = GT_Update(carNumber, 1 << (Axis - 1));
                        CommandPrint("X轴GT_Update", rtn);

                        //位置2，Y轴
                        rtn = GT_SetPos(carNumber, (short)(Axis + 1), (int)XYPoint[XYPoint.Count - 1][1]);
                        Console.WriteLine($"Y轴移动到位置2: { (int)XYPoint[XYPoint.Count - 1][1]}");
                        rtn = GT_Update(carNumber, 1 << (Axis));
                        CommandPrint("Y轴GT_Update", rtn);
                        MoveDetect((int)XYPoint[XYPoint.Count - 1][0],  (int)XYPoint[XYPoint.Count - 1][1]);

                    }                  
                }
            }
        }