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);
}