pioneer vs中保存某些数据量到文件文本

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

ofstream positiondata("positiondata.txt");
double posData[5]={0};
int frameCount =0;
void grabNewPositionData(ArRobot* robot){
    frameCount++;
    posData[0] = robot->getX();
    posData[1] = robot->getY();
    posData[2] = robot->getTh();
    posData[3] = robot->getLeftVel();
    posData[4] = robot->getRightVel();
}



/**************************************
Log the current Odometry values
**************************************/
void logPositionData(ArRobot* robot){

    if(frameCount==0){
        positiondata << "X\tY\tTh\tlVel\trVel" << endl; 
    }

    positiondata << posData[0] << "\t" <<
                    posData[1] << "\t" <<
                    posData[2] << "\t" <<
                    posData[3] << "\t" <<
                    posData[4] << endl;

}

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;
         grabNewPositionData(&robot);
         logPositionData(&robot);
    }
    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;
         grabNewPositionData(&robot);
         logPositionData(&robot);
     }
    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;
          grabNewPositionData(&robot);
         logPositionData(&robot);
    }
    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;
         grabNewPositionData(&robot);
         logPositionData(&robot);
     }
    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;
          grabNewPositionData(&robot);
         logPositionData(&robot);
    }
    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;
         grabNewPositionData(&robot);
         logPositionData(&robot);
     }
    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;
          grabNewPositionData(&robot);
         logPositionData(&robot);
    }
    else
        break;
 }
    robot.stop();

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