Пример #1
0
// turn on motors, and off sonar, and off amigobot sounds, when connected
void ConnHandler::connected(void)
{
  printf("Connected\n");
  myRobot->comInt(ArCommands::SONAR, 0);
  myRobot->comInt(ArCommands::ENABLE, 1);
  myRobot->comInt(ArCommands::SOUNDTOG, 0);
}
void ConnHandler::connected(void)
{
  ArLog::log(ArLog::Normal, "ConnHandler: Connected. Turning off sonar,");
  // turn off sonar, turn off amigobot sounds
  myRobot->comInt(ArCommands::SONAR, 0);
  myRobot->comInt(ArCommands::SOUNDTOG, 0);
}
Пример #3
0
int main(int argc, char** argv)
{
  // set up our simpleConnector
  ArSimpleConnector simpleConnector(&argc, argv);
  // robot
  ArRobot robot;
  // a key handler so we can do our key handling
  ArKeyHandler keyHandler;

  ArLog::init(ArLog::StdOut,ArLog::Verbose);

  // if there are more arguments left then it means we didn't
  // understand an option
  if (!simpleConnector.parseArgs() || argc > 1)
  {    
    simpleConnector.logOptions();
    keyHandler.restore();
    exit(1);
  }

  // mandatory init
  Aria::init();
  ArLog::init(ArLog::StdOut, ArLog::Terse, NULL, true);

  // let the global aria stuff know about it
  Aria::setKeyHandler(&keyHandler);
  // toss it on the robot
  robot.attachKeyHandler(&keyHandler);

  // set up the robot for connecting
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    keyHandler.restore();
    return 1;
  }

  // turn on the motors for the velocity response test
  robot.comInt(ArCommands::ENABLE, 1);
  velTime.setToNow();

  // turn off the sonar
  robot.comInt(ArCommands::SONAR, 0);

  ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
  robot.addUserTask("iotest", 100, &userTaskCB);

  robot.comInt(ArCommands::IOREQUEST, 1);
  requestTime.setToNow();

  //start the robot running, true so that if we lose connection the run stops
  robot.run(true);
  
  // now exit
  Aria::shutdown();
  return 0;


}
Пример #4
0
// turn on motors, and off sonar, and off amigobot sounds, when connected
void ConnHandler::connected(void)
{
	printf("directMotionDemo connection handler: Connected\n");
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::ENABLE, 1);
	myRobot->comInt(ArCommands::SOUNDTOG, 0);
}
Пример #5
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;
}
Пример #6
0
/*
 * To Read the parameter
*/
void RosAriaNode::readParameters()
{
  // Robot Parameters  
  robot->lock();
  ros::NodeHandle n_("~");
  if (n_.hasParam("TicksMM"))
  {
    n_.getParam( "TicksMM", TicksMM);
    ROS_INFO("Setting TicksMM from ROS Parameter: %d", TicksMM);
    robot->comInt(93, TicksMM);
  }
  else
  {
    TicksMM = robot->getOrigRobotConfig()->getTicksMM();
    n_.setParam( "TicksMM", TicksMM);
    ROS_INFO("Setting TicksMM from robot EEPROM: %d", TicksMM);
  }
  
  if (n_.hasParam("DriftFactor"))
  {
    n_.getParam( "DriftFactor", DriftFactor);
    ROS_INFO("Setting DriftFactor from ROS Parameter: %d", DriftFactor);
    robot->comInt(89, DriftFactor);
  }
  else
  {
    DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
    n_.setParam( "DriftFactor", DriftFactor);
    ROS_INFO("Setting DriftFactor from robot EEPROM: %d", DriftFactor);
  }
  
  if (n_.hasParam("RevCount"))
  {
    n_.getParam( "RevCount", RevCount);
    ROS_INFO("Setting RevCount from ROS Parameter: %d", RevCount);
    robot->comInt(88, RevCount);
  }
  else
  {
    RevCount = robot->getOrigRobotConfig()->getRevCount();
    n_.setParam( "RevCount", RevCount);
    ROS_INFO("Setting RevCount from robot EEPROM: %d", RevCount);
  }
  robot->unlock();
}
int main()
{
  ArModuleLoader::Status status;
  ArSerialConnection con;
  ArRobot robot;
  int ret;
  std::string str;

  Aria::init();

  status=ArModuleLoader::load("./joydriveActionMod", &robot);
  printStatus(status);

  if (status == ArModuleLoader::STATUS_INIT_FAILED)
    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);

  robot.run(true);
  
  status=ArModuleLoader::close("./joydriveActionMod");
  printStatus(status);

  Aria::shutdown();
  return 0;
}
Пример #8
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;
}
Пример #9
0
int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;

  ArSerialConnection serialConnection;
  ArTcpConnection tcpConnection;
    
  if (tcpConnection.open("localhost", 8101)) {
    robot.setDeviceConnection(&tcpConnection);
  } else {
    serialConnection.setPort("/dev/ttyUSB0");
    robot.setDeviceConnection(&serialConnection);
  }
  robot.blockingConnect();
   
  printf("Setting robot to run async\n");
  robot.runAsync(false);

  printf("Turning off sound\n");
  robot.comInt(ArCommands::SOUNDTOG, 0);

  printf("Enabling motors\n");
  robot.enableMotors();

  // add a set of actions that combine together to effect the wander behavior
  /*ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 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);*/

  printf("Locking\n");
  robot.lock();
  robot.setVel(100.0);
  robot.unlock();
  printf("Sleeping\n");
  ArUtil::sleep(3*1000);
  printf("Awake\n");

  
  // wait for robot task loop to end before exiting the program
  //while (true);
  //robot.waitForRunExit();
  

  Aria::exit(0);
  return 0;
}
Пример #10
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;
}
Пример #11
0
int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	

//	robot.runAsync(true);
	
//	ArMap map("office.map");
	// set it up to ignore empty file names (otherwise if a configuration omits
	// the map file, the whole configuration change will fail)
//	map.setIgnoreEmptyFileName(true);
	// ignore the case, so that if someone is using MobileEyes or
	// MobilePlanner from Windows and changes the case on a map name,
	// it will still work.
//	map.setIgnoreCase(true);


	robot.runAsync(true);
	robot.enableMotors();
	robot.moveTo(ArPose(0,0,0));
	//robot.lock();
	robot.comInt(ArCommands::ENABLE, 1);
	robot.addRangeDevice(&sonarDev);
	//robot.unlock();
//	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
<<<<<<< HEAD
Пример #12
0
int main(int argc, char **argv){
	Aria::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	ArSimpleConnector connector(& parser);

	parser.loadDefaultArguments();
	Aria::logOptions();
	if (!connector.parseArgs()){
		cout << "Unknown settings\n";
		Aria::exit(0);
		exit(1);
	}

	if (!connector.connectRobot(&robot)){
		cout << "Unable to connect\n";
		Aria::exit(0);
		exit(1);
	}

	robot.runAsync(true);
	robot.lock();
	robot.comInt(ArCommands::ENABLE, 1);
	robot.unlock();



	ArSonarDevice sonar;
	robot.addRangeDevice(&sonar);

	G_id = 0;
	G_SONAR_FD = fopen("../sensors/sonars","w");
	G_pose_fd  = fopen("../sensors/pose","w");
	int numSonar = robot.getNumSonar();
	while(1){
		readPosition(robot);
		readSonars(robot, 8);
		setMotors(robot);
		usleep(20000);
	}

	fclose(G_SONAR_FD);
	fclose(G_pose_fd);
	Aria::exit(0);
}
Пример #13
0
int main(int argc, char **argv) 
{
    Aria::init();
    ArArgumentParser parser(&argc, argv);
    parser.loadDefaultArguments();
    ArRobot robot;
    ArRobotConnector robotConnector(&parser, &robot);
    if(!robotConnector.connectRobot())
    {
      ArLog::log(ArLog::Terse, "dpptuExample: Could not connect to the robot.");
      if(parser.checkHelpAndWarnUnparsed())
      {
          Aria::logOptions();
          Aria::exit(1);
      }
    }
    if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
    {
      Aria::logOptions();
      Aria::exit(1);
    }
    ArLog::log(ArLog::Normal, "dpptuExample: Connected to robot.");

  robot.runAsync(true);

  // an object for keyboard control, class defined above, this also adds itself as a user task
  KeyPTU ptu(&robot);


  // turn off the sonar
  robot.comInt(ArCommands::SONAR, 0);

  printf("Press '?' for available commands\r\n");

  // run, if we lose connection to the robot, exit
  robot.waitForRunExit();

  Aria::exit(0);
}
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;
}
Пример #15
0
int main(int argc, char **argv)
{
  Aria::init();
  ArSimpleConnector con(&argc, argv);
  ArRobot robot;
  ArP2Arm arm;


  if(!Aria::parseArgs()) 
  {
    Aria::logOptions();
    Aria::exit(1);
    return 1;
  }

  ArLog::log(ArLog::Normal, "armExample: Connecting to the robot.");
  if(!con.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "armExample: Could not connect to the robot. Exiting.");
    Aria::exit(1);
    return 1;
  }
  robot.runAsync(true);

  // turn off sonar
  robot.comInt(28, 0);

  // Set up and initialize the arm
  arm.setRobot(&robot);
  if (arm.init() != ArP2Arm::SUCCESS)
  {
    ArLog::log(ArLog::Terse, "armExample: Error initializing the P2 Arm!");
    return 1;
  }

  // Print out some of the settings
  P2ArmJoint *joint;
  printf("Current joint info:\nJoint   Vel  Home  Center\n");
  for (int i=1; i<=ArP2Arm::NumJoints; i++)
  {
    joint = arm.getJoint(i);
    printf("  %2i:  %5i %5i   %5i\n", i, joint->myVel, joint->myHome, joint->myCenter);
  }
  printf("\n");

  // Put the arm to work
  printf("Powering on  (takes a couple seconds to stabilize)\n");
  arm.powerOn();

  // Request one status packet and print out the arm's status
  printf("Current arm status:\n");
  arm.requestStatus(ArP2Arm::StatusSingle);
  ArUtil::sleep(200);  // Give time to get the packet
  printf("Arm Status: ");
  if (arm.getStatus() & ArP2Arm::ArmGood)
    printf("Good=1 ");
  else
    printf("Good=0 ");
  if (arm.getStatus() & ArP2Arm::ArmInited)
    printf("Inited=1 ");
  else
    printf("Inited=0 ");
  if (arm.getStatus() & ArP2Arm::ArmPower)
    printf("Power=1 ");
  else
    printf("Power=0 ");
  if (arm.getStatus() & ArP2Arm::ArmHoming)
    printf("Homing=1 ");
  else
    printf("Homing=0 ");
  printf("\n\n");

  // Move the arm joints to specific positions
  printf("Moving Arm...\n");
  int deploy_offset[] = {0, -100, 10, 40, 80, -55, 20};
  for (int i=1; i<=ArP2Arm::NumJoints; i++)
  {
    joint = arm.getJoint(i);
    arm.moveToTicks(i, joint->myCenter + deploy_offset[i]);
  }

  // Wait for arm to achieve new position, printing joint positions and M for
  // moving, NM for not moving.
  arm.requestStatus(ArP2Arm::StatusContinuous);
  ArUtil::sleep(300); // wait a moment for arm status packet update with joints moving
  bool moving = true;
  while (moving)
  {
      moving = false;
      printf("Joints: ");
      for (int i=1; i<=ArP2Arm::NumJoints; i++)
      {
        printf("[%d] %.0f, ", i, arm.getJointPos(i));
        if (arm.getMoving(i))
        {
	        printf("M;  ");
	        moving = true;
	      }
        else
        {
          printf("NM; ");
        }
	    }
      printf("\r");
  }
  printf("\n\n");

  // Return arm to park, wait, and disconnect. (Though the arm will automatically park
  // on client disconnect)
  printf("Parking arm.\n");
  arm.park();

  // Wait 5 seconds or until arm shuts off
  for(int i = 5; (i > 0) && (arm.getStatus() & ArP2Arm::ArmPower); i--)
  {
    ArUtil::sleep(1000);
  }

  // Disconnect from robot, etc., and exit.
  printf("Shutting down ARIA and exiting.\n");
  Aria::exit(0);
  return(0);
}
int main(int argc, char **argv)
{
  Aria::init();

  // parse our args and make sure they were all accounted for
  ArSimpleConnector connector(&argc, argv);

  ArRobot robot;

  // the laser. ArActionTriangleDriveTo will use this laser object since it is
  // named "laser" when added to the ArRobot.
  ArSick sick;

  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }
  
  // a key handler so we can do our key handling
  ArKeyHandler keyHandler;
  // let the global aria stuff know about it
  Aria::setKeyHandler(&keyHandler);
  // toss it on the robot
  robot.attachKeyHandler(&keyHandler);

  // add the laser to the robot
  robot.addRangeDevice(&sick);

  ArSonarDevice sonar;
  robot.addRangeDevice(&sonar);
  
  ArActionTriangleDriveTo triangleDriveTo;
  ArFunctorC<ArActionTriangleDriveTo> lineGoCB(&triangleDriveTo, 
				      &ArActionTriangleDriveTo::activate);
  keyHandler.addKeyHandler('g', &lineGoCB);
  keyHandler.addKeyHandler('G', &lineGoCB);
  ArFunctorC<ArActionTriangleDriveTo> lineStopCB(&triangleDriveTo, 
					&ArActionTriangleDriveTo::deactivate);
  keyHandler.addKeyHandler('s', &lineStopCB);
  keyHandler.addKeyHandler('S', &lineStopCB);

  ArActionLimiterForwards limiter("limiter", 150, 0, 0, 1.3);
  robot.addAction(&limiter, 70);
  ArActionLimiterBackwards limiterBackwards;
  robot.addAction(&limiterBackwards, 69);

  robot.addAction(&triangleDriveTo, 60);

  ArActionKeydrive keydrive;
  robot.addAction(&keydrive, 55);


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

  robot.comInt(ArCommands::SONAR, 1);
  robot.comInt(ArCommands::ENABLE, 1);
  
  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  // now set up the laser
  connector.setupLaser(&sick);

  sick.runAsync();

  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    Aria::shutdown();
    return 1;
  }

  printf("If you press the 'g' key it'll go find a triangle, if you press 's' it'll stop.\n");

  robot.waitForRunExit();
  return 0;
}
Пример #17
0
int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  ArAnalogGyro gyro(&robot);
  ArSonarDevice sonar;
  ArRobotConnector robotConnector(&parser, &robot);
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);


  // Connect to the robot, get some initial data from it such as type and name,
  // and then load parameter files for this robot.
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "gotoActionExample: 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, "gotoActionExample: Connected to robot.");

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

  // Make a key handler, so that escape will shut down the program
  // cleanly
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit\n");

  // Collision avoidance actions at higher priority
  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);

  // Goto action at lower priority
  ArActionGoto gotoPoseAction("goto");
  robot.addAction(&gotoPoseAction, 50);
  
  // Stop action at lower priority, so the robot stops if it has no goal
  ArActionStop stopAction("stop");
  robot.addAction(&stopAction, 40);


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

  const int duration = 30000; //msec
  ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds, then cancelling goal and exiting.", duration/1000);

  bool first = true;
  int goalNum = 0;
  ArTime start;
  start.setToNow();
  while (Aria::getRunning()) 
  {
    robot.lock();

    // Choose a new goal if this is the first loop iteration, or if we 
    // achieved the previous goal.
    if (first || gotoPoseAction.haveAchievedGoal())
    {
      first = false;
      goalNum++;
      if (goalNum > 4)
        goalNum = 1; // start again at goal #1

      // set our positions for the different goals
      if (goalNum == 1)
        gotoPoseAction.setGoal(ArPose(2500, 0));
      else if (goalNum == 2)
        gotoPoseAction.setGoal(ArPose(2500, 2500));
      else if (goalNum == 3)
        gotoPoseAction.setGoal(ArPose(0, 2500));
      else if (goalNum == 4)
        gotoPoseAction.setGoal(ArPose(0, 0));

      ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f", 
		    gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
    }

    if(start.mSecSince() >= duration) {
      ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
      gotoPoseAction.cancelGoal();
      robot.unlock();
      ArUtil::sleep(3000);
      break;
    }

    robot.unlock();
    ArUtil::sleep(100);
  }
  
  // Robot disconnected or time elapsed, shut down
  Aria::exit(0);
  return 0;
}
Пример #18
0
void PeoplebotTest::userTask(void)
{
  switch (myState)
  {
    case IDLE:
      // start wandering
      printf("Starting to wander for the first time\n");
      myStateTime.setToNow();
      myState = WANDERING;
      myRobot->addAction(myConstantVelocity, 25);
      printf("Opening up ACTS\n");
      myCmd = "DISPLAY=";
      myCmd += myHostname.c_str();
      myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
      system(myCmd.c_str());
      break;
    case WANDERING:
      if (timeout(myWanderingTimeout))
      {
	myRobot->comInt(ArCommands::SONAR,0);
	myRobot->remAction(myConstantVelocity);
	myRobot->setVel(0);
	myRobot->setRotVel(0);
	myState = RESTING;
	mySonar = 0;
	printf("Going to rest now\n");
	printf("Killing ACTS\n");
	system("killall -9 acts &> /dev/null");
	myStateTime.setToNow();
	myTotalWanderTime += myWanderingTimeout;
      }
      else if (myRobot->getVel() > 0 && mySonar != 1)
      {
	// ping front sonar
	//printf("Enabling front sonar\n");
	mySonar = 1;
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::SONAR, 1);
	myRobot->comInt(ArCommands::SONAR, 4);
      }
      else if (myRobot->getVel() < 0 && mySonar != -1)
      {
	// ping rear sonar
	//printf("Enabling rear sonar\n");
	mySonar = -1;
	myRobot->comInt(ArCommands::SONAR, 0);
	myRobot->comInt(ArCommands::SONAR, 5);
      }
      break;
    case RESTING:
      if (timeout(myRestingTimeout))
      {
	printf("Going to wander now\n");
	myState = WANDERING;
	myStateTime.setToNow();
	myTotalRestTime += myRestingTimeout;
	myRobot->clearDirectMotion();
	myRobot->addAction(myConstantVelocity, 25);
	printf("Opening up ACTS\n");
	myCmd = "DISPLAY=";
	myCmd += myHostname.c_str();
	myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &";
	system(myCmd.c_str());
      }
      break;
    case OTHER:
    default:
      break;
  };
}
Пример #19
0
int main(int argc, char **argv)
{
  int ret;
  std::string str;
  // the serial connection (robot)
  ArSerialConnection serConn;
  // tcp connection (sim)
  ArTcpConnection tcpConn;
  // the robot
  ArRobot robot;
  // the laser
  ArSick sick;
  // the laser connection
  ArSerialConnection laserCon;

  bool useSimForLaser = false;


  std::string hostname = "prod.local.net";

  // timeouts in minutes
  int wanderTime = 0;
  int restTime = 0;


  // check arguments
  if (argc == 3 || argc == 4)
  {
    wanderTime = atoi(argv[1]);
    restTime = atoi(argv[2]);
    if (argc == 4)
      hostname = argv[3];
  }
  else
  {
    printf("\nUsage:\n\tpeoplebotTest <wanderTime> <restTime> <hostname>\n\n");
    printf("Times are in minutes.  Hostname is the machine to pipe the ACTS display to\n\n");
    wanderTime = 15;
    restTime = 45;
  }

  printf("Wander time - %d minutes\nRest time - %d minutes\n", wanderTime, restTime);
  printf("Sending display to %s.\n\n", hostname.c_str());

  // sonar, must be added to the robot
  ArSonarDevice sonar;

  // the actions we'll use to wander
  ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
  ArActionAvoidFront avoidFrontFar;

  // Make a key handler, so that escape will shut down the program
  // cleanly
  ArKeyHandler keyHandler;

  // mandatory init
  Aria::init();

  // Add the key handler to Aria so other things can find it
  Aria::setKeyHandler(&keyHandler);

  // Attach the key handler to a robot now, so that it actually gets
  // some processing time so it can work, this will also make escape
  // exit
  robot.attachKeyHandler(&keyHandler);


  // First we see if we can open the tcp connection, if we can we'll
  // assume we're connecting to the sim, and just go on...  if we
  // can't open the tcp it means the sim isn't there, so just try the
  // robot

  // modify this next line if you're not using default tcp connection
  tcpConn.setPort();

  // see if we can get to the simulator  (true is success)
  if (tcpConn.openSimple())
  {
    // we could get to the sim, so set the robots device connection to the sim
    printf("Connecting to simulator through tcp.\n");
    robot.setDeviceConnection(&tcpConn);
  }
  else
  {
    // we couldn't get to the sim, so set the port on the serial
    // connection and then set the serial connection as the robots
    // device

    // modify the next line if you're not using the first serial port
    // to talk to your robot
    serConn.setPort();
    printf(
      "Could not connect to simulator, connecting to robot through serial.\n");
    robot.setDeviceConnection(&serConn);
  }
  
  
  // add the sonar to the robot
  robot.addRangeDevice(&sonar);

  // add the laser
  robot.addRangeDevice(&sick);

  // try to connect, if we fail exit
  if (!robot.blockingConnect())
  {
    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);

  // turn off the sonar to start with
  robot.comInt(ArCommands::SONAR, 0);

  // add the actions
  robot.addAction(&recover, 100);
  robot.addAction(&bumpers, 75);
  robot.addAction(&avoidFrontNear, 50);
  robot.addAction(&avoidFrontFar, 49);
  
  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  if (!useSimForLaser)
  { 
    sick.setDeviceConnection(&laserCon);

    if ((ret = laserCon.open("/dev/ttyS2")) != 0)
    {
      str = tcpConn.getOpenMessage(ret);
      printf("Open failed: %s\n", str.c_str());
      Aria::shutdown();
      return 1;
    }
    sick.configureShort(false);
  }
  else 
  {
    sick.configureShort(true);
  }

  sick.runAsync();

  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.lock();
  robot.comInt(ArCommands::ENABLE, 1);
  robot.unlock();

  // add the peoplebot test
  PeoplebotTest pbTest(&robot, wanderTime, restTime, hostname);

  robot.waitForRunExit();

  // now exit
  Aria::shutdown();
  return 0;
}
Пример #20
0
int main(int argc, char **argv)
{

//----------------------initialized robot server------------------------------------------
  Aria::init();
  Arnl::init();
  
  ArServerBase server;
	
//-----------------------------------------------------------------------------------
	VCCHandler ptz(&robot); 		//create keyboard for control vcc50i

	
	G_PTZHandler->reset();
	ArUtil::sleep(300);
	G_PTZHandler->panSlew(30);
//-----------------------------------------------------------------------------------

  argc = 2 ;

  argv[0] = "-map";
  argv[1] = "map20121111.map";
  
  // Parse the command line arguments.
  ArArgumentParser parser(&argc, argv);

  // Set up our simpleConnector
  ArSimpleConnector simpleConnector(&parser);

  // Set up our simpleOpener
  ArServerSimpleOpener simpleOpener(&parser);
  

//*******
  // Set up our client for the central server
//   ArClientSwitchManager clientSwitch(&server, &parser);
//************
  
  // Load default arguments for this computer (from /etc/Aria.args, environment
  // variables, and other places)
//   parser.loadDefaultArguments();

  // set up a gyro
  ArAnalogGyro gyro(&robot);
  //gyro.activate();
  // Parse arguments for the simple connector.
//   if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
//   {
//     ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
//     Aria::logOptions();
//     Aria::exit(1);
//   }


  // The laser object, will be used if we have one


  // Add the laser to the robot
  robot.addRangeDevice(&sick);

  // Sonar, must be added to the robot, used by teleoperation and wander to
  // detect obstacles, and for localization if SONARNL
  ArSonarDevice sonarDev;

  // Add the sonar to the robot
  robot.addRangeDevice(&sonarDev);
  
  // Set up where we'll look for files
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), "./", "maps");
  ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
  
  // Set up the map, this will look for files in the examples
  // directory (unless the file name starts with a /, \, or .
  // You can take out the 'fileDir' argument to look in the current directory
  // instead
  
  ArMap arMap(fileDir);
  // set it up to ignore empty file names (otherwise the parseFile
  // on the config will fail)
  arMap.setIgnoreEmptyFileName(true);
  
//********************************
//Localization
//********************************
  

  ArLocalizationManager locManager(&robot, &arMap);
  ArLog::log(ArLog::Normal, "Creating laser localization task");
  ArLocalizationTask locTask (&robot, &sick, &arMap);
  locManager.addLocalizationTask(&locTask);

  
//*******************************
//Path planning
//*******************************
  
  // Make the path task planning task
  ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
  G_PathPlanning = &pathTask;
  // Set up things so data can be logged (only do it with the laser
  // since it can overrun a 9600 serial connection which the sonar is
  // more likely to have)
  ArDataLogger dataLogger(&robot);
  dataLogger.addToConfig(Aria::getConfig());

  
  
  
  // add our logging to the config
//   ArLog::addToConfig(Aria::getConfig());

  // First open the server 
  if (!simpleOpener.open(&server, fileDir, 240))
  {
    if (simpleOpener.wasUserFileBad())
      ArLog::log(ArLog::Normal, "Bad user file");
    else
      ArLog::log(ArLog::Normal, "Could not open server port");
    exit(2);
  }

  // Connect the robot
  if (!simpleConnector.connectRobot(&robot))
  {
    ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
    Aria::exit(3);
  }

  
//-----------------------------------------------
//**************************
  // Set up a class that'll put the movement and gyro parameters into ArConfig
  ArRobotConfig robotConfig(&robot);
  robotConfig.addAnalogGyro(&gyro);
//*****************************
  
  
  robot.enableMotors();
  robot.clearDirectMotion();

  // if we are connected to a simulator, reset it to its start position
  robot.comInt(ArCommands::RESETSIMTOORIGIN, 1);
  robot.moveTo(ArPose(0,0,0));


  // Set up laser using connector (command line arguments, etc.)
   simpleConnector.setupLaser(&sick);

  // Start the robot thread.
  robot.runAsync(true);
  
  // Start the laser thread.
  sick.runAsync();

  // Try to connect the laser
  if (!sick.blockingConnect())
    ArLog::log(ArLog::Normal, "Warning: Couldn't connect to SICK laser, it won't be used");
  else
    ArLog::log(ArLog::Normal, "Connected to laser.");

//***************************************
  // Add additional range devices to the robot and path planning task.
  // IRs if the robot has them.
  robot.lock();
//   ArIRs irs;
//   robot.addRangeDevice(&irs);
//   pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
//******************************************


  // Forbidden regions from the map
  ArForbiddenRangeDevice forbidden(&arMap);
  robot.addRangeDevice(&forbidden);

  // This is the place to add a range device which will hold sensor data
  // and delete it appropriately to replan around blocked paths.
  ArGlobalReplanningRangeDevice replanDev(&pathTask);

  // Create objects that add network services:
  
  // Drawing in the map display:
  ArServerInfoDrawings drawings(&server);
  drawings.addRobotsRangeDevices(&robot);
  drawings.addRangeDevice(&replanDev);

  /* If you want to draw the destination put this code back in:
  ArServerDrawingDestination destination(
	  &drawings, &pathTask, "destination",
	  500, 500,
	  new ArDrawingData("polyDots",
			    ArColor(0xff, 0xff, 0x0),
			    800, // size
			    49), // just below the robot
  */ 

  /* If you want to see the local path planning area use this 
    (You can enable this particular drawing from custom commands 
    which is set up down below in ArServerInfoPath) 
  ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
  ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> 
  drawingFunctorP(pathTask, &ArPathPlanningTask::drawSearchRectangle);
  drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP); 
  */

  /* If you want to see the points making up the local path in addition to the
   * main path use this. 
  ArDrawingData drawingDataP2("polyDots", ArColor(0,128,0), 100, 70);
  ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> 
  drawingFunctorP2(pathTask, &ArPathPlanningTask::drawPathPoints);
  drawings.addDrawing(&drawingDataP2, "Path Points", &drawingFunctorP2);
  */

  // Misc. simple commands:
  ArServerHandlerCommands commands(&server);
	

  // These provide various kinds of information to the client:
  ArServerInfoRobot serverInfoRobot(&server, &robot);
  ArServerInfoSensor serverInfoSensor(&server, &robot);
  ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
  serverInfoPath.addSearchRectangleDrawing(&drawings);
  serverInfoPath.addControlCommands(&commands);
//-------------------------receive commands or events from client---------------------
  
//   ArServerHandlerCommands commands(&server);
	server.addData("RobotVideo"			,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&VideoServerBase::RobotVideoCB) ,"","");
	server.addData("turn"						,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&turn_func) ,"","");
	server.addData("RobotMotion"	  ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotMotion) ,"","");
	server.addData("CameraMotion"   ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_CameraMotion) ,"","");
	server.addData("RobotTurnLeft"  ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnLeft) ,"","");
	server.addData("RobotTurnRight" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnRight) ,"","");
	server.addData("TargetApproach" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach) ,"","");
	server.addData("TargetApproachObstacles" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach_Obstacles) ,"","");
	server.addData("GlassesCancel" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_GlassesCancel) ,"","");
	server.addData("Calibration" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_Calibration) ,"","");

	server.addClientRemovedCallback(new ArGlobalFunctor1< ArServerClient * >(&clientCloseCallback));
//-------------------------receive commands or events from client---------------------


  
//***********************  
  ArServerInfoLocalization serverInfoLocalization (&server, &robot, &locManager);
  ArServerHandlerLocalization serverLocHandler (&server, &robot, &locManager);


  // Provide the map to the client (and related controls):
  // This uses both lines and points now, since everything except
  // sonar localization uses both (path planning with sonar still uses both)
  ArServerHandlerMap serverMap(&server, &arMap);

  

  // Add some simple (custom) commands for testing and debugging:
  ArServerSimpleComUC uCCommands(&commands, &robot);                   // Send any command to the microcontroller
  ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
  ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro);        // monitor the gyro
  ArServerSimpleComLogRobotConfig configCommands(&commands, &robot);   // trigger logging of the robot config parameters
  ArServerSimpleServerCommands serverCommands(&commands, &server);     // monitor networking behavior (track packets sent etc.)


  /* Set up the possible modes for remote control from a client such as
   * MobileEyes:
   */

  // Mode To go to a goal or other specific point:
  ArServerModeGoto modeGoto(&server, &robot, &pathTask, &arMap, ArPose(0,0,0));

  // Add a simple (custom) command that allows you to give a list of 
  // goals to tour, instead of all. Useful for testing and debugging.
  modeGoto.addTourGoalsInListSimpleCommand(&commands);

  // Mode To stop and remain stopped:
  ArServerModeStop modeStop(&server, &robot);

  // cause the sonar to turn off automatically
  // when the robot is stopped, and turn it back on when commands to move
  // are sent. (Note, this should not be done if you need the sonar
  // data to localize, or for other purposes while stopped)
  ArSonarAutoDisabler sonarAutoDisabler(&robot);

  // Teleoperation modes To drive by keyboard, joystick, etc:
  ArServerModeRatioDrive modeRatioDrive(&server, &robot);  // New, improved mode
  ArServerModeDrive modeDrive(&server, &robot);            // Older mode for compatability

  // Drive mode's configuration and custom (simple) commands:
  modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
  modeDrive.addControlCommands(&commands);
  modeRatioDrive.addControlCommands(&commands);

  // Wander mode 
//   ArServerModeWander modeWander(&server, &robot);
//*********************************
  // Prevent driving if localization is lost:
  ArActionLost actionLostRatioDrive (&locManager, NULL, &modeRatioDrive);
  modeRatioDrive.getActionGroup ()->addAction (&actionLostRatioDrive, 110);

  // Prevent wandering if lost:
//   ArActionLost
//   actionLostWander (&locManager, NULL, &modeWander);
//   modeWander.getActionGroup ()->addAction (&actionLostWander, 110);

  // This provides a small table of interesting information for the client
  // to display to the operator:
  ArServerInfoStrings stringInfo(&server);
  Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
  
  Aria::getInfoGroup()->addStringInt(
	  "Motor Packet Count", 10, 
	  new ArConstRetFunctorC<int, ArRobot>(&robot, 
					       &ArRobot::getMotorPacCount));


  // Make Stop mode the default (If current mode deactivates without entering
  // a new mode, then Stop Mode will be selected)
  modeStop.addAsDefaultMode();






  // Create the service that allows the client to monitor the communication 
  // between the robot and the client.
  //
  ArServerHandlerCommMonitor handlerCommMonitor(&server);

  // Create service that allows client to change configuration parameters in ArConfig 
  ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
				      Arnl::getTypicalDefaultParamFileName(),
				      Aria::getDirectory());



  // Read in parameter files.  read the paras from input
  Aria::getConfig()->useArgumentParser(&parser);
  if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
  {
    ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
    Aria::exit(5);
  }

  // Warn about unknown params.
  if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
  {
    ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
    simpleConnector.logOptions();
    simpleOpener.logOptions();
    Aria::exit(6);
  }

  // Warn if there is no map
  if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0)
  {
    ArLog::log(ArLog::Normal, "");
    ArLog::log(ArLog::Normal, "### Warning, No map file is set up, you can make a map with sickLogger or arnlServer, and Mapper3; More info in docs/Mapping.txt and README.txt. Set the map with the -map command line option, or by changing the config with MobileEyes or by editing the config file.");
    ArLog::log(ArLog::Normal, "");    
  }

  // find out where we'll want to put files
  ArLog::log(ArLog::Normal, "");
  ArLog::log(ArLog::Normal, 
	     "Directory for maps and file serving: %s", fileDir);
  
  ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
  ArLog::log(ArLog::Normal, "");

  // If you want MobileSim to try and load up the same map as you are
  // using in guiServer then uncomment out the next line and this object
  // will send a command to MobileSim to do so, but make sure you start 
  // MobileSim from the Arnl/examples directory or use the --cwd option, 
  // so that the map names used by MobileSim match  the map names used 
  // by guiServer
  //ArSimMapSwitcher mapSwitcher(&robot, &arMap);

/******************************************************
 * ****************************************************
 * 			Camera 
 * ****************************************************
 * *****************************************************/
	
  
  robot.unlock();
    // Localize robot at home.
  locTask.localizeRobotAtHomeBlocking();

  locTask.forceUpdatePose(ArPose(0,0,0));

  resetMotion();
  

	

// robot.enableMotors();
// robot.runAsync(true);
  //locTask.localizeRobotAtHomeBlocking();
  server.runAsync();

  VideoServerBase videoserver;
  videoserver.runAsync();

	robot.comInt(ArCommands::SOUNDTOG, 0);
	
	//G_PathPlanning->pathPlanToPose(ArPose(1500, -1500 , -32),true,true);
	//while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL );

	//S_TargetApproach_Obstacles1();

	//ArUtil::sleep(5000);


 //   G_PTZHandler->panRel(60);
	//	ArUtil::sleep(3000);
	//while(1)
	//S_TargetApproach1();
	cout << "done!!!!!!!---------------------" <<endl;
  robot.waitForRunExit();
  Aria::exit(0);
}
Пример #21
0
int main(int argc, char **argv)
{
  std::string str;
  int ret;
  int successes = 0, failures = 0;
  int action;
  bool exitOnFailure = true;
  
  ArSerialConnection con;
  ArRobot robot;
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);
  srand(time(NULL));
  robot.runAsync(false);
// if (!exitOnFailure)
//    ArLog::init(ArLog::None, ArLog::Terse);
  //else
  //ArLog::init(ArLog::None);
  while (1)
  {
    if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
	(ret = con.open()) != 0)
    {
      str = con.getOpenMessage(ret);
      printf("Open failed: %s\n", str.c_str());
      ++failures;
      if (exitOnFailure)
      {
	printf("Failed\n");
	exit(0);
      }
      else
      {
	ArUtil::sleep(200);
	robot.unlock();
	continue;
      }
    }
    robot.lock();
    robot.setDeviceConnection(&con);
    robot.unlock();
    ArUtil::sleep((rand() % 5) * 100);
    if (robot.asyncConnect())
    {
      robot.waitForConnectOrConnFail();
      robot.lock();
      if (!robot.isConnected())
      {
	if (exitOnFailure)
	{
	  printf("Failed after %d tries.\n", successes);
	  exit(0);
	}
	printf("Failed to connect successfully");
	++failures;
      }
      robot.comInt(ArCommands::SONAR, 0);
      robot.comInt(ArCommands::SOUNDTOG, 0);
      //robot.comInt(ArCommands::PLAYLIST, 0);
      robot.comInt(ArCommands::ENCODER, 1);
      ArUtil::sleep(((rand() % 20) + 3) * 100);
      ++successes;
      // okay, now try to leave it in a messed up state
      action = rand() % 8;
      robot.dropConnection();
      switch (action) {
      case 0:
	printf("Discon  0 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	break;
      case 1:
	printf("Discon  1 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	break;
      case 2:
	printf("Discon  2 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	ArUtil::sleep(100);
	robot.com(2);
	break;
      case 3:
	printf("Discon 10 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(10);
	break;
      case 4:
	printf("Discon    ");
	robot.disconnect();
	break;
      default:
	printf("Leave     ");
	break;
      }
      robot.unlock();
    }
    else
    {
      if (exitOnFailure)
      {
	printf("Failed after %d tries.\n", successes);
	exit(0);
      }
      printf("Failed to start connect ");
      ++failures;
    }
    if ((rand() % 2) == 0)
    {
      printf(" ! RadioDisconnect ! ");
      con.write("|||\15", strlen("!!!\15"));
      
      ArUtil::sleep(100);
      con.write("WMD\15", strlen("WMD\15"));
      ArUtil::sleep(200);
    }
    if ((rand() % 2) == 0)
    {
      printf(" ! ClosePort !\n");
      con.close();
    }
    else
      printf("\n");
    printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
	   (float)successes/(float)(successes+failures)*100);

    ArUtil::sleep((rand() % 2)* 1000);
  }
  return 0; 
}
Пример #22
0
void TakeBlockToWall::handler(void)
{
  Color tempColor;

  switch (myState) 
  {
  case STATE_START:
    setState(STATE_ACQUIRE_BLOCK);
    myDropWall = COLOR_FIRST_WALL;
    myLapWall = COLOR_SECOND_WALL;
    printf("!! Started state handling!\n");
    //handler();
    return;
    break;
  case STATE_ACQUIRE_BLOCK:
    if (myNewState)
    {
      printf("!! Acquire block\n");
      myNewState = false;
      mySony->panTilt(0, -10);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_BLOCK);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myGripper->getGripState() == 2 &&
	myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock: Successful (have cube?)\n");
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    } 
    else if (myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock: Successful (cube in gripper?)\n");
      setState(STATE_PICKUP_BLOCK);
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED || 
	myStateStart.mSecSince() > 35000)
    {
      printf("###### AcqiureBlock: failed\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireBlock: successful\n");
      setState(STATE_PICKUP_BLOCK);
      //handler();
      return;
    }
    break;
  case STATE_PICKUP_BLOCK:
    if (myNewState)
    {
      printf("!! Pickup block\n");
      myNewState = false;
      mySony->panTilt(0, -15);
      myAcquire->deactivate();
      myPickUp->activate();
      myPickUp->setChannel(COLOR_BLOCK);
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myPickUp->getState() == PickUp::STATE_FAILED)
    {
      printf("###### PickUpBlock: failed\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
    {
      printf("###### PickUpBlock: successful\n");
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_BACKUP:
    if (myNewState)
    {
      printf("!! Backup\n");
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->activate();
      myNewState = false;
    }
    if (myStateStart.mSecSince() > 2000)
    {
      printf("###### Backup: done\n");
      setState(STATE_ACQUIRE_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_BLOCK2:
    if (myNewState)
    {
      printf("!! Acquire block 2\n");
      myNewState = false;
      mySony->panTilt(0, -25);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_BLOCK);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myGripper->getGripState() == 2 &&
	myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock2: Successful (have cube?)\n");
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    }
    else if (myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock2: Successful (cube in gripper?)\n");
      setState(STATE_PICKUP_BLOCK2);
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStart.mSecSince() > 35000)
    {
      printf("###### AcqiureBlock2: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireBlock2: successful\n");
      setState(STATE_PICKUP_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_PICKUP_BLOCK2:
    if (myNewState)
    {
      printf("!! Pickup block 2\n");
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->activate();
      mySony->panTilt(0, -25);
      myPickUp->setChannel(COLOR_BLOCK);
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myPickUp->getState() == PickUp::STATE_FAILED)
    {
      printf("###### PickUpBlock2: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
    {
      printf("###### PickUpBlock2: successful\n");
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_DROP_WALL:
    if (myNewState)
    {
      printf("!! Acquire Drop wall, channel %d\n", myDropWall);
      myNewState = false;
      mySony->panTilt(0, -5);
      myAcquire->activate();
      myAcquire->setChannel(myDropWall);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myGripper->getGripState() != 2 || 
	myGripper->getBreakBeamState() == 0)
    {
      printf("###### AcquireDropWall:: failed (lost cube)\n");
      setState(STATE_BACKUP);	       
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStart.mSecSince() > 35000)
    {
      printf("###### AcquireDropWall:: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireDropWall: successful\n");
      setState(STATE_DRIVETO_DROP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_DRIVETO_DROP_WALL:
    if (myNewState)
    {
      printf("!! Driveto Drop wall, channel %d\n", myDropWall);
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->activate();
      myDriveTo->setChannel(myDropWall);
      myBackup->deactivate();
    }
    if (myGripper->getGripState() != 2 || 
	myGripper->getBreakBeamState() == 0)
    {
      printf("###### DriveToDropWall:: failed (lost cube)\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    if (myDriveTo->getState() == DriveTo::STATE_FAILED)
    {
      printf("###### DriveToDropWall: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
    {
      printf("###### DriveToDropWall: succesful\n");
      setState(STATE_DROP);
      //handler();
      return;
    }
    break;
  case STATE_DROP:
    if (myNewState)
    {
      printf("!! Drop\n");
      printf("@@@@@ Drop lowering lift\n");
      myGripper->liftDown(); 
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
      myGripOpenSent = false;
    }
    
    myGripper->liftDown();
    if ((myStateStart.mSecSince() > 500 && 
	 myGripper->isLiftMaxed() && myStateStart.mSecSince() < 5000) ||
	myStateStart.mSecSince() > 5000)
    {
      myGripper->gripOpen();
      /*if (!myGripOpenSent)
      {
	ArUtil::sleep(3);
	myGripper->gripOpen();
      }
      myGripOpenSent = true;
      */
    }
    if (myGripper->getGripState() == 1 || myStateStart.mSecSince() > 6000)
    {
      printf("###### Drop: success\n");
      setState(STATE_DROP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_DROP_BACKUP:
    if (myNewState)
    {
      printf("!! Drop backup\n");
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->activate();
      myNewState = false;
    }
    if (myStateStart.mSecSince() > 2000)
    {
      printf("###### Drop_Backup: done\n");
      setState(STATE_ACQUIRE_LAP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_LAP_WALL:
    if (myNewState)
    {
      printf("!! Acquire Lap wall, channel %d\n", myLapWall);
      myNewState = false;
      mySony->panTilt(0, -5);
      myAcquire->activate();
      myAcquire->setChannel(myLapWall);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myBackup->deactivate();
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStart.mSecSince() > 35000)
    {
      printf("###### AcquireLapWall:: failed\n");
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireLapWall: successful\n");
      setState(STATE_DRIVETO_LAP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_DRIVETO_LAP_WALL:
    if (myNewState)
    {
      printf("!! Driveto Lap wall, channel %d\n", myLapWall);
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->activate();
      myDriveTo->setChannel(myLapWall);
      myBackup->deactivate();
    }
    if (myDriveTo->getState() == DriveTo::STATE_FAILED)
    {
      printf("###### DriveToLapWall: failed\n");
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
    {
      printf("###### DriveToLapWall: succesful\n");
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    break;
  case STATE_SWITCH:
    printf("!! Switching walls around.\n");
    tempColor = myDropWall;
    myDropWall = myLapWall;
    myLapWall = tempColor;
    setState(STATE_ACQUIRE_BLOCK);
    //handler();
    return;
  case STATE_FAILED:
    printf("@@@@@ Failed to complete the task!\n");
    myRobot->comInt(ArCommands::SONAR, 0);
    ArUtil::sleep(50);
    myRobot->comStr(ArCommands::SAY, "\52\77\37\62\42\70");
    ArUtil::sleep(500);
    Aria::shutdown();
    myRobot->disconnect();
    myRobot->stopRunning();
    return;
  }

}
Пример #23
0
int main (int argc, char** argv) {
	Aria::init();
	ArRobot robot;
	ArSonarDevice sonar;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}

	ArSonarDevice sonarDev;
	ArPose* poseList = readPostitions("positions.txt");
	robot.runAsync(true);
	robot.enableMotors();
	robot.moveTo(ArPose(0,0,0));
	robot.comInt(ArCommands::ENABLE, 1);
	robot.addRangeDevice(&sonarDev);
	ArActionGoto gotoPoseAction("goto", ArPose(0, 0, 0), 200);
	ArActionAvoidFront avoidFront("avoid front");
	ArActionStallRecover stallRecover("stall recover");
	robot.addAction(&gotoPoseAction, 50);
	robot.addAction(&avoidFront, 60);
	robot.moveTo(ArPose(0,0,0));
	int length = ARRAY_SIZE(poseList);
	cout<<"do dai"<<length;
	ArServerBase server;
	ArServerSimpleOpener simpleOpener(&parser);
	char fileDir[1024];
	  ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(),
				 "ArNetworking/examples");

	  // first open the server up
	  if (!simpleOpener.open(&server, fileDir, 240))
	  {
	    if (simpleOpener.wasUserFileBad())
	      printf("Bad user/password/permissions file\n");
	    else
	      printf("Could not open server port\n");
	    exit(1);
	  }
	ArServerInfoRobot serverInfo(&server, &robot);
	GotoGoal gotoGoal(&robot, &sonar, &server, &serverInfo);
	gotoGoal.init(argc, argv);
	float angle = 0;
	VideoCapture cap;
	cap.open(0);
	Rect trackWindow;
	//var check find ball
	bool checkObject = false;
	int hsize = 16;

	namedWindow( "threshold", 0 );
	namedWindow( "trackbar", 0 );
	namedWindow( "Histogram", 0 );
	namedWindow( "main", 0 );
	createTrackbar( "Vmin", "trackbar", &vmin, 256, 0 );
	createTrackbar( "Vmax", "trackbar", &vmax, 256, 0 );
	createTrackbar( "Smin", "trackbar", &smin, 256, 0 );

	CascadeClassifier c;
	c.load("cascade.xml");
	Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj;
	float vel = 0;
	int i = 0;
	while(1)
	{
		cap >> frame;
		if( frame.empty() ){
			cout<<"error camera"<<endl;
			break;
		}
		frame.copyTo(image);
		cvtColor(image, hsv, COLOR_BGR2HSV);
		int _vmin = vmin, _vmax = vmax;
		inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)),	Scalar(180, 256, MAX(_vmin, _vmax)), mask);
		gotoPoseAction.setGoal(poseList[i]);
		while (!gotoPoseAction.haveAchievedGoal()) 
		{
			ArLog::log(ArLog::Normal, "goal(%.2f, %0.2f) x = %.2f, y = %.2f", poseList[i].getX(), poseList[i].getY(), robot.getX(), robot.getY());
//			if (!checkObject)
			   checkObject = detect(frame, c);
			if (checkObject)
				cout <<"Phat hien doi tuong"<<endl;
			else
				cout <<"Khong phat hien doi tuong"<<endl;
			if (checkObject) {
				if(trackObject(hsv, mask)) {
					float d = distance();
					if (d < 250) {
						gotoGoal.move(-200);
					} else if ( d >= 250 && d <= 300) {
						gotoGoal.stop();
					}
					else {
						vel = d * 0.7;
						vel = (int) (vel/50) * 50;
						if(vel > 200) {
							vel = 200;
							gotoGoal.setVel(vel);
						}
						angle =  determindRotate();
						cout <<"khoang cach: "<<d<<"\tGoc quay: "<<angle<<"\t van toc = "<<vel<<endl;
						if (angle != 0) {
							gotoGoal.stop();
							gotoGoal.rotate(angle);
						}
					}
				}
			}
			imshow("main", image);
			imshow( "threshold", mask );
			imshow( "Histogram", histimg );
		}
		i++;
	}

	ArUtil::sleep(2000);
	Aria::shutdown();

}
Пример #24
0
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;
}
Пример #25
0
int main(void)
{
  ArSerialConnection con;
  ArRobot robot;
  int ret;
  std::string str;
  ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
  ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
  ArActionLimiterBackwards backwardsLimiter;
  ArActionConstantVelocity stop("stop", 0);
  ArActionConstantVelocity backup("backup", -200);
  ArSonarDevice sonar;
  ArACTS_1_2 acts;
  ArSonyPTZ sony(&robot);
  ArGripper gripper(&robot, ArGripper::GENIO);
  
  Acquire acq(&acts, &gripper);
  DriveTo driveTo(&acts, &gripper, &sony);
  PickUp pickUp(&acts, &gripper, &sony);

  TakeBlockToWall takeBlock(&robot, &gripper, &sony, &acq, &driveTo, &pickUp,
			    &backup);

  Aria::init();

   if (!acts.openPort(&robot))
   {
     printf("Could not connect to acts\n");
     exit(1);
   }
  
  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;
  }

  sony.init();
  ArUtil::sleep(1000);
  //robot.setAbsoluteMaxTransVel(400);

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

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

  robot.run(true);
  
  Aria::shutdown();
  return 0;
}
Пример #26
0
int main(int argc, char** argv)
{
  // Initialize some global data
  Aria::init();

  // If you want ArLog to print "Verbose" level messages uncomment this:
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);

  // This object parses program options from the command line
  ArArgumentParser parser(&argc, argv);

  // Load some default values for command line arguments from /etc/Aria.args
  // (Linux) or the ARIAARGS environment variable.
  parser.loadDefaultArguments();

  // Central object that is an interface to the robot and its integrated
  // devices, and which manages control of the robot by the rest of the program.
  ArRobot robot;

  // Object that connects to the robot or simulator using program options
  ArRobotConnector robotConnector(&parser, &robot);

  // If the robot has an Analog Gyro, this object will activate it, and 
  // if the robot does not automatically use the gyro to correct heading,
  // this object reads data from it and corrects the pose in ArRobot
  ArAnalogGyro gyro(&robot);

  // Connect to the robot, get some initial data from it such as type and name,
  // and then load parameter files for this robot.
  if (!robotConnector.connectRobot())
  {
    // Error connecting:
    // if the user gave the -help argumentp, then just print out what happened,
    // and continue so options can be displayed later.
    if (!parser.checkHelpAndWarnUnparsed())
    {
      ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything");
    }
    // otherwise abort
    else
    {
      ArLog::log(ArLog::Terse, "Error, could not connect to robot.");
      Aria::logOptions();
      Aria::exit(1);
    }
  }

  // Connector for laser rangefinders
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  // Connector for compasses
  ArCompassConnector compassConnector(&parser);

  // Parse the command line options. Fail and print the help message if the parsing fails
  // or if the help was requested with the -help option
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    exit(1);
  }

  // Used to access and process sonar range data
  ArSonarDevice sonarDev;
  
  // Used to perform actions when keyboard keys are pressed
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);

  // ArRobot contains an exit action for the Escape key. It also 
  // stores a pointer to the keyhandler so that other parts of the program can
  // use the same keyhandler.
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit\n");

  // Attach sonarDev to the robot so it gets data from it.
  robot.addRangeDevice(&sonarDev);

  
  // Start the robot task loop running in a new background thread. The 'true' argument means if it loses
  // connection the task loop stops and the thread exits.
  robot.runAsync(true);

  // Connect to the laser(s) if lasers were configured in this robot's parameter
  // file or on the command line, and run laser processing thread if applicable
  // for that laser class.  (For the purposes of this demo, add all
  // possible lasers to ArRobot's list rather than just the ones that were
  // specified with the connectLaser option (so when you enter laser mode, you
  // can then interactively choose which laser to use from the list which will
  // show both connected and unconnected lasers.)
  if (!laserConnector.connectLasers(false, false, true))
  {
    printf("Could not connect to lasers... exiting\n");
    Aria::exit(2);
  }

  // Create and connect to the compass if the robot has one.
  ArTCM2 *compass = compassConnector.create(&robot);
  if(compass && !compass->blockingConnect()) {
    compass = NULL;
  }
  
  // Sleep for a second so some messages from the initial responses
  // from robots and cameras and such can catch up
  ArUtil::sleep(1000);

  // We need to lock the robot since we'll be setting up these modes
  // while the robot task loop thread is already running, and they 
  // need to access some shared data in ArRobot.
  robot.lock();

  // now add all the modes for this demo
  // these classes are defined in ArModes.cpp in ARIA's source code.
  ArModeLaser laser(&robot, "laser", 'l', 'L');
  ArModeTeleop teleop(&robot, "teleop", 't', 'T');
  ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U');
  ArModeWander wander(&robot, "wander", 'w', 'W');
  ArModeGripper gripper(&robot, "gripper", 'g', 'G');
  ArModeCamera camera(&robot, "camera", 'c', 'C');
  ArModeSonar sonar(&robot, "sonar", 's', 'S');
  ArModeBumps bumps(&robot, "bumps", 'b', 'B');
  ArModePosition position(&robot, "position", 'p', 'P', &gyro);
  ArModeIO io(&robot, "io", 'i', 'I');
  ArModeActs actsMode(&robot, "acts", 'a', 'A');
  ArModeCommand command(&robot, "command", 'd', 'D');
  ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass);


  // activate the default mode
  teleop.activate();

  // turn on the motors
  robot.comInt(ArCommands::ENABLE, 1);

  robot.unlock();
  
  // Block execution of the main thread here and wait for the robot's task loop
  // thread to exit (e.g. by robot disconnecting, escape key pressed, or OS
  // signal)
  robot.waitForRunExit();

  Aria::exit(0);


}
int main(int argc, char **argv)
{
  bool done;
  double distToTravel = 2300;

  // whether to use the sim for the laser or not, if you use the sim
  // for hte laser, you have to use the sim for the robot too
  bool useSim = false;
  // the laser
  ArSick sick;
  // connection
  ArDeviceConnection *con;
  // Laser connection
  ArSerialConnection laserCon;
  // robot
  ArRobot robot;

  // set a default filename
  //std::string filename = "c:\\log\\1scans.2d";
  std::string filename = "1scans.2d";
  // see if we want to use a different filename
  //if (argc > 1)
  //Lfilename = argv[1];
  printf("Logging to file %s\n", filename.c_str());
  // start the logger with good values
  sick.configureShort(useSim, ArSick::BAUD38400,
		 ArSick::DEGREES180, ArSick::INCREMENT_HALF);
  ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
  
  // mandatory init
  Aria::init();

  // add it to the robot
  robot.addRangeDevice(&sick);

  //ArAnalogGyro gyro(&robot);


  // if we're not using the sim, make a serial connection and set it up
  if (!useSim)
  {
    ArSerialConnection *serCon;
    serCon = new ArSerialConnection;
    serCon->setPort();
    //serCon->setBaud(38400);
    con = serCon;
  }
  // if we are using the sim, set up a tcp connection
  else
  {
    ArTcpConnection *tcpCon;
    tcpCon = new ArTcpConnection;
    tcpCon->setPort();
    con = tcpCon;
  }

  // set the connection on the robot
  robot.setDeviceConnection(con);
  // try to connect, if we fail exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }


  // set up a key handler so escape exits and attach to the robot
  ArKeyHandler keyHandler;
  robot.attachKeyHandler(&keyHandler);

  // run the robot, true here so that the run will exit if connection lost
  robot.runAsync(true);



  // if we're not using the sim, set up the port for the laser
  if (!useSim)
  {
    laserCon.setPort(ArUtil::COM3);
    sick.setDeviceConnection(&laserCon);
  }


  // now that we're connected to the robot, connect to the laser
  sick.runAsync();


  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    robot.disconnect();
    Aria::shutdown();
    return 1;
  }

#ifdef WIN32
  // wait until someone pushes the motor button to go
  while (1)
  {
    robot.lock();
    if (!robot.isRunning())
      exit(0);
    if (robot.areMotorsEnabled())
    {
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
#endif

  // basically from here on down the robot just cruises around a bit

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

  ArTime startTime;
  // move a couple meters
  robot.move(distToTravel);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(0);
    done = robot.isMoveDone(60);
    robot.unlock();
  } while (!done);

  /*
  // rotate a few times
  robot.lock();
  robot.setVel(0);
  robot.setRotVel(60);
  robot.unlock();
  ArUtil::sleep(12000);
  */

  robot.lock();
  robot.setHeading(180);
  robot.unlock();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(180);
    done = robot.isHeadingDone();
    robot.unlock();
  } while (!done);

  // move a couple meters
  robot.lock();
  robot.move(distToTravel);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(180);
    done = robot.isMoveDone(60);
    robot.unlock();
  } while (!done);

  robot.lock();
  robot.setHeading(0);
  robot.setVel(0);
  robot.unlock();
  startTime.setToNow();
  do {
    ArUtil::sleep(100);
    robot.lock();
    robot.setHeading(0);
    done = robot.isHeadingDone();
    robot.unlock();
  } while (!done);


  sick.lockDevice();
  sick.disconnect();
  sick.unlockDevice();
  robot.lock();
  robot.disconnect();
  robot.unlock();
  // now exit
  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
		// (and also the default mode for reading data).
		lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking);
		if (lLastError != ERROR_SUCCESS)
			return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout."));
		// ============================ FIN CONFIG COM =================================//

	  bool first = true;
	  int goalNum = 0;
	  int color = 3;
	  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
		  printf("El contador esta en: --> %d <---\n",goalNum);
		  if (goalNum > 20)
			goalNum = 1;

		  //comienza la secuencia de puntos
		  if (goalNum == 1)
		  {
			gotoPoseAction.setGoal(ArPose(1150, 0));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			// 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();
			// Create the sound queue.
			ArSoundsQueue soundQueue;
			// Run the sound queue in a new thread
			soundQueue.runAsync();
			std::vector<const char*> filenames;
			filenames.push_back("sound-r2a.wav");
			soundQueue.play(filenames[0]);
		  }
		  else if (goalNum == 2)
		  {
			  printf("Gira 90 grados izquierda\n");
			  robot.unlock();
			  turn.myActivate = 1;
			  turn.myDirection = 1;
			  turn.activate();
			  ArUtil::sleep(1000);
			  turn.deactivate();
			  turn.myActivate = 0;
			  turn.myDirection = 0;
			  robot.lock();
		  }
		  else if (goalNum == 3)
		  {
			gotoPoseAction.setGoal(ArPose(1150, 2670));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			// 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();
		  }
		  else if (goalNum == 4)
		  {
			  printf("Gira 90 grados izquierda\n");
			  robot.unlock();
			  turn2.myActivate = 1;
			  turn2.myDirection = 1;
			  turn2.activate();
			  ArUtil::sleep(1000);
			  turn2.deactivate();
			  turn2.myActivate = 0;
			  turn2.myDirection = 0;
			  robot.lock();
		  }
		  else if (goalNum == 5)
		  {
			gotoPoseAction.setGoal(ArPose(650, 2670));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 6)
		  {
			  printf("Gira 90 grados izquierda\n");
			  robot.unlock();
			  turn2.myActivate = 1;
			  turn2.myDirection = 1;
			  turn2.activate();
			  ArUtil::sleep(1000);
			  turn2.deactivate();
			  turn2.myActivate = 0;
			  turn2.myDirection = 0;
			  robot.lock();
		  }
		  else if (goalNum == 7)
		  {
			gotoPoseAction.setGoal(ArPose(650, 0));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 8)
		  {
			gotoPoseAction.setGoal(ArPose(1800,1199));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 9)
		  {
			gotoPoseAction.setGoal(ArPose(2600, 1199));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 10)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 850));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 2)
			  {
				gotoPoseAction.setGoal(ArPose(3500, 1199));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); 
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 1550));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 11)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 613));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 2)
			  {
				  printf("Gira 180 grados derecha\n");
				  robot.unlock();
				  turn2.myActivate = 1;
				  turn2.myDirection = 2;
				  turn2.activate();
				  ArUtil::sleep(2000);
				  turn2.deactivate();
				  turn2.myActivate = 0;
				  turn2.myDirection = 0;
				  robot.lock();
				  goalNum = 19;
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 1785));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 12)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(3300, 413));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(3300, 1985));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 13)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(3500, 413));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(3500, 1985));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 14)
		  {
			  robot.unlock();
			  //Valor para el while
			  bool fContinue = true;
				// <<<<<<------------- 1 Parte Secuencia: BAJA BRAZO ------------->>>>>> //
				lLastError = serial.Write("b");
				if (lLastError != ERROR_SUCCESS)
					return ::ShowError(serial.GetLastError(), _T("Unable to send data"));

				//-------------------------E S C U C H A   C O M ----------------------------//
				do
				{
					// Wait for an event
					lLastError = serial.WaitEvent();
					if (lLastError != ERROR_SUCCESS)
						return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event."));

					// Save event
					const CSerial::EEvent eEvent = serial.GetEventType();

					// Handle break event
					if (eEvent & CSerial::EEventBreak)
					{
						printf("\n### BREAK received ###\n");
					}

					// Handle CTS event
					if (eEvent & CSerial::EEventCTS)
					{
						printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off");
					}

					// Handle DSR event
					if (eEvent & CSerial::EEventDSR)
					{
						printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off");
					}

					// Handle error event
					if (eEvent & CSerial::EEventError)
					{
						printf("\n### ERROR: ");
						switch (serial.GetError())
						{
						case CSerial::EErrorBreak:		printf("Break condition");			break;
						case CSerial::EErrorFrame:		printf("Framing error");			break;
						case CSerial::EErrorIOE:		printf("IO device error");			break;
						case CSerial::EErrorMode:		printf("Unsupported mode");			break;
						case CSerial::EErrorOverrun:	printf("Buffer overrun");			break;
						case CSerial::EErrorRxOver:		printf("Input buffer overflow");	break;
						case CSerial::EErrorParity:		printf("Input parity error");		break;
						case CSerial::EErrorTxFull:		printf("Output buffer full");		break;
						default:						printf("Unknown");					break;
						}
						printf(" ###\n");
					}

					// Handle ring event
					if (eEvent & CSerial::EEventRing)
					{
						printf("\n### RING ###\n");
					}

					// Handle RLSD/CD event
					if (eEvent & CSerial::EEventRLSD)
					{
						printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off");
					}

					// Handle data receive event
					if (eEvent & CSerial::EEventRecv)
					{
						// Read data, until there is nothing left
						DWORD dwBytesRead = 0;
						char szBuffer[101];
						do
						{
							// Lee datos del Puerto COM
							lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead);
							if (lLastError != ERROR_SUCCESS)
								return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port."));

							if (dwBytesRead > 0)
							{
								//Preseteo color
								int color = 0;
								// Finaliza el dato, asi que sea una string valida
								szBuffer[dwBytesRead] = '\0';
								// Display the data
								printf("%s", szBuffer);

								// <<<<<<----------- 2 Parte Secuencia: CIERRA GRIPPER ----------->>>>>> //
								if (strchr(szBuffer,76))
								{
									lLastError = serial.Write("c");
									if (lLastError != ERROR_SUCCESS)
										return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
								}
								
								// <<<<<<------------- 3 Parte Secuencia: SUBE BRAZO ------------->>>>>> //
								if (strchr(szBuffer,117))
								{
									lLastError = serial.Write("s");
									if (lLastError != ERROR_SUCCESS)
										return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
								}

								// <<<<<<------------- 4 Parte Secuencia: COLOR ------------->>>>>> //
								if (strchr(szBuffer,72))
								{
									lLastError = serial.Write("C");
									if (lLastError != ERROR_SUCCESS)
										return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
								}

								// <<<<<<---------- 5.1 Parte Secuencia: COLOR ROJO---------->>>>>> //
								if (strchr(szBuffer,82))
								{
									color = 1;
									//salir del bucle
									fContinue = false;
								}

								// <<<<<<---------- 5.2 Parte Secuencia: COLOR AZUL ---------->>>>>> //
								if (strchr(szBuffer,66))
								{
									color = 2;
									//salir del bucle
									fContinue = false;
								}

								// <<<<<<---------- 5.3 Parte Secuencia: COLOR VERDE ---------->>>>>> //
								if (strchr(szBuffer,71))
								{
									color = 3;
									//salir del bucle
									fContinue = false;
								}
							}
						}
						while (dwBytesRead == sizeof(szBuffer)-1);
					}
				}
				while (fContinue);
				// Close the port again
				serial.Close();
				robot.lock();
		  }
		  else if (goalNum == 15)
		  {
			  printf("Gira 180 grados derecha\n");
			  robot.unlock();
			  turn2.myActivate = 1;
			  turn2.myDirection = 2;
			  turn2.activate();
			  ArUtil::sleep(2000);
			  turn2.deactivate();
			  turn2.myActivate = 0;
			  turn2.myDirection = 0;
			  robot.lock();
		  }
		  else if (goalNum == 16)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(3300, 413));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(3300, 1985));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 17)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 603));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 1795));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 18)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 860));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 1540));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 19)
		  {
			gotoPoseAction.setGoal(ArPose(2600, 1199));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 20)
		  {
			gotoPoseAction.setGoal(ArPose(1800, 1199));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		}

		if(start.mSecSince() >= duration) {
		  ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000);
		  gotoPoseAction.cancelGoal();
		  robot.unlock();
		  ArUtil::sleep(3000);
		  break;
		}
	    
		robot.unlock();
		ArUtil::sleep(10);
	  }

	  // Robot desconectado al terminal el sleep
	  Aria::shutdown();

	//------------ F I N   M A I N    D E L   P R O G R A M A   D E L    R O B O T-----------//
    
    return 0;
}
Пример #29
0
int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser argParser(&argc, argv);
  argParser.loadDefaultArguments();
  ArRobot robot;
  ArRobotConnector robotConnector(&argParser, &robot);
  ArLaserConnector laserConnector(&argParser, &robot, &robotConnector);

  // Always try to connect to the first laser:
  argParser.addDefaultArgument("-connectLaser");

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


  // Trigger argument parsing
  if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(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.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.");
  }


  // turn on the motors, turn off amigobot sounds
  robot.enableMotors();
  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", 225, 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);
  
  // wait for robot task loop to end before exiting the program
  robot.waitForRunExit();
  
  Aria::exit(0);
}
Пример #30
0
void TakeBlockToWall::handler(void)
{
  Color tempColor;

  switch (myState) 
  {
  case STATE_START:
    setState(STATE_ACQUIRE_BLOCK);
    myDropWall = COLOR_FIRST_WALL;
    myLapWall = COLOR_SECOND_WALL;
    printf("!! Started state handling!\n");
    //handler();
    return;
    break;
  case STATE_ACQUIRE_BLOCK:
    if (myNewState)
    {
      printf("!! Acquire block\n");
      myNewState = false;
      myAMPTU->panTilt(0, -40);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_BLOCK);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myGripper->getGripState() == 2 &&
	myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock: Successful (have cube?)\n");
      setState(STATE_PICKUP_BACKUP);
      //handler();
      return;
    } 
    else if (myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock: Successful (cube in gripper?)\n");
      setState(STATE_PICKUP_BLOCK);
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED || 
	myStateStartTime.mSecSince() > 35000)
    {
      printf("###### AcqiureBlock: failed\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireBlock: successful\n");
      setState(STATE_PICKUP_BLOCK);
      //handler();
      return;
    }
    break;
  case STATE_PICKUP_BLOCK:
    if (myNewState)
    {
      printf("!! Pickup block\n");
      myNewState = false;
      myAMPTU->panTilt(0, -35);
      myAcquire->deactivate();
      myPickUp->activate();
      myPickUp->setChannel(COLOR_BLOCK);
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myPickUp->getState() == PickUp::STATE_FAILED)
    {
      printf("###### PickUpBlock: failed\n");
      setState(STATE_BACKUP);
      //handler();
      return;
    }
    else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
    {
      printf("###### PickUpBlock: successful\n");
      setState(STATE_PICKUP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_BACKUP:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(BACKUP_DIST * .75);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
    {
      printf("###### Backup: Failed, going forwards\n");
      myRobot->clearDirectMotion();
      setState(STATE_FORWARD);      
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > BACKUP_DIST * .95 * .75)
    {
      printf("###### Backup: Succeeded\n");
      myRobot->clearDirectMotion();
      setState(STATE_ACQUIRE_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_FORWARD:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(-BACKUP_DIST * .75);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
    {
      printf("###### Forward: Failed\n");
      myRobot->clearDirectMotion();
      setState(STATE_FAILED);      
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95 * .75))
    {
      printf("###### Forward: Succeeded\n");
      myRobot->clearDirectMotion();
      setState(STATE_ACQUIRE_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_BLOCK2:
    if (myNewState)
    {
      printf("!! Acquire block 2\n");
      myNewState = false;
      myAMPTU->panTilt(0, -40);
      myAcquire->activate();
      myAcquire->setChannel(COLOR_BLOCK);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myGripper->getGripState() == 2 &&
	myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock2: Successful (have cube?)\n");
      setState(STATE_PICKUP_BACKUP);
      //handler();
      return;
    }
    else if (myGripper->getBreakBeamState() != 0)
    {
      printf("###### AcquireBlock2: Successful (cube in gripper?)\n");
      setState(STATE_PICKUP_BLOCK2);
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStartTime.mSecSince() > 35000)
    {
      printf("###### AcqiureBlock2: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireBlock2: successful\n");
      setState(STATE_PICKUP_BLOCK2);
      //handler();
      return;
    }
    break;
  case STATE_PICKUP_BLOCK2:
    if (myNewState)
    {
      printf("!! Pickup block 2\n");
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->activate();
      myAMPTU->panTilt(0, -55);
      myPickUp->setChannel(COLOR_BLOCK);
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myPickUp->getState() == PickUp::STATE_FAILED)
    {
      printf("###### PickUpBlock2: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
    {
      printf("###### PickUpBlock2: successful\n");
      setState(STATE_PICKUP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_PICKUP_BACKUP:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(BACKUP_DIST);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95))
    {
      printf("###### PickUp_BackUp: done\n");
      myRobot->clearDirectMotion();
      setState(STATE_ACQUIRE_DROP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_DROP_WALL:
    if (myNewState)
    {
      printf("!! Acquire Drop wall, channel %d\n", myDropWall);
      myNewState = false;
      myAMPTU->panTilt(0, -30);
      myAcquire->activate();
      myAcquire->setChannel(myDropWall);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myGripper->getGripState() != 2 || 
	myGripper->getBreakBeamState() == 0)
    {
      printf("###### AcquireDropWall:: failed (lost cube %d %d)\n",
	     myGripper->getGripState(), myGripper->getBreakBeamState());
      setState(STATE_BACKUP);	       
      //handler();
      return;
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStartTime.mSecSince() > 35000)
    {
      printf("###### AcquireDropWall:: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireDropWall: successful\n");
      setState(STATE_DRIVETO_DROP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_DRIVETO_DROP_WALL:
    if (myNewState)
    {
      printf("!! DropOff Drop wall, channel %d\n", myDropWall);
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->activate();
      myDropOff->setChannel(myDropWall);
      myTableLimiter->deactivate();
    }
    if (myDropOff->getState() == DropOff::STATE_FAILED)
    {
      printf("###### DropOffDropWall: failed\n");
      setState(STATE_FAILED);
      //handler();
      return;
    }
    else if (myDropOff->getState() == DropOff::STATE_SUCCEEDED)
    {
      printf("###### DropOffDropWall: succesful\n");
      setState(STATE_DROP_BACKUP);
      //handler();
      return;
    }
    break;
  case STATE_DROP_BACKUP:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(BACKUP_DIST);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95))
    {
      printf("###### Drop_Backup: done\n");
      myRobot->clearDirectMotion();
      setState(STATE_ACQUIRE_LAP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_ACQUIRE_LAP_WALL:
    if (myNewState)
    {
      printf("!! Acquire Lap wall, channel %d\n", myLapWall);
      myNewState = false;
      myAMPTU->panTilt(0, -30);
      myAcquire->activate();
      myAcquire->setChannel(myLapWall);
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->activate();
    }
    if (myAcquire->getState() == Acquire::STATE_FAILED ||
	myStateStartTime.mSecSince() > 35000)
    {
      printf("###### AcquireLapWall:: failed\n");
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
    {
      printf("###### AcquireLapWall: successful\n");
      setState(STATE_DRIVETO_LAP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_DRIVETO_LAP_WALL:
    if (myNewState)
    {
      printf("!! Driveto Lap wall, channel %d\n", myLapWall);
      myNewState = false;
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->activate();
      myDriveTo->setChannel(myLapWall);
      myDropOff->deactivate();
      myTableLimiter->activate();
    }
    if (myDriveTo->getState() == DriveTo::STATE_FAILED)
    {
      printf("###### DriveToLapWall: failed\n");
      setState(STATE_BACKUP_LAP_WALL);
      //handler();
      return;
    }
    else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
    {
      printf("###### DriveToLapWall: succesful\n");
      setState(STATE_BACKUP_LAP_WALL);
      //handler();
      return;
    }
    break;
  case STATE_BACKUP_LAP_WALL:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(BACKUP_DIST * .75);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
    {
      printf("###### BackupLapWall: Failed, going forwards\n");
      myRobot->clearDirectMotion();
      setState(STATE_FORWARD_LAP_WALL);      
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95 * .75))
    {
      printf("###### BackupLapWall: Succeeded\n");
      myRobot->clearDirectMotion();
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    break;
  case STATE_FORWARD_LAP_WALL:
    if (myNewState)
    {
      myNewState = false;
      myRobot->move(-BACKUP_DIST * .75);
      myAcquire->deactivate();
      myPickUp->deactivate();
      myDriveTo->deactivate();
      myDropOff->deactivate();
      myTableLimiter->deactivate();
    }
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
    {
      printf("###### ForwardLapWall: Failed\n");
      myRobot->clearDirectMotion();
      setState(STATE_FAILED);      
    }
    if (myStateStartTime.mSecSince() > BACKUP_TIME || 
	myStateStartPos.findDistanceTo(myRobot->getPose()) > 
	ArMath::fabs(BACKUP_DIST * .95 * .75))
    {
      printf("###### ForwardLapWall: Succeeded\n");
      myRobot->clearDirectMotion();
      setState(STATE_SWITCH);
      //handler();
      return;
    }
    break;

  case STATE_SWITCH:
    printf("!! Switching walls around.\n");
    tempColor = myDropWall;
    myDropWall = myLapWall;
    myLapWall = tempColor;
    setState(STATE_ACQUIRE_BLOCK);
    //handler();
    return;
  case STATE_FAILED:
    printf("@@@@@ Failed to complete the task!\n");
    myRobot->comInt(ArCommands::SONAR, 0);
    ArUtil::sleep(50);
    myRobot->comStr(ArCommands::SAY, "\52\77\37\62\42\70");
    ArUtil::sleep(500);
    Aria::shutdown();
    myRobot->disconnect();
    myRobot->stopRunning();
    return;
  }

}