Beispiel #1
0
AREXPORT void arrobot_setpose(double x, double y, double th)
{
	AR_DEBUG_LOGINFO();
  AR_ASSERT_RETURN(robot);
  robot->lock();
  robot->moveTo(ArPose(x, y, th));
  robot->unlock();
}
/*****************************************************************
**								Robot Search Subroutine
******************************************************************/
void S_RobotMotion( ArServerClient *serverclient, ArNetPacket *socket)
{

	G_PTZHandler->reset();
	ArUtil::sleep(500);
	G_PTZHandler->tiltRel(-10);


	double robotHeading=0;
  double robotDstX = robot.getPose().getX();
  double robotDstY = robot.getPose().getY();

	//---------Generate the next motion by random numbers------------
	cout << "-------- Generate the next motion by random numbers --------" <<endl;
	srand( time( NULL ) ); 
	switch(rand()%4) 
	{
	case 0:		//¡û
		robotDstY += 1000;
		robotHeading = 90;
		break;
	case 1:		//¡ü
		robotDstX += 1000;
		robotHeading = 0;
		break;
	case 2:		//¡ú
		robotDstY -= 1000;
		robotHeading = -90;
		break;
	case 3:		//¡ý
		robotDstX -= 1000;
		robotHeading = 180;
		break;
	}


  G_PathPlanning->pathPlanToPose(ArPose(robotDstX, robotDstY, robotHeading), true,true);
	cout << "RobotMotion is processing..." <<endl;

  while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
	{
		if (G_PathPlanning->getState() == ArPathPlanningTask::ABORTED_PATHPLAN)		{ G_PathPlanning->cancelPathPlan();break;}
		else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)		{G_PathPlanning->pathPlanToPose(ArPose(-300,-100,0),true,true);}
	}

 
	serverclient->sendPacketTcp(socket);
//-------------------------------------------------------------------------------------------------------

}
void basic_turn(int turnAngal)
{
 // ArTime start;
 // G_PTZHandler->reset();
	//ArUtil::sleep(500);
	//G_PTZHandler->tiltRel(-10);
	//CameraMoveCount=0;
  //robot.lock();

	double robotHeading = robot.getPose().getTh();
	robotHeading += turnAngal;
  double robotCurrentX = robot.getPose().getX() ;
  double robotCurrentY = robot.getPose().getY();

	G_PathPlanning->pathPlanToPose(ArPose(robotCurrentX, robotCurrentY , robotHeading),true,true);
	while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL );


	//robot.setHeading(robot.getTh()+turnAngal);
 // robot.unlock();
  
  //start.setToNow();
  //while (1)
  //{
	 // robot.lock();
	 // if (robot.isHeadingDone())
	 // {
		//  printf(" Finished turn\n");
		//  
		//  ArUtil::sleep(50);
		//  cout << " current heading: " << "    " << robot.getTh()<<endl;
		//	robot.unlock();
		//  break;
	 // }
	 // if (start.mSecSince() > 5000)
	 // {
		//  printf(" Turn timed out\n");
		//  
		//  cout << " current heading: " << "    " << robot.getTh()<<endl;
		//	robot.unlock();
		//  break;
	 // }
	 // robot.unlock();
	 // ArUtil::sleep(10);
  //}  
}
Beispiel #4
0
void SimpleTask::task(void)
{
  if (!myGoalDone)
    return;

  myGoalDone = false;
  myState++;
  if (myState == 1)
    myPathTask->pathPlanToPose(ArPose(1000, 0, 0), true);
  //else if (myState == 1)
  //myPathTask->pathPlanToGoal("Goal1");
  else
  {
    myState = 0;
    myPathTask->pathPlanToPose(myHome, true);
  }
}
ArPose GPSMapTools::getCurrentPosFromGPS()
{
  ArLLACoords gpsCoords(myGPS->getLatitude(), myGPS->getLongitude(), myGPS->getAltitude());
  double x, y, z;
  x = y = z = NAN;
  // XXX need to have previously saved origin in myMapGPSCoords; either recreate
  // here each time, or reset whenever a new map is either created or loaded
  // elsewhere, and in ctor if there is a map in arnl.
  if(!myHaveMapGPSCoords)
  {
    // get origin from ArMap and setup MAp->GPS coords translator
    if(myMap->hasOriginLatLongAlt())
    {
      ArPose p = myMap->getOriginLatLong();
      myOrigin = ArLLACoords(p.getX(), p.getY(), myMap->getOriginAltitude());
      resetMapCoords(myOrigin);
    }
  }
  
  myMapGPSCoords.convertLLA2MapCoords(gpsCoords, x, y, z);
  return ArPose(x, y);
}
void GPSMapTools::setOrigin()
{
  if(!checkGPS("setting map origin")) return;
  if(!checkMap("setting map origin")) return;
  if(myRobot)
  {
    ArLog::log(ArLog::Normal, "GPSMapTools: Resetting robot odometric position to 0,0,0.");
    myRobot->moveTo(0,0,0);
    myRobot->com(ArCommands::SIM_RESET);
  }
  if(myMap->hasOriginLatLongAlt())
  {
    ArLog::log(ArLog::Terse, "GPSMapTools: setting map origin: Warning: Map already has an origin point, it will be replaced by current position.");
  }
  myOrigin = ArLLACoords(myGPS->getLatitude(), myGPS->getLongitude(), myGPS->getAltitude());
  resetMapCoords(myOrigin);
  myMap->lock();
  myMap->setOriginLatLongAlt(true, ArPose(myGPS->getLatitude(), myGPS->getLongitude()), myGPS->getAltitude());
  myMap->writeFile(myMap->getFileName(), true);
  myMap->unlock();
  reloadMapFile();
}
void S_TargetApproach( ArServerClient *serverclient, ArNetPacket *socket)
{
	//Important: to halt current movement of camera, making the reading curret.
	G_PTZHandler->haltPanTilt();
	cout << "The last step: TargetApproach!" <<endl;

   //G_PathPlanning->setCollisionRange(1000);
	//G_PathPlanning->setFrontClearance(40);
	//G_PathPlanning->setGoalDistanceTolerance(2500);
  double camAngle = -1 * G_PTZHandler->getPan();
  double robotHeading = robot.getPose().getTh();
  double robotCurrentX = robot.getPose().getX() ;
  double robotCurrentY = robot.getPose().getY();
  double angle = 0.0;
  double distance =0.0;
  
  double targetX, targetY;
  int disThreshold = 500;
   //test mode
//     G_PTZHandler->panRel(-30);
	//G_PathPlanning->setFrontClearance(40);
	//G_PathPlanning->setObsThreshold(1);

  cout << "--------------Path Planning Information--------------------" <<endl;
  cout << "SafeCollisionRange = " << G_PathPlanning->getSafeCollisionRange()  << endl;
	cout << "FrontClearance     = " << G_PathPlanning->getFrontClearance()      << endl
		   << "GoalDistanceTolerance = " << G_PathPlanning->getGoalDistanceTolerance() << endl
			 << "setGoalOccupiedFailDistance = " << G_PathPlanning->getGoalOccupiedFailDistance() << endl
			 << "getObsThreshold = " << G_PathPlanning->getObsThreshold() <<endl
			 << "getLocalPathFailDistance = " << G_PathPlanning->getLocalPathFailDistance() << endl;
			 //getCurrentGoal


  
  
//--------------- Set up the laser range device to read the distance from the target -----------------------------
 // for(int i =0; i< 20;i++)
	//sick.currentReadingPolar(robotHeading+ camAngle -2.5, robotHeading+ camAngle +2.5, &angle);
	
//Begin:  

  sick.lockDevice();

	//if (distance == 0 || distance>disThreshold)
  distance = sick.currentReadingPolar(/*robotHeading+*/ camAngle -2.5, /*robotHeading+*/ camAngle +2.5, &angle)-disThreshold;
   //double distance = sick.currentReadingPolar(89, 90, &angle);
  cout << "The closest reading is " << distance << " at " << angle << " degree , " << "disThreshold : " <<disThreshold << " " << robotHeading << " " << camAngle<< endl;

  sick.unlockDevice();
		

  
//----------------------------------------------------------------------------------------------------------------
  
  //basic_turn(camAngle);
  
  cout << "Camera Angle is " << camAngle << endl;

  cout << "before calculation, check originalX, originalY " << robotCurrentX << " " << robotCurrentY << " robotHeading is " << robotHeading << endl<<endl;  
  coordinateCalculation(robotCurrentX,robotCurrentY,&targetX,&targetY,camAngle,robotHeading,distance);
  //
  //cout << "before movement, check targetX, target Y" << targetX << " " << targetY << "robotHeading is "<< robotHeading << endl << endl;
  

	cout << "targetX : " <<targetX << " targetY" <<targetY;
  G_PathPlanning->pathPlanToPose(ArPose(targetX,targetY,camAngle),true,true);
  cout << "RobotMotion is processing..." <<endl;

  G_PTZHandler->reset();
	ArUtil::sleep(200);
	G_PTZHandler->tiltRel(-10);
  while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
  {
		if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE)
		{
			G_PathPlanning->cancelPathPlan();cout <<  "x " << robot.getPose().getX()<< " y " <<robot.getPose().getY() <<endl; break;
		}
		else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)
		{
//			getchar();
//getchar();
//getchar();
//			getchar();
//			disThreshold+=50;
//			
//			goto Begin;
		}
  }

  //serverclient->sendPacketTcp(socket);

  ArUtil::sleep(3000);


  cout << "RobotMotion is heading home..." <<endl;
  G_PathPlanning->pathPlanToPose(ArPose(0,0,0),true,true);
  
  while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL )
	{
		//cout << G_PathPlanning->getState() <<endl;
		if (G_PathPlanning->getState() == ArPathPlanningTask::FAILED_MOVE)
		{
			G_PathPlanning->cancelPathPlan(); break;
		}


	//	//else if(G_PathPlanning->getState() == ArPathPlanningTask::FAILED_PLAN)
 // //  {G_PathPlanning->pathPlanToPose(ArPose(-300,30,0),true,true);}

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

  ArArgumentParser parser(&argc, argv);
  ArServerSimpleOpener simpleOpener(&parser);

  // parse the command line... fail and print the help if the parsing fails
  // or if help was requested
  parser.loadDefaultArguments();
  if (!simpleOpener.parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    simpleOpener.logOptions();
    exit(1);
  }


  // first open the server up
  if (!simpleOpener.open(&server))
  {
    if (simpleOpener.wasUserFileBad())
      printf("Error: Bad user/password/permissions file.\n");
    else
      printf("Error: Could not open server port. Use -help to see options.\n");
    exit(1);
  }


  // This is the service that provides drawing data to the client.
  ArServerInfoDrawings drawings(&server);

  // Add our custom drawings
  drawings.addDrawing(
      //                shape:      color:               size:   layer:
      new ArDrawingData("polyLine", ArColor(255, 0, 0),  2,      49),
      "exampleDrawing_Home", 
      new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleHomeDrawingNetCallback)
  );
  drawings.addDrawing(                                    
      new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48),
      "exampleDrawing_Dots", 
      new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleDotsDrawingNetCallback)
  );
  drawings.addDrawing(
      new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52),
      "exampleDrawing_XMarksTheSpot", 
      new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleXDrawingNetCallback)
  );
  drawings.addDrawing(
      new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100),
      "exampleDrawing_Arrows", 
      new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleArrowsDrawingNetCallback)
  );

  Circle circle(&drawings, "exampleDrawing_circle", 
    new ArDrawingData("polySegments", ArColor(255, 150, 0), 3, 120));
  circle.setPos(ArPose(0, -5000));
  circle.setRadius(1000);
  circle.setNumPoints(360);

  // log whatever we wanted to before the runAsync
  simpleOpener.checkAndLog();

  // run the server thread in the background
  server.runAsync();

  printf("Server is now running...\n");


  // Add a key handler mostly that windows can exit by pressing
  // escape, note that the key handler prevents you from running this program
  // in the background on Linux.
  ArKeyHandler *keyHandler;
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    printf("To exit, press escape.\n");
  }

 
  double circleRadius = 1000;
  while(true) 
  {
    ArUtil::sleep(100);
    circleRadius += 50;
    if(circleRadius > 5000)
      circleRadius = 0;
    circle.setRadius(circleRadius);
  }

  Aria::shutdown();
  exit(0);  
}
int main(int argc, char **argv)
{
  int ret;
  std::string str;
  ArSerialConnection con;
  double dist, angle;
  std::list<ArPoseWithTime *> *readings;
  std::list<ArPoseWithTime *>::iterator it;
  double farDist, farAngle;
  bool found;
  ArGlobalFunctor failedConnectCB(&failedConnect);
  
  Aria::init();
  
  sick = new ArSick;
  // open the connection, if it fails, exit
  if ((ret = con.open("/dev/ttyS2")) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  sick->configure(false);
  sick->setDeviceConnection(&con);

  sick->setFilterNearDist(0);
  sick->setMinRange(0);
  sick->addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
  sick->runAsync();

  ArUtil::sleep(100);
  sick->lockDevice();
  sick->asyncConnect();
  sick->unlockDevice();
  while (!sick->isConnected())
    ArUtil::sleep(100);

  printf("Connected\n");
//  while (sick->isConnected())
  int times = 0;
  while (sick->getRunning())
  {
    //dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle);
    sick->lockDevice();
    dist = sick->currentReadingPolar(-90, 90, &angle);
    if (dist < sick->getMaxRange())
      printf("Closest reading %.2f mm away at %.2f degrees\n", dist, angle);
    else
      printf("No close reading.\n");
    readings = sick->getCurrentBuffer();
    int i = 0;
    for (it = readings->begin(), found = false; it != readings->end(); it++)
    {
      i++;
      dist = (*it)->findDistanceTo(ArPose(0, 0));
      angle = (*it)->findAngleTo(ArPose(0, 0));
      if (!found || dist > farDist)
      {
	found = true;
	farDist = dist;
	farAngle = angle;
      }
    }
    if (found)
      printf("Furthest reading %.2f mm away at %.2f degrees\n", 
	     farDist, farAngle);
    else
      printf("No far reading found.\n");
    printf("%d readings\n\n", i);
    
    sick->unlockDevice();
    ArUtil::sleep(100);
  }
  sick->lockDevice();
  sick->stopRunning();
  sick->disconnect();
  sick->unlockDevice();
  system("echo 'succeeded' >> results");
  return 0;
}
Beispiel #10
0
ArPose RosArnlNode::rosPoseToArPose(const geometry_msgs::Pose& p)
{
  return ArPose( p.position.x * 1000.0, p.position.y * 1000.0, tf::getYaw(p.orientation) / (M_PI/180.0) );
}
AREXPORT void ArLaser::setSensorPosition(
	double x, double y, double th, double z)
{
  setSensorPosition(ArPose(x, y, th), z);
}
int _tmain(int argc, char** argv)
{
    //-------------- 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);
    ActionTurns turn(400, 55);
    robot.addAction(&turn, 49);
    ActionTurns turn2(400, 55);
    robot.addAction(&turn2, 49);
    turn.deactivate();
    turn2.deactivate();

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

    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();
            }
            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)
            {
                //secuencia de drop de la figura
            }
            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();

//----------------------------------------------------------------------------------//

    return 0;
}
int main(int argc, char **argv)
{


  Aria::init();

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

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

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

 imgdb.setRobot(&robot);

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

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


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

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

  robot.setAbsoluteMaxRotVel(30);

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

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

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

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



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

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

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


  const int duration = 500000;

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

  while (Aria::getRunning())
  {
    current = imgdb.getEdge();
    for (int i = 0; i < current->edge.size(); i++) {
        for (int j = 0; j < current->edge[i].size() - 1; j++) {
    //        printf("i=%d\n",i);
            double dis = sqrt(pow(current->edge[i][j].z,2)+(pow(current->edge[i][j].x,2)));
       //     printf("dis=%f,mindis=%f\n",dis,minDistance);
             if(dis<minDistance){
                minDistance=dis;
   printf("min=%f\n",minDistance);
             }
        }
    }
    //引入避障action,前方安全距离为500mm,侧边安全距离为340mm
 //   Avoid avoidSide("Avoid side", ArPose(0, 0), 800, 200, minDistance);
  //  robot.addAction(&avoidSide,80);
    robot.lock();

    if (first || avoidSide.haveAchievedGoal())
    {
      first = false;
      goalNum++;
      printf("count goalNum = %d\n", goalNum);
      if (goalNum > 2){
        goalNum = 1; // start again at goal #1
      }
//   avoidSide.setGoal(ArPose(10000,0));
        if(goalNum==1){
        printf("zuobiaox=%f,zuobiaoy=%f\n",-current->person_point.z,current->person_point.x);
        avoidSide.setGoal(ArPose(1000*current->person_point.z, -1000*current->person_point.x));
        printf("goalNum == 1\n\n");
        }
        else if(goalNum==2){
            printf("zuobiaox=%f,zuobiaoy=%f\n",-current->person_point.z,current->person_point.x);
            avoidSide.setGoal(ArPose(1000*current->person_point.z, -1000*current->person_point.x));
            printf("goalNum == 2\n\n");
        }

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

  Aria::exit(0);
  return 0;
}
Beispiel #14
0
void GPSMapTools::addHomeHere(ArArgumentBuilder *args)
{
  if(!checkGPS("adding home")) return;
  if(!checkMap("adding home")) return;
  ArPose p = getCurrentPosFromGPS();
  ArLog::log(ArLog::Normal, "GPSMapTools: Adding home in map at GPS position (x=%.2f, y=%.2f)",  p.getX(), p.getY());
  myMap->lock();
  ArMapObject *newobj;
  myMap->getMapObjects()->push_back(newobj = new ArMapObject("Home", p, "Home", "ICON", args->getFullString(), false, ArPose(0, 0), ArPose(0, 0)));
  printf("\tnew map object is:\n\t%s\n", newobj->toString());
  newobj->log();
  myMap->writeFile(myMap->getFileName(), true);
  myMap->unlock();
  reloadMapFile();
}
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;
}
int main(void)
{
  ArPose pose;

  ArLine xLine(-2000, 0, 2000, 0);
  ArLine yLine(100, 500, 100, -500);
  ArLineSegment xLineSeg(-2000, 0, 2000, 0);
  ArLineSegment yLineSeg(100, 500, 100, -500);

  // test all our segments
  testIntersection(&xLine, &yLine, 100, 0, "xLine and yLine");
  testIntersection(&xLineSeg, &yLine, 100, 0, "xLineSeg and yLine");
  testIntersection(&yLineSeg, &xLine, 100, 0, "yLineSeg and xLine");
  testIntersection(&xLineSeg, &yLineSeg, 100, 0, "xLineSeg and yLineSeg");
  

  // test the perp on all the segments
  testPerp(&xLineSeg, ArPose(-2000, 50), ArPose(-2000, 0), "xLineSeg end1");
  testPerp(&xLineSeg, ArPose(2000, 50), ArPose(2000, 0), "xLineSeg end2");
  testPerp(&xLineSeg, ArPose(357, 50), ArPose(357, 0), "xLineSeg middle");
  testNotPerp(&xLineSeg, ArPose(2001, 0), "xLineSeg beyond end2");
  testNotPerp(&xLineSeg, ArPose(3000, 0), "xLineSeg way beyond end2");
  testNotPerp(&xLineSeg, ArPose(-2001, 0), "xLineSeg beyond end1");
  testNotPerp(&xLineSeg, ArPose(-3000, 0), "xLineSeg way beyond end1");
  
  testPerp(&xLineSeg, ArPose(1000, 0), ArPose(1000, 0), "xLineSeg point on line");

  printf("All tests completed successfully\n");

  return 0;
}
AREXPORT bool ArLineFinder::combineLines(void)
{
  int start = 0;
  int len = myLines->size();
  // this is the min line distance
  std::map<int, ArLineFinderSegment *> *newLines;
  int numNewLines = 0;
  int numNewMerges = 0;
  ArLineFinderSegment *newLine;
  
  newLines = new std::map<int, ArLineFinderSegment *>;

  if (myPrinting)
    ArLog::log(ArLog::Normal, "new iteration\n");
  
  bool nextMerged = false;
  for (start = 0; start < len; start++)
  {
    if (nextMerged)
    {
      nextMerged = false;
      continue;
    }

    if (start + 1 == len)
    {
      if (myPrinting)
	ArLog::log(ArLog::Normal, "inserted last one %g",
 	     ArPose((*myLines)[start]->getX1(), 
		    (*myLines)[start]->getY1()).findDistanceTo(
		  ArPose((*myLines)[start]->getX2(), (*myLines)[start]->getY2())));
      (*newLines)[numNewLines] = new ArLineFinderSegment(*((*myLines)[start]));
      numNewLines++;
      continue;
    }

    newLine = averageSegments((*myLines)[start], (*myLines)[start+1]);
    if (newLine != NULL)
    {
      
      if (myPrinting)
	ArLog::log(ArLog::Normal, "merged %g %g to %g", 
		   (*myLines)[start]->getLength(),
		   (*myLines)[start+1]->getLength(),
		   newLine->getLength());
      (*newLines)[numNewLines] = newLine;
      numNewLines++;
      numNewMerges++;
      nextMerged = true;
    }
    else
    {
      if (myPrinting)
	ArLog::log(ArLog::Normal, "inserted anyways %g", 
		   (*myLines)[start]->getLength());
      (*newLines)[numNewLines] = new ArLineFinderSegment(*((*myLines)[start]));
      numNewLines++;
    }
    
  }

  // move the new lines over and delete the old ones
  if (myLines != NULL && myLines->begin() != myLines->end())
  {
    ArUtil::deleteSetPairs(myLines->begin(), myLines->end());
    delete myLines;
    myLines = NULL;
  } 
  else if (myLines != NULL)
  {  
    delete myLines;
    myLines = NULL;
  }
  myLines = newLines;
  // if we didn't merge any just return
  if (numNewMerges == 0)
    return true;
  
  // otherwise do it again
  return combineLines();
}
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;
}
int APIENTRY _tWinMain(HINSTANCE hInstance,
                     HINSTANCE hPrevInstance,
                     LPTSTR    lpCmdLine,
                     int       nCmdShow)
{
	UNREFERENCED_PARAMETER(hPrevInstance);
	UNREFERENCED_PARAMETER(lpCmdLine);

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

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

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


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

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

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

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

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

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

      ArLog::log(ArLog::Normal, "Siguiente punto en %.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 desconectado al terminal el sleep
  Aria::shutdown();
  return 0;

//----------------------------------------------------------------------------------------------------------

 	// TODO: Place code here.
	MSG msg;
	HACCEL hAccelTable;

	// Initialize global strings
	LoadString(hInstance, IDS_APP_TITLE, szTitle, MAX_LOADSTRING);
	LoadString(hInstance, IDC_VENTANAROBOT, szWindowClass, MAX_LOADSTRING);
	MyRegisterClass(hInstance);

	// Perform application initialization:
	if (!InitInstance (hInstance, nCmdShow))
	{
		return FALSE;
	}

	hAccelTable = LoadAccelerators(hInstance, MAKEINTRESOURCE(IDC_VENTANAROBOT));

	// Main message loop:
	while (GetMessage(&msg, NULL, 0, 0))
	{
		if (!TranslateAccelerator(msg.hwnd, hAccelTable, &msg))
		{
			TranslateMessage(&msg);
			DispatchMessage(&msg);
		}
	}

	return (int) msg.wParam;
}