C++ ArRobot::addAction方法代码示例

int main(int argc, char** argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  ArSonarDevice sonar;

  // 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, "actionExample: 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, "actionExample: Connected to robot.");

  // Create instances of the actions defined above, plus ArActionStallRecover, 
  // a predefined action from Aria.
  ActionGo go(500, 350);
  ActionTurn turn(400, 10);
  ArActionStallRecover recover;

    
  // Add the range device to the robot. You should add all the range 
  // devices and such before you add actions
  robot.addRangeDevice(&sonar);

 
  // Add our actions in order. The second argument is the priority, 
  // with higher priority actions going first, and possibly pre-empting lower
  // priority actions.
  robot.addAction(&recover, 100);
  robot.addAction(&go, 50);
  robot.addAction(&turn, 49);

  // Enable the motors, disable amigobot sounds
  robot.enableMotors();

  // Run the robot processing cycle.
  // 'true' means to return if it loses connection,
  // after which we exit the program.
  robot.run(true);
  
  Aria::exit(0);
}

int main(int argc, char** argv)
{
  Aria::init();

  ArSimpleConnector conn(&argc, argv);
  ArRobot robot;
  ArSonarDevice sonar;

  // Create instances of the actions defined above, plus ArActionStallRecover, 
  // a predefined action from Aria.
  ActionGo go(500, 350);
  ActionTurn turn(400, 10);
  ArActionStallRecover recover;

    
  // Parse all command-line arguments
  if(!Aria::parseArgs())
  {
    Aria::logOptions();
    return 1;
  }
  
  // Connect to the robot
  if(!conn.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "actionExample: Could not connect to robot! Exiting.");
    return 2;
  }

  // Add the range device to the robot. You should add all the range 
  // devices and such before you add actions
  robot.addRangeDevice(&sonar);

 
  // Add our actions in order. The second argument is the priority, 
  // with higher priority actions going first, and possibly pre-empting lower
  // priority actions.
  robot.addAction(&recover, 100);
  robot.addAction(&go, 50);
  robot.addAction(&turn, 49);

  // Enable the motors, disable amigobot sounds
  robot.enableMotors();

  // Run the robot processing cycle.
  // 'true' means to return if it loses connection,
  // after which we exit the program.
  robot.run(true);
  
  Aria::shutdown();
  return 0;
}

int main(int argc, char **argv)
{

  std::string str;
  int ret;
  ArTcpConnection con;
  ArRobot robot;

  ActionTest at1(-50, 333);
  ActionTest at2(25, 666);
  ActionTest at3(25, 0);
  ActionTest at4(0, -999);

  Aria::init();
  
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.addAction(&at1, 100);
  robot.addAction(&at2, 100);
  robot.addAction(&at3, 100);
  robot.addAction(&at4, 100);

  robot.run(true);
  Aria::shutdown();
  return 0;



}

int main(void)
{
  ArTcpConnection con;
  ArRobot robot;
  int ret;
  std::string str;
  JoydriveAction jdAct;
  FillerThread ft;

  ft.create();

  FillerThread ft2;

  ft2.create();

  Aria::init();
  /*
  if (!jdAct.joystickInited())
  {
    printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
    Aria::shutdown();
    return 1;
  }
  */
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.comInt(ArCommands::SONAR, 0);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  lastLoopTime.setToNow();
  loopTime = robot.getCycleTime();

  robot.addAction(&jdAct, 100);
  robot.runAsync(true);
  
  robot.waitForRunExit();
  Aria::shutdown();
  return 0;
}

int main(void)
{
  ArTcpConnection con;

  ArRobot robot;
  ArSonarDevice sonar;

  int ret;
  std::string str;
  ArActionStallRecover recover;
  ArActionConstantVelocity constantVelocity("Constant Velocity", 400);

  Aria::init();

  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  robot.addRangeDevice(&sonar);
  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  robot.addAction(&recover, 100);
  robot.addAction(&constantVelocity, 25);
  robot.run(true);
  
  Aria::shutdown();
  return 0;
}

int main()
{

/*
	TODO
	Check that the Starclass is updating the global pose right	- not done
	Check that the MapClass is giving the right pose			- not done
	check khenglee can use the behaviours						- not done
	check emergency control										- not done

*/
	Network yarp;

	SamgarModule VR("Vrobot","Loco","wheel",SamgarModule::run); // Cant have spaces or underscores
	VR.AddPortS("TransitIn");
	SetupRobot();


	ActionEmergencyControl EmergencyControl;
	UpdateMap		UpdMap		(&VR);
	UpOdo           OdoUp       (&VR);
	Transit         TransitIn   (&VR);
//	PlaySounder		SoundPlayer	(&VR);
//	BehaveMove		MoveBehave	(&VR);
//	MoveCAM			CAMMove		(&VR);

	// lowest priority might actully be highest becouse coms direct to robot and not through desiredaction.
	robot.addAction(&EmergencyControl,99); // need to check this works
	robot.addAction(&UpdMap,99);
	robot.addAction(&OdoUp,99);
	robot.addAction(&TransitIn,70);
//	robot.addAction(&SoundPlayer,100);
//	robot.addAction(&MoveBehave,100);
//	robot.addAction(&CAMMove,100);
	robot.run(true);

	robot.disconnect();
	Aria::shutdown();
	return 0;
}

int main(int argc, char **argv)
{
  ArRobot robot;
  Aria::init();
  ArSimpleConnector connector(&argc, argv);
  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    return 1;
  }

  // Instance of the JoydriveAction class defined above
  JoydriveAction jdAct;

  // if the joydrive action couldn't find the joystick, then exit.
  if (!jdAct.joystickInited())
  {
    printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
    Aria::exit(1);
    return 1;
  }
  
  // Connect to the robot
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    return 2;
  }


  // disable sonar, enable motors, disable amigobot sound
  robot.comInt(ArCommands::SONAR, 0);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  // add the action
  robot.addAction(&jdAct, 100);
  // run the robot, true so it'll exit if we lose connection
  robot.run(true);
  
  // now exit program
  Aria::exit(0);
  return 0;
}

int main(int argc, char **argv)
{
	Aria::init();
	ArArgumentParser argParser(&argc, argv);
	argParser.loadDefaultArguments();
	ArRobot robot;
	ArSick laser;
	std::ofstream stream; // for loggin
	time_t timer;
	char buffer[120];
	//for loggin
	ArRobotConnector robotConnector(&argParser, &robot);
	ArLaserConnector laserConnector(&argParser, &robot, &robotConnector);
	int32_t count = 0;
	readyLog(stream);
	if (!robotConnector.connectRobot())
	{
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if (argParser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}


	// Trigger argument parsing
	if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
	{
		Aria::logOptions();
		Aria::exit(1);
		return 1;
	}

	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);
	robot.attachKeyHandler(&keyHandler);

	puts("This program will make the robot wander around. It uses some avoidance\n"
		"actions if obstacles are detected, otherwise it just has a\n"
		"constant forward velocity.\n\nPress CTRL-C or Escape to exit.");

	//ArSonarDevice sonar;
	//robot.addRangeDevice(&sonar);
	robot.addRangeDevice(&laser);

	robot.runAsync(true);



	// try to connect to laser. if fail, warn but continue, using sonar only
	if (!laserConnector.connectLasers())
	{
		ArLog::log(ArLog::Normal, "Warning: unable to connect to requested lasers, will wander using robot sonar only.");
	}

	double sampleAngle, dist;
	auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle);
	auto degreeStr = laser.getDegreesChoicesString();

	std::cout << "auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle);" << sampleRange << std::endl;
	std::cout << "auto degreeStr = laser.getDegreesChoicesString(); : " << degreeStr << std::endl;


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

	// add a set of actions that combine together to effect the wander behavior
	ArActionStallRecover recover;
	ArActionBumpers bumpers;
	ArActionAvoidFront avoidFrontNear("Avoid Front Near", 100, 0);
	ArActionAvoidFront avoidFrontFar;
	ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
	
	robot.addAction(&recover, 100);
	robot.addAction(&bumpers, 75);
	robot.addAction(&avoidFrontNear, 50);
	robot.addAction(&avoidFrontFar, 49);
	robot.addAction(&constantVelocity, 25);

	stream << "IMPORTANT !! == robot.getRobotRadius() : " << robot.getRobotRadius << std::endl;
	std::string timestamp;
	while (1)
	{
		//

		//
		time(&timer);
		strftime(buffer, 80, " - timestamp : %I:%M:%S", localtime(&timer));
		timestamp = buffer;
		dist = robot.checkRangeDevicesCurrentPolar(-70, 70, &sampleAngle) - robot.getRobotRadius();
		stream << count << timestamp << "checkRangeDevicesCurrentPolar(-70, 70, &angle)  :  " << dist << std::endl;
		Sleep(500);
		count++;
	}

//.........这里部分代码省略.........

int main(int argc, char **argv) 
{

  Aria::init();
  ArRobot robot;
  ArArgumentParser argParser(&argc, argv);
  ArSimpleConnector connector(&argParser);
  ArGripper gripper(&robot);
  ArSonarDevice sonar;
  robot.addRangeDevice(&sonar);

  argParser.loadDefaultArguments();

  if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::shutdown();
    return 1;
  }
  
  if (!connector.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "gripperExample: Could not connect to robot... exiting");
    Aria::shutdown();
    return 2;
  }
  ArLog::log(ArLog::Normal, "gripperExample: Connected to robot.");

  ArLog::log(ArLog::Normal, "gripperExample: GripperType=%d", gripper.getType());
  gripper.logState();
  if(gripper.getType() == ArGripper::NOGRIPPER)
  {
    ArLog::log(ArLog::Terse, "gripperExample: Error: Robot does not have a gripper. Exiting.");
    Aria::shutdown();
    return -1;
  }

  // Teleoperation actions with obstacle-collision avoidance
  ArActionLimiterTableSensor tableLimit;
  robot.addAction(&tableLimit, 110);
  ArActionLimiterForwards limitNearAction("near", 300, 600, 250);
  robot.addAction(&limitNearAction, 100);
  ArActionLimiterForwards limitFarAction("far", 300, 1100, 400);
  robot.addAction(&limitFarAction, 90);
  ArActionLimiterBackwards limitBackAction;
  robot.addAction(&limitBackAction, 50);
  ArActionJoydrive joydriveAction("joydrive", 400, 15);
  robot.addAction(&joydriveAction, 40);
  joydriveAction.setStopIfNoButtonPressed(false);
  ArActionKeydrive keydriveAction;
  robot.addAction(&keydriveAction, 30);
  

  // Handlers to control the gripper and print out info (classes defined above)
  PrintGripStatus printStatus(&gripper);
  GripperControlHandler gripControl(&gripper);
  printStatus.addRobotTask(&robot);
  gripControl.addKeyHandlers(&robot);

  // enable motors and run (if we lose connection to the robot, exit)
  ArLog::log(ArLog::Normal, "You may now operate the robot with arrow keys or joystick. Operate the gripper with the u, d, o, c, and page up/page down keys.");
  robot.enableMotors();
  robot.run(true);
  
  Aria::shutdown();
  return 0;
}

int main(int argc, char **argv)
{


  Aria::init();

  argc = 3;
  argv[0] = "";
  argv[1] = "-rp";
  argv[2] = "/dev/ttyUSB0";
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  ArAnalogGyro gyro(&robot);
  ArSonarDevice sonar;
  ArRobotConnector robotConnector(&parser, &robot);

  if(!robotConnector.connectRobot())
  {
    if(parser.checkHelpAndWarnUnparsed())
    {
        Aria::logOptions();
        Aria::exit(1);
    }
  }

  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
  }

 imgdb.setRobot(&robot);

  int Rc;
  pthread_t slam_thread;
  Rc = pthread_create(&slam_thread, NULL, slam_event, NULL);
  if (Rc) { printf("Create slam thread failed!\n"); exit(-1); }

  ImageList* current = NULL;
  while (!current) current = imgdb.getEdge();
    // current->person_point


  robot.addRangeDevice(&sonar);
  robot.runAsync(true);

  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit\n");

  robot.setAbsoluteMaxRotVel(30);

  ArActionStallRecover recover;
  ArActionBumpers bumpers;
  double minDistance=10000;
 // ArPose endPoint(point.x, point.z);

//  current->collect_x
//          current->collect_y
//          robot.getX()
//          robot.getY()
//  current = imgdb.getEdge();

//  for (int i = 0; i < current->edge.size(); i++) {
//      for (int j = 0; j < current->edge[i].size() - 1; j++) {
//          double dis = sqrt(pow(current->edge[i][j].z,2)+(pow(current->edge[i][j].x,2)));
//           if(dis<minDistance){
//              minDistance=dis;
//           }
//      }
//  }

 // 引入避障action,前方安全距离为500mm,侧边安全距离为340mm
  Avoid avoidSide("Avoid side", ArPose(0, 0), 800, 100, 1000*minDistance);



  robot.addAction(&recover, 100);
  robot.addAction(&bumpers, 95);
  robot.addAction(&avoidSide, 80);

  ArActionStop stopAction("stop");
  robot.addAction(&stopAction, 50);

  robot.enableMotors();
  robot.comInt(ArCommands::SOUNDTOG, 0);


  const int duration = 500000;

  bool first = true;
  int goalNum = 0;
  ArTime start;
  start.setToNow();

  while (Aria::getRunning())
  {
    current = imgdb.getEdge();
//.........这里部分代码省略.........

int main(void)
{
  ArSerialConnection con;
  ArRobot robot;
  int ret;
  std::string str;
  ArActionLimiterForwards limiter("speed limiter near", 225, 600, 250);
  ArActionLimiterForwards limiterFar("speed limiter far", 225, 1100, 400);
  ArActionTableSensorLimiter tableLimiter;
  ArActionLimiterBackwards backwardsLimiter;
  ArActionConstantVelocity stop("stop", 0);
  ArSonarDevice sonar;
  ArACTS_1_2 acts;
  ArPTZ *ptz;
  ptz = new ArVCC4(&robot, true);
  ArGripper gripper(&robot);
  
  Acquire acq(&acts, &gripper);
  DriveTo driveTo(&acts, &gripper, ptz);
  DropOff dropOff(&acts, &gripper, ptz);
  PickUp pickUp(&acts, &gripper, ptz);
  

  TakeBlockToWall takeBlock(&robot, &gripper, ptz, &acq, &driveTo, &pickUp,
			    &dropOff, &tableLimiter);

  if (!acts.openPort(&robot))
  {
    printf("Could not connect to acts, exiting\n");
    exit(0);    
  }
  Aria::init();
  
  robot.addRangeDevice(&sonar);
  //con.setBaud(38400);
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  ptz->init();
  ArUtil::sleep(8000);
  printf("### 2222\n");
  ptz->panTilt(0, -40);
  printf("### whee\n");
  ArUtil::sleep(8000);
  robot.setAbsoluteMaxTransVel(400);

  robot.setStateReflectionRefreshTime(250);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  ArUtil::sleep(200);
  robot.addAction(&tableLimiter, 100);
  robot.addAction(&limiter, 99);
  robot.addAction(&limiterFar, 98);
  robot.addAction(&backwardsLimiter, 97);
  robot.addAction(&acq, 77);
  robot.addAction(&driveTo, 76);
  robot.addAction(&pickUp, 75);
  robot.addAction(&dropOff, 74);
  robot.addAction(&stop, 30);

  robot.run(true);
  
  Aria::shutdown();
  return 0;
}

int __cdecl _tmain (int argc, char** argv)
{

	//------------ I N I C I O   M A I N    D E L   P R O G R A M A   D E L    R O B O T-----------//

	  //inicializaion de variables
	  Aria::init();
	  ArArgumentParser parser(&argc, argv);
	  parser.loadDefaultArguments();
	  ArSimpleConnector simpleConnector(&parser);
	  ArRobot robot;
	  ArSonarDevice sonar;
	  ArAnalogGyro gyro(&robot);
	  robot.addRangeDevice(&sonar);
	  ActionGos go(500, 350);	  
	  robot.addAction(&go, 48);
	  ActionTurns turn(400, 110);
	  robot.addAction(&turn, 49);
	  ActionTurns turn2(400, 110);
	  robot.addAction(&turn2, 49);

	  // presionar tecla escape para salir del programa
	  ArKeyHandler keyHandler;
	  Aria::setKeyHandler(&keyHandler);
	  robot.attachKeyHandler(&keyHandler);
	  printf("Presionar ESC para salir\n");

	  // uso de sonares para evitar colisiones con las paredes u 
	  // obstaculos grandes, mayores a 8cm de alto
	  ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250);
	  ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400);
	  ArActionLimiterTableSensor tableLimiterAction;
	  robot.addAction(&tableLimiterAction, 100);
	  robot.addAction(&limiterAction, 95);
	  robot.addAction(&limiterFarAction, 90);


	  // Inicializon la funcion de goto
	  ArActionGoto gotoPoseAction("goto");
	  robot.addAction(&gotoPoseAction, 50);
	  
	  // Finaliza el goto si es que no hace nada
	  ArActionStop stopAction("stop");
	  robot.addAction(&stopAction, 40);

	  // Parser del CLI
	  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
	  {    
		Aria::logOptions();
		exit(1);
	  }
	  
	  // Conexion del robot
	  if (!simpleConnector.connectRobot(&robot))
	  {
		printf("Could not connect to robot... exiting\n");
		Aria::exit(1);
	  }
	  robot.runAsync(true);

	  // enciende motores, apaga sonidos
	  robot.enableMotors();
	  robot.comInt(ArCommands::SOUNDTOG, 0);

	  // Imprimo algunos datos del robot como posicion velocidad y bateria
		robot.lock();
		ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
			robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
		robot.unlock();

	  const int duration = 100000; //msec
	  ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);

	  // ============================ INICIO CONFIG COM =================================//
	    CSerial serial;
		LONG    lLastError = ERROR_SUCCESS;

		// Trata de abrir el com seleccionado
		lLastError = serial.Open(_T("COM3"),0,0,false);
		if (lLastError != ERROR_SUCCESS)
			return ::ShowError(serial.GetLastError(), _T("Imposible abrir el COM"));

		// Inicia el puerto serial (9600,8N1)
		lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
		if (lLastError != ERROR_SUCCESS)
			return ::ShowError(serial.GetLastError(), _T("Imposible setear la config del COM"));

		// Register only for the receive event
		lLastError = serial.SetMask(CSerial::EEventBreak |
									CSerial::EEventCTS   |
									CSerial::EEventDSR   |
									CSerial::EEventError |
									CSerial::EEventRing  |
									CSerial::EEventRLSD  |
									CSerial::EEventRecv);
		if (lLastError != ERROR_SUCCESS)
			return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask"));

		// Use 'non-blocking' reads, because we don't know how many bytes
		// will be received. This is normally the most convenient mode
//.........这里部分代码省略.........

int main(void)
{
  // The connection we'll use to talk to the robot
  ArTcpConnection con;
  // the robot
  ArRobot robot;
  // the sonar device
  ArSonarDevice sonar;

  // some stuff for return values
  int ret;
  std::string str;

  // the behaviors from above, and a stallRecover behavior that uses defaults
  ActionGo go(500, 350);
  ActionTurn turn(400, 30);
  ArActionStallRecover recover;

  // this needs to be done
  Aria::init();

  // open the connection, just using the defaults, if it fails, exit
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  // add the range device to the robot, you should add all the range 
  // devices and such before you add actions
  robot.addRangeDevice(&sonar);
  // set the robot to use the given connection
  robot.setDeviceConnection(&con);
  
  // do a blocking connect, if it fails exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  // enable the motors, disable amigobot sounds
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  // add our actions in a good order, the integer here is the priority, 
  // with higher priority actions going first
  robot.addAction(&recover, 100);
  robot.addAction(&go, 50);
  robot.addAction(&turn, 49);
  
  // run the robot, the true here is to exit if it loses connection
  robot.run(true);
  
  // now just shutdown and go away
  Aria::shutdown();
  return 0;
}

int main(int argc, char** argv)
{
  Aria::init();
  ArLog::init(ArLog::StdErr, ArLog::Normal);

  ArArgumentParser argParser(&argc, argv);
  ArSimpleConnector connector(&argParser);
  ArGPSConnector gpsConnector(&argParser);
  ArRobot robot;

  ArActionLimiterForwards nearLimitAction("limit near", 300, 600, 250);
  ArActionLimiterForwards farLimitAction("limit far", 300, 1100, 400);
  ArActionLimiterBackwards limitBackwardsAction;
  ArActionJoydrive joydriveAction;
  ArActionKeydrive keydriveAction;

  ArSonarDevice sonar;
  ArSick laser;

  argParser.loadDefaultArguments();
  if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    return -1;
  }

  robot.addRangeDevice(&sonar);
  robot.addRangeDevice(&laser);

  ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connecting to robot...");
  if(!connector.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "gpsRobotTaskExample: Could not connect to the robot. Exiting.");
    return -2;
  }
  ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connected to the robot.");


  // Connect to GPS
  ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connecting to GPS, it may take a few seconds...");
  ArGPS *gps = gpsConnector.createGPS();
  if(!gps || !gps->connect());
  {
    ArLog::log(ArLog::Terse, "gpsRobotTaskExample: Error connecting to GPS device.  Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help. Exiting.");
    return -3;
  }


  // Create an GPSLogTask which will register a task with the robot.
  GPSLogTask gpsTask(&robot, gps, joydriveAction.getJoyHandler()->haveJoystick() ? joydriveAction.getJoyHandler() : NULL);


  // Add actions
  robot.addAction(&nearLimitAction, 100);
  robot.addAction(&farLimitAction, 90);
  robot.addAction(&limitBackwardsAction, 80);
  robot.addAction(&joydriveAction, 50);
  robot.addAction(&keydriveAction, 40);

  // allow keydrive action to drive robot even if joystick button isn't pressed
  joydriveAction.setStopIfNoButtonPressed(false);

  // Start the robot  running
  robot.runAsync(true);

  // Connect to the laser
  connector.setupLaser(&laser);
  laser.runAsync();
  if(!laser.blockingConnect())
    ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Warning, could not connect to SICK laser, will not use it.");

  robot.lock();

  robot.enableMotors();
  robot.comInt(47, 1);  // enable joystick driving on some robots

  // Add exit callback to reset/unwrap steering wheels on seekur (critical if the robot doesn't have sliprings); does nothing for other robots 
  Aria::addExitCallback(new ArRetFunctor1C<bool, ArRobot, unsigned char>(&robot, &ArRobot::com, (unsigned char)120));
  Aria::addExitCallback(new ArRetFunctor1C<bool, ArRobot, unsigned char>(&robot, &ArRobot::com, (unsigned char)120));

  robot.unlock();

  ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Running... (drive robot with joystick or arrow keys)");
  robot.waitForRunExit();


  return 0;
}

int APIENTRY _tWinMain(HINSTANCE hInstance,
                     HINSTANCE hPrevInstance,
                     LPTSTR    lpCmdLine,
                     int       nCmdShow)
{
	UNREFERENCED_PARAMETER(hPrevInstance);
	UNREFERENCED_PARAMETER(lpCmdLine);

  //-------------- M A I N    D E L   P R O G R A M A   D E L    R O B O T------------//
  //----------------------------------------------------------------------------------------------------------
	
  //inicializaion de variables
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArSimpleConnector simpleConnector(&parser);
  ArRobot robot;
  ArSonarDevice sonar;
  ArAnalogGyro gyro(&robot);
  robot.addRangeDevice(&sonar);

  // presionar tecla escape para salir del programa
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit\n");

  // uso de sonares para evitar colisiones con las paredes u 
  // obstaculos grandes, mayores a 8cm de alto
  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);


  // Inicializon la funcion de goto
  ArActionGoto gotoPoseAction("goto");
  robot.addAction(&gotoPoseAction, 50);
  
  // Finaliza el goto si es que no hace nada
  ArActionStop stopAction("stop");
  robot.addAction(&stopAction, 40);

  // Parser del CLI
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    exit(1);
  }
  
  // Conexion del robot
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::exit(1);
  }
  robot.runAsync(true);

  // enciende motores, apaga sonidos
  robot.enableMotors();
  robot.comInt(ArCommands::SOUNDTOG, 0);

  const int duration = 100000; //msec
  ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);

  bool first = true;
  int horiz = 1800;
  int vert = 380;
  int goalNum = 0;
  ArTime start;
  start.setToNow();
  while (Aria::getRunning()) 
  {
    robot.lock();
    // inicia el primer punto 
    if (first || gotoPoseAction.haveAchievedGoal())
    {
      first = false;
	  
      goalNum++; //cambia de 0 a 1 el contador
      if (goalNum > 7)
        goalNum = 1;

	  //comienza la secuencia de puntos
      if (goalNum == 1)
        gotoPoseAction.setGoal(ArPose(horiz, vert*0));
      else if (goalNum == 2)
        gotoPoseAction.setGoal(ArPose(0, vert*1));
      else if (goalNum == 3)
        gotoPoseAction.setGoa l(ArPose(horiz, vert*2)+5);
      else if (goalNum == 4)
        gotoPoseAction.setGoal(ArPose(0, vert*3));
	  else if (goalNum == 5)
        gotoPoseAction.setGoal(ArPose(horiz, vert*4+5));
	  else if (goalNum == 6)
        gotoPoseAction.setGoal(ArPose(0, vert*5));
	  else if (goalNum == 7)
        gotoPoseAction.setGoal(ArPose(0, vert*0));
//.........这里部分代码省略.........