pioneer vs 实现简单四边形移动 和 简单避障

#include "Aria.h"
#include <iostream>
using namespace std;


int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  // Connect to the robot, get some initial data from it such as type and name,
  // and then load parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "simpleConnect: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        // -help not given
        Aria::logOptions();
        Aria::exit(1);
    }
  }
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
  }
  ArLog::log(ArLog::Normal, "simpleConnect: Connected to robot.");
  robot.enableMotors();
  // Start the robot processing cycle running in the background.
  // True parameter means that if the connection is lost, then the 
  // run loop ends.
  robot.runAsync(true);
  // Print out some data from the SIP.  We must "lock" the ArRobot object
  // before calling its methods, and "unlock" when done, to prevent conflicts
  // with the background thread started by the call to robot.runAsync() above.
  // See the section on threading in the manual for more about this.
 
 for(;;)
 {
	if(robot.getX()<1000)
	 {
		  robot.lock();
		 robot.setVel(100);
		 robot.unlock();
		 ArUtil::sleep(500);
		 cout<<"forwardx-->"<<robot.getX()<<"_y_"<<robot.getY()<<"_th_"<<robot.getTh()<<"_b_"<<robot.getBatteryVoltage()<<endl;
	}
	else
		break;
 }
for(;;)
 {
	if(robot.getTh()<90)
	 {
		 robot.lock();
		 robot.setRotVel(10);
		 robot.unlock();
		 ArUtil::sleep(500);
		 cout<<"1--turn90"<<robot.getX()<<"_y_"<<robot.getY()<<"_th_"<<robot.getTh()<<"_b_"<<robot.getBatteryVoltage()<<endl;
	 }
	else
		{
			robot.clearDirectMotion();
			break;
		}
 }
  for(;;)
 {
	if(robot.getY()<1000)
	 {
		 robot.lock();
		 robot.setRotVel(0);
		 robot.setVel(100);
		 robot.unlock();
		 ArUtil::sleep(500);
		  cout<<"Y||^"<<robot.getX()<<"_y_"<<robot.getY()<<"_th_"<<robot.getTh()<<"_b_"<<robot.getBatteryVoltage()<<endl;
	}
	else
		break;
 }

  for(;;)
 {
	if(robot.getTh()<180 && robot.getTh()>0)
	 {
		 robot.lock();
		 robot.setRotVel(10);
		 robot.unlock();
		 ArUtil::sleep(500);
		 cout<<"2turn90"<<robot.getX()<<"_y_"<<robot.getY()<<"_th_"<<robot.getTh()<<"_b_"<<robot.getBatteryVoltage()<<endl;
	 }
	else
		{
			robot.clearDirectMotion();
			break;
		}
 }

   for(;;)
 {
	if(robot.getX()>0)
	 {
		 robot.lock();
		 robot.setRotVel(0);
		 robot.setVel(100);
		 robot.unlock();
		 ArUtil::sleep(500);
		  cout<<"backwordX <--"<<robot.getX()<<"_y_"<<robot.getY()<<"_th_"<<robot.getTh()<<"_b_"<<robot.getBatteryVoltage()<<endl;
	}
	else
		break;
 }

   for(;;)
 {
	if(robot.getTh()>(-1800)&&robot.getTh()<(-90))
	 {
		 robot.lock();
		 robot.setRotVel(10);
		 robot.unlock();
		 ArUtil::sleep(500);
		 cout<<"3--turn90"<<robot.getX()<<"_y_"<<robot.getY()<<"_th_"<<robot.getTh()<<"_b_"<<robot.getBatteryVoltage()<<endl;
	 }
	else
		{
			robot.clearDirectMotion();
			break;
		}
 }

    for(;;)
 {
	if(robot.getY()>0)
	 {
		 robot.lock();
		 robot.setRotVel(0);
		 robot.setVel(100);
		 robot.unlock();
		 ArUtil::sleep(500);
		  cout<<"backforwardY_4_x_"<<robot.getX()<<"_y_"<<robot.getY()<<"_th_"<<robot.getTh()<<"_b_"<<robot.getBatteryVoltage()<<endl;
	}
	else
		break;
 }
	robot.stop();

  robot.waitForRunExit();
  // exit
  ArLog::log(ArLog::Normal, "simpleConnect: Exiting.");
  Aria::exit(0);
  return 0;
}

simple sonar avoid obs----- turn to left or right

#include "Aria.h"
#include <iostream>
using namespace std;


int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  // Connect to the robot, get some initial data from it such as type and name,
  // and then load parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "simpleConnect: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        // -help not given
        Aria::logOptions();
        Aria::exit(1);
    }
  }
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
  }
  ArLog::log(ArLog::Normal, "simpleConnect: Connected to robot.");
  robot.enableMotors();
  // Start the robot processing cycle running in the background.
  // True parameter means that if the connection is lost, then the 
  // run loop ends.
  robot.runAsync(true);
  // Print out some data from the SIP.  We must "lock" the ArRobot object
  // before calling its methods, and "unlock" when done, to prevent conflicts
  // with the background thread started by the call to robot.runAsync() above.
  // See the section on threading in the manual for more about this.
 int left=1,right=-1;
 for(;;)
 {
	if(robot.getX()<1000)
	 {
		 int n = 8;
		 double sonarValue[8]={0};
		 for(int i = 0; i< n;i++)
			 sonarValue[i]=robot.getSonarRange(i);
		 int minNum=0;
		 double minRange = sonarValue[0];
		 for(int i=1;i<n;i++)
		 {
			if(sonarValue[i] < minRange)
				{
					minRange = sonarValue[i];
					minNum = i;
				}
		 }
		 if(minNum < 4 && minNum > 0)
		 {
		  robot.lock();
		 robot.setVel2(100,-100);
		 robot.unlock();
		 ArUtil::sleep(500);
		 }
		 else
		 {
		 robot.lock();
		 robot.setVel2(-100,100);
		 robot.unlock();
		 }
		 ArUtil::sleep(500);
		 cout<<"forwardx-->"<<robot.getX()<<"_y_"<<robot.getY()<<"_th_"<<robot.getTh()<<"_b_"<<robot.getBatteryVoltage()<<endl;
	}
	else
		break;
 }

	robot.stop();

  robot.waitForRunExit();
  // exit
  ArLog::log(ArLog::Normal, "simpleConnect: Exiting.");
  Aria::exit(0);
  return 0;
}