Ejemplo n.º 1
0
//-----------------------------------------------------------------------------
int CCBRobot::init()
{
  if ( mFgInitialized ) {
    PRT_WARN0( "Robot already initialized" );
    return 1;
  }
  if ( mCBDriver->init() == 0 ) {
    return 0; // failure
  }
  if ( mCBDriver->openPort( "/dev/ttyS2" ) == 0 ) {
    ERROR0( "Failed to open create port: /dev/ttyS2" );
    return 0; // failure
  }

  mFgInitialized = true;
  return 1; // success
}
Ejemplo n.º 2
0
//-----------------------------------------------------------------------------
tActionResult CChatterboxCtrl::actionUnloading()
{
  if (mElapsedTime > 2.0) {
    mUnloadingTime = mElapsedTime;

    return COMPLETED;
  }

  mDrivetrain->stop();
  mLights->setLight( ALL_LIGHTS, BLACK );
  if (mPuckLoad > 0) {
    if (system("madplay r2d2.mp3") == -1)
      PRT_WARN0("Sound error ");
    mPuckLoad = 0;
  }

  return IN_PROGRESS;
}
Ejemplo n.º 3
0
//-----------------------------------------------------------------------------
int CCBRobot::findDevice( ABinarySensorArray* &device, std::string devName )
{
  if ( not mFgInitialized ) {
    PRT_WARN0( "Robot is not initialized, call init() first" );
    device = NULL;
    return 0; // error
  }

  if (( devName !=  CB_DEVICE_BUMPER ) &&
      ( devName !=  CB_DEVICE_VIRTUAL_WALL ) &&
      ( devName !=  CB_DEVICE_CLIFF ) &&
      ( devName !=  CB_DEVICE_OVERCURRENT ) &&
      ( devName !=  CB_DEVICE_BUTTON ) &&
      ( devName !=  CB_DEVICE_WHEEL_DROP ) ) {
    ERROR1( "No such device: %s", devName.c_str() );
    device = NULL;
    return 0; // error
  }

  //************************************
  // Bumper
  if ( devName ==  CB_DEVICE_BUMPER ) {
    // check if device already exists
    if ( mCBBumper == NULL ) {
      mCBBumper = new CCBBumper( mCBDriver,  CB_DEVICE_BUMPER );
      device = mCBBumper;
      return mCBBumper->init();
    }

    // return already existing device
    device = mCBBumper;
    return 1;
  }

  //************************************
  // Virtual wall
  if ( devName ==  CB_DEVICE_VIRTUAL_WALL ) {
    // check if device already exists
    if ( mCBVirtualWall == NULL ) {
      mCBVirtualWall = new CCBVirtualWallSensor( mCBDriver,  CB_DEVICE_VIRTUAL_WALL );
      device = mCBVirtualWall;
      return mCBVirtualWall->init();
    }

    // return already existing device
    device = mCBVirtualWall;
    return 1;
  }

  //************************************
  // Button
  if ( devName ==  CB_DEVICE_BUTTON ) {
    // check if device already exists
    if ( mCBCreateButton == NULL ) {
      mCBCreateButton = new CCBCreateButton( mCBDriver,  CB_DEVICE_BUTTON );
      device = mCBCreateButton;
      return mCBCreateButton->init();
    }

    // return already existing device
    device = mCBCreateButton;
    return 1;
  }

  //************************************
  // Wheel drop sensor
  if ( devName ==  CB_DEVICE_WHEEL_DROP ) {
    // check if device already exists
    if ( mCBWheelDropSensor == NULL ) {
      mCBWheelDropSensor =
        new CCBWheelDropSensor( mCBDriver, CB_DEVICE_WHEEL_DROP );
      device = mCBWheelDropSensor;
      return mCBWheelDropSensor->init();
    }

    // return already existing device
    device = mCBWheelDropSensor;
    return 1;
  }

  //************************************
  // Cliff sensor
  if ( devName ==  CB_DEVICE_CLIFF ) {
    // check if device already exists
    if ( mCBCliffSensor == NULL ) {
      mCBCliffSensor = new CCBCliffSensor( mCBDriver,  CB_DEVICE_CLIFF );
      device = mCBCliffSensor;
      return mCBCliffSensor->init();
    }

    // return already existing device
    device = mCBCliffSensor;
    return 1;
  }

  //************************************
  // Overcurrent sensor
  if ( devName ==  CB_DEVICE_OVERCURRENT ) {
    // check if device already exists
    if ( mCBOverCurrentSensor == NULL ) {
      mCBOverCurrentSensor = new CCBOverCurrentSensor( mCBDriver,  CB_DEVICE_OVERCURRENT );
      device = mCBOverCurrentSensor;
      return mCBOverCurrentSensor->init();
    }

    // return already existing device
    device = mCBOverCurrentSensor;
    return 1;
  }

  return 0; // should never get here
}
Ejemplo n.º 4
0
//-----------------------------------------------------------------------------
int CCBRobot::findDevice( ARangeFinder* &device, std::string devName )
{
  if ( not mFgInitialized ) {
    PRT_WARN0( "Robot is not initialized, call init() first" );
    device = NULL;
    return 0; // error
  }

  if (( devName != CB_DEVICE_LASER ) &&
      ( devName != CB_DEVICE_WALL ) &&
      ( devName != CB_DEVICE_IR ) ) {
    ERROR1( "No such device: %s", devName.c_str() );
    device = NULL;
    return 0; // error
  }

  //************************************
  // Laser

  if ( devName == CB_DEVICE_LASER ) {
    // check if device already exists
    if ( mCBLaser == NULL ) {
      mCBLaser = new CCBLaser( mCBDriver, CB_DEVICE_LASER );
      device = mCBLaser;
      return mCBLaser->init();
    }

    // return already existing device
    device = mCBLaser;
    return 1; // success
  }

  //************************************
  // IR sensor
  if ( devName == CB_DEVICE_IR ) {
    // check if device already exists
    if ( mCBIrSensor == NULL ) {
      mCBIrSensor = new CCBIrSensor( mCBDriver, CB_DEVICE_IR );
      device = mCBIrSensor;
      return mCBIrSensor->init();
    }

    // return already existing device
    device = mCBIrSensor;
    return 1; // success
  }

  //************************************
  // Wall sensor
  if ( devName == CB_DEVICE_WALL ) {
    // check if device already exists
    if ( mCBWallSensor == NULL ) {
      mCBWallSensor = new CCBWallSensor( mCBDriver, CB_DEVICE_WALL );
      device = mCBWallSensor;
      return mCBWallSensor->init();
    }

    // return already existing device
    device = mCBWallSensor;
    return 1; // success
  }

  return 0; // should not be able to reach this, but silences compiler
}
Ejemplo n.º 5
0
//-----------------------------------------------------------------------------
void CCBRobot::run()
{
  if ( not mFgInitialized ) {
    PRT_WARN0( "Robot is not initialized, call init() first" );
    return;
  }

  while ( mFgRunning ) {
    // get data from ICreate
    //printf("mLastLoopDuration %f \n",mLastLoopDuration);
    if ( mCBDriver->readSensorData( mLastLoopDuration ) == 1 ) {

      // update all devices
      if ( mCBDrivetrain )
        mCBDrivetrain->updateData( mUpdateInterval );
      if ( mCBPowerPack )
        mCBPowerPack->updateData( mLastLoopDuration );
      if ( mCBLaser )
        mCBLaser->updateData( mLastLoopDuration );
      if ( mCBLights )
        mCBLights->updateData( mLastLoopDuration );
      if ( mCBSound )
        mCBSound->updateData( mLastLoopDuration );
      if ( mCBIrSensor )
        mCBIrSensor->updateData( mLastLoopDuration );
      if ( mCBTextDisplay )
        mCBTextDisplay->updateData( mLastLoopDuration );
      if ( mCBBumper )
        mCBBumper->updateData( mLastLoopDuration );
      if ( mCBWallSensor )
        mCBWallSensor->updateData( mLastLoopDuration );
      if ( mCBWheelDropSensor )
        mCBWheelDropSensor->updateData( mLastLoopDuration );
      if ( mCBCliffSensor )
        mCBCliffSensor->updateData( mLastLoopDuration );
      if ( mCBOverCurrentSensor )
        mCBOverCurrentSensor->updateData( mLastLoopDuration );
      if ( mCBFrontFiducial )
        mCBFrontFiducial->updateData( mLastLoopDuration );
      if ( mCBTopFiducial )
        mCBTopFiducial->updateData( mLastLoopDuration );
      if ( mCBPhotoSensor )
        mCBPhotoSensor->updateData( mLastLoopDuration );
      if ( mCBCreateButton )
        mCBCreateButton->updateData( mLastLoopDuration );
      if ( mCBVirtualWall )
        mCBVirtualWall->updateData( mLastLoopDuration );

      updateDevices();
      // Low side drivers updateData() is empty, no need to call it here
      //if ( mCBLowSideDriver )
      //  mCBLowSideDriver->updateData();
    }
    // update all registered controllers
    updateControllers();

    //******************************************************
    // last step - keep everything in sync
    synchronize( mUpdateInterval );
  } // while
}
Ejemplo n.º 6
0
//-----------------------------------------------------------------------------
extern "C" int Init ( Stg::Model* mod )
{
  std::string logFileName = "fasr_analyst.log";
  std::string value;
  float probBroadcast = 1.0;
  char* policy = new char[40];
  static bool init = false;
  Rapi::CLooseStageRobot* robot;
  CTaskManager* taskManager;
  CAnalyst* analyst;
  ABaseRobotCtrl* robotCtrl;

  static CCharger charger ( mod->GetWorld()->GetModel ( "charger" ) );
  static CDestination depotDestination ( mod->GetWorld()->GetModel ( "depot" ) );


  if ( not init ) {
    init = true;
    //mod->AddShutdownCallback ( ( Stg::stg_model_callback_t ) shutDownCallback, NULL );
    printf ( "-----------------------------------\n" );
    printf ( "FASR \n" );
    printf ( "  build %s %s \n", __DATE__, __TIME__ );
    printf ( "  compiled against RAPI version %s (%s) build %s\n", RAPI_VERSION(),
             RAPI_GIT_VERSION(), RAPI_BUILD() );
    printf ( "\n" );

    //************************************
    // init general stuff
    ErrorInit ( 2, false );
    initRandomNumberGenerator();
  }
  //************************************
  // create robot and its controller
  robot = new Rapi::CLooseStageRobot ( mod );
  if ( ! mod->GetPropertyStr ( "policy", &policy,  NULL ) ) {
    PRT_WARN0 ( "No policy specified " );
    exit ( -1 );
  }
  else {
    if ( strcmp ( policy, "fixed" ) == 0 )
      robotCtrl = new CFixedRatioPolicyRobotCtrl ( robot );
    else if ( strcmp ( policy, "replan" ) == 0 )
      robotCtrl = new CReplanPolicyRobotCtrl ( robot );
    else if ( strcmp ( policy, "static" ) == 0 )
      robotCtrl = new CStaticPolicyRobotCtrl ( robot );
    else if ( strcmp ( policy, "loop" ) == 0 )
      robotCtrl = new CLoopPolicyRobotCtrl ( robot );
    else if ( strcmp ( policy, "epsilon" ) == 0 )
      robotCtrl = new CEpsilonSamplerRobotCtrl ( robot );
    else if ( strcmp ( policy, "wait" ) == 0 ) {
      for ( unsigned int i=0; i< Stg::World::args.size(); i++ ) {
        if ( Stg::World::args[i].compare ( 0, 3, "-pb" ) == 0 ) {
          value = Stg::World::args[i].substr ( 4, Stg::World::args[i].size()-4 );
          probBroadcast = atof ( value.c_str() );
        }
      }
      printf ( "running with prob broadcast %f \n", probBroadcast );
      robotCtrl = new CWaitProbPolicyRobotCtrl ( robot, probBroadcast );
    }
    else {
      PRT_WARN1 ( "Unknown policy: %s", policy );
      exit ( -1 );
    }
  }

  //************************************
  // determine name of log file
  for ( unsigned int i=0; i< Stg::World::args.size(); i++ ) {
    if ( Stg::World::args[i].compare ( 0, 3, "-lf" ) == 0 ) {
      logFileName = Stg::World::args[i].substr ( 4, Stg::World::args[i].size()-4 );
    }
  }

  //************************************
  // get tasks from the task manager and hand them to the robot
  taskManager = CTaskManager::getInstance (
                  mod->GetWorld()->GetModel ( "taskmanager" ) );
  analyst = CAnalyst::getInstance ( mod->GetWorld()->GetModel ( "taskmanager" ), logFileName );

  taskManager->registerRobot ( robotCtrl );
  analyst->registerRobot ( robotCtrl );
  robotCtrl->addCharger ( &charger );
  robotCtrl->setDepot ( &depotDestination );

#ifdef GUI
  gui = CFasrGui::getInstance ( 0, NULL );
  gui->registerRobot ( robot );
#endif

  return 0; // ok

}