Ejemplo n.º 1
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;
}
Ejemplo n.º 2
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;
}
Ejemplo n.º 3
0
int main(int argc, char **argv)
{
  // robot
  ArRobot robot;
  // the laser
  ArSick sick;


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

  // the actions we'll use to wander
  // recover from stalls
  //ArActionStallRecover recover;
  // react to bumpers
  //ArActionBumpers bumpers;
  // limiter for close obstacles
  ArActionLimiterForwards limiter("speed limiter near", 1600, 0, 0, 1.3);
  // limiter for far away obstacles
  //ArActionLimiterForwards limiterFar("speed limiter near", 300, 1000, 450, 1.1);
  //ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
  // limiter for the table sensors
  //ArActionLimiterTableSensor tableLimiter;
  // actually move the robot
  ArActionConstantVelocity constantVelocity("Constant Velocity", 1500);
  // turn the orbot if its slowed down
  ArActionTurn turn;

  // mandatory init
  Aria::init();

  // Parse all our args
  ArSimpleConnector connector(&argc, argv);
  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }
  
  // add the sonar to the robot
  //robot.addRangeDevice(&sonar);
  // add the laser to the robot
  robot.addRangeDevice(&sick);

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

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

  // add the actions
  //robot.addAction(&recover, 100);
  //robot.addAction(&bumpers, 75);
  robot.addAction(&limiter, 49);
  //robot.addAction(&limiter, 48);
  //robot.addAction(&tableLimiter, 50);
  robot.addAction(&turn, 30);
  robot.addAction(&constantVelocity, 20);

  robot.setStateReflectionRefreshTime(50);
  limiter.activate();
  turn.activate();
  constantVelocity.activate();

  robot.clearDirectMotion();
  //robot.setStateReflectionRefreshTime(50);
  robot.setRotVelMax(50);
  robot.setTransAccel(1500);
  robot.setTransDecel(100);

  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  connector.setupLaser(&sick);

  // 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");
    Aria::shutdown();
    return 1;
  }
  
  sick.lockDevice();
  sick.setMinRange(250);
  sick.unlockDevice();
  robot.lock();
  ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
  robot.addUserTask("iotest", 100, &userTaskCB);
  requestTime.setToNow();
  robot.comInt(ArCommands::IOREQUEST, 1);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.unlock();

  robot.waitForRunExit();
  // now exit
  Aria::shutdown();
  return 0;
}