机器人圆形队列算法（待完成）

ArActionKeydrive control;

ArPose min_pose;

ArPose max_pose;

ArPose var_pose;

ArPose mid_pose;

ArPose ori_pose;   //原点 origin

int min,max,var;

int i;

int r;  //期望的半径

int v;  //一次最多移动的距离

 

while(1){

 

 

min=5000;

max=0;

for(i=0;i<=60;i++)

{

    control.left();  //左转一度

    for (i = 0; i <= 7; ++i)

     {

         var=robot1.getSonarRange(i));

        if(var<=min)  //查看距离，满足条件存储最大或最小值的点，以ArPose的格式储存

              set_min_pose();

         else if(var>=max)

              set_max_pose();

 

     }

}

 

mid_pose=middle(min_pose,max_pose); //取中点

d=length(mid_pose，ori_pose;)

 

if (d>=r) gomove(mid_pose);

else (d<=r) backmove(mid_pose);  //开始移

delay();  //延时等待移动完成

/*重新设置中参数de代码*/

ori_pose.setPose(0,0,0);

 

}