wander.cpp

移动机械臂的漫游程序,使用者可以根据需要修改,它同时支持先锋机器人

#include <fstream>
#include <string>
#include <iostream>
#include "Aria.h"

using namespace std;

/*  This program will just have the robot wander around, it uses some avoidance 
  routines, then just has a constant velocity.

  You can press escape while it was running to cause the program to
  close up and exit.

*/
  ArRobot robot;
  ArSonarDevice sonar;
  
  void robotMove(int distance1);
  void robotTurn(int angle);
   ofstream ballon;
  string str;
  char buff[100];
 
  ArPose Position;
  ArPose PositionEnd;
  ArSensorReading *readings;
  ArPose sonarpose;
  ArPose robotNewPose(0,0,0);
  
//  PositionEnd.setPose(2500,1500);
int main(int argc, char **argv)
{
  
  Aria::init();

  ArSimpleConnector connector(&argc, argv);

  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }

 
  // add the sonar to the robot
  robot.addRangeDevice(&sonar);
  
  // try to connect, if we fail exit
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  // turn on the motors, turn off amigobot sounds
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);
    //printf("sonar number=%d  sonar range=%d\n",robot.getNumSonar(),robot.getSonarRange(7));
	robot.runAsync(true);
	robotMove(1500);   //前进1.5米
	robotTurn(90);     //左转90度，右转为负
	robotMove(2000);
	robotTurn(90);
	robotMove(1500);
	robotTurn(90);
	robotMove(2000);
	robot.lock();
	robot.disconnect();
	robot.unlock();
	Aria::shutdown();
	return 0;
}
void robotMove(int distance1)
{
	robot.move(distance1);
	do {
		
	} while(!robot.isMoveDone());
}
void robotTurn(int angle)
{
	robot.setDeltaHeading(angle);
	do {
		
	} while(!robot.isHeadingDone());
	
}