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

最近学习上的事比较多，复习随机过程，还有准备今天下午计算机系统机构的讲课，最主要是这个机器人圆形队列算法的具体函数实现写了加起来有10来个小时吧，今天终于把一些难点搞定，之前总没有意识到八个声纳是有角度的，每次测坐标的时候都没有加修正值，走了很多了弯路，现在到了正轨，还有对于机器人自我坐标清零函数一直没有在开发库里找到，害的我在几千行的开发库的原cpp文档找对应的函数，眼睛都为此上升的几度，最中和巧合的在用户培训手册看到了这个函数接口，众里寻他千百度，得来全不费工夫哈，程序还没完成，但都是一些小工作量的东西。先贴到博客里以示庆祝

 

#include "Aria.h"
#include <ariaUtil.h> //加入数学库
ArActionKeydrive control;
ArActionGoto gotoPoseAction;
ArRobot robot;
ArPose min_pose;
ArPose max_pose;
ArPose var_pose;
ArPose mid_pose;
ArPose ori_pose;   //原点 origin
double min_d,max_d,var_d;
int i,j;
double d;
double r=2000;  //期望的半径
double v=500;  //一次最多移动的距离
double fx,fy,fth;
double a[8]={90,45,15,-15,-45,-90,-150,150};

void set_min_pose(double var,int i)
{   double x,y,th,cos,sin;
    min_d=var;
    th=a[i]+robot.getTh();
    cos=ArMath::cos(th);
 sin=ArMath::sin(th);
    printf("%f/n",cos);
 x=cos*var;
 y=sin*var;
 min_pose.setX(x);
    min_pose.setY(y);
 printf("min_pose:%6f,%6f,%6f/n",x,y,th);

}

void set_max_pose(double var,int i)
{   double x,y,th,cos,sin;
    max_d=var;
    th=a[i]+robot.getTh();
    cos=ArMath::cos(th);
 sin=ArMath::sin(th);
    x=cos*var;
 y=sin*var;
 max_pose.setX(x);
    max_pose.setY(y);
 printf("th is:%f/n",th);
 printf("max_pose:%6f,%6f,%6f/n",x,y,th);
 
}

ArPose middle(const ArPose& min_pose,const ArPose& max_pose)
{   double x1,y1,x2,y2,x3,y3;
 min_pose.getPose(&x1,&y1);
    max_pose.getPose(&x2,&y2);
    x3=(x1+x2)/2;
    y3=(y1+y2)/2;
 mid_pose.setPose(x3,y3);
 return mid_pose;
}   

 
double length(const ArPose mid_pose,const ArPose ori_pose)
{ double length;
  length=ori_pose.findDistanceTo (mid_pose);
  return length;
}

void gomove(ArPose mid_pose)
{  double step;
   step=(d-r)>=v?(d-r):v;
   gotoPoseAction.setCloseDist (step) ;
   gotoPoseAction.setGoal(mid_pose);
   printf("gomove!!!!!!!!!/n");
}

void backmove(ArPose mid_pose)
{  double step;
   double x,y,th;
   step=(d-r)>=v?(d-r):v;
   mid_pose.getPose(&x,&y,&th);
   x=0-x;
   y=0-y;
   mid_pose.setPose (x,y,th);
  // gotoPoseAction.setCloseDist(step);
   gotoPoseAction.setGoal(mid_pose);
   printf("backmove!!!!!!!!!/n");
}

int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArSimpleConnector simpleConnector(&parser);
 
  ArSonarDevice sonar;
  ArAnalogGyro gyro(&robot);
  robot.addRangeDevice(&sonar);

  // Make a key handler, so that escape will shut down the program
  // cleanly
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit/n");

  // Collision avoidance actions at higher priority
  ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
  ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
  ArActionLimiterTableSensor tableLimiterAction;
 
  robot.addAction(&tableLimiterAction, 100);
  robot.addAction(&limiterAction, 95);
  robot.addAction(&limiterFarAction, 90);

  // Goto action at lower priority
  ArActionGoto gotoPoseAction("goto");
  robot.addAction(&gotoPoseAction, 50);
 
  // control action priority
  robot.addAction(&control, 45);

  // Stop action at lower priority, so the robot stops if it has no goal
  ArActionStop stopAction("stop");
  robot.addAction(&stopAction, 40);

  // Parse all command line arguments
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {   
    Aria::logOptions();
    exit(1);
  }
 
  // Connect to the robot
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting/n");
    Aria::exit(1);
  }
  robot.runAsync(true);

  // turn on the motors, turn off amigobot sounds
  robot.enableMotors();
  robot.comInt(ArCommands::SOUNDTOG, 0);

 
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$

  while (Aria::getRunning())
  {
  
robot.moveTo(ArPose(0,0,0));

printf("mark1");
min_d=5000;
max_d=0;

for(j=0;j<=300;j++)
{  
    control.left();  //左转一度
 
    for (i = 0; i <= 7; ++i)
 {
  var_d=robot.getSonarRange(i);
        printf("%d sonar is %f/n",i,var_d);
  if(var_d<=min_d)  //查看距离，满足条件存储最大或最小值的点，以ArPose的格式储存
   set_min_pose(var_d,i);
  if(var_d>=max_d&&var_d!=5000)
   set_max_pose(var_d,i);

 }
}

 

 

printf("/nmark3/n");
mid_pose=middle(min_pose,max_pose); //取中点
d=length(min_pose,ori_pose);
printf("d=%f/n",d);
//if (d>=r) gomove(mid_pose);
//else if (d<=r) backmove(mid_pose);  //开始移

gotoPoseAction.setCloseDist (d-v);
gotoPoseAction.setGoal(min_pose);

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

min_pose.getPose(&fx,&fy,&fth);
printf("min pose:%f,%f,%f/n",fx,fy,fth);
max_pose.getPose(&fx,&fy,&fth);
printf("max pose:%f,%f,%f/n",fx,fy,fth);
mid_pose.getPose(&fx,&fy,&fth);
printf("mid pose:%f,%f,%f/n",fx,fy,fth);
ori_pose.getPose(&fx,&fy,&fth);
printf("ori pose:%f,%f,%f/n",fx,fy,fth);

ArUtil::sleep(5000);

}

//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
   
 
 
  // Robot disconnected or time elapsed, shut down
  Aria::shutdown();
  return 0;
}