c语言，想把这个代码改成走方波轨迹，走直角的那种

Question

int COSLRobot::PathPlanning()
{


    static int idx=0;
    static double des_X = 1;
    static double des_Y = 1;
    double A;
    curpose.x,curpose.y,curpose.a;
    tgtpos.x,tgtpos.y,tgtpos.a;
    if (idx==0) SetTargetP(1,1);
    A = sqrt(pow(tgtpos.x - curpose.x, 2) + pow(tgtpos.y - curpose.y, 2));
    if(A<0.1)
    {
        idx=idx+1;
        des_X = idx;
        if((idx%2，) == 0)
            des_Y=2;
        if((idx%2)  == 1)
            des_Y=1;
    }
    SetTargetP(des_X,des_Y);

    return 1;
}

Answer 1

int COSLRobot::PathPlanning()
{
    static int idx = 0;
    static double des_X = 1;
    static double des_Y = 1;
    double A;
    // 假设curpose和tgtpos已经被正确定义和初始化
    // double curpose.x, curpose.y, curpose.a;
    // double tgtpos.x, tgtpos.y, tgtpos.a;

    // 初始化目标位置
    if (idx == 0) SetTargetP(1, 1);

    // 计算当前位置到目标位置的距离
    A = sqrt(pow(tgtpos.x - curpose.x, 2) + pow(tgtpos.y - curpose.y, 2));
    
    // 如果距离小于阈值，则更新目标位置
    if(A < 0.1)
    {
        idx = idx + 1; // 增加步骤数
        
        if((idx % 4) == 0) // 每四步后Y坐标减小
            des_Y -= 1;
        else if((idx % 4) == 2) // 每四步中的第三步Y坐标增大
            des_Y += 1;
        else if((idx % 4) == 1) // 每四步中的第二步X坐标增大
            des_X += 1;
        else if((idx % 4) == 3) // 每四步中的第四步X坐标减小
            des_X -= 1;
    }
    
    // 设置新的目标位置
    SetTargetP(des_X, des_Y);
    
    return 1;
}