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