//----------------------------------------------------------------------------- void CCBRobot::synchronize( double interval ) { double timeNow; double duration; double sleepDuration = 0.0; struct timeval tv; gettimeofday( &tv, 0 ); timeNow = tv.tv_sec + tv.tv_usec * 1e-6; duration = timeNow - mLastSynchronizeTime; if ( duration < interval ) { sleepDuration = interval - duration; mSlowRunCount = (mSlowRunCount > 0 ) ? mSlowRunCount - 1 : 0; usleep(( int )(( sleepDuration ) * 1e6 ) ); mLastLoopDuration = interval; } else { mSlowRunCount = (mSlowRunCount < mSlowRunThreshold) ? mSlowRunCount + 1 : mSlowRunCount; if( mSlowRunCount >= mSlowRunThreshold ) { mSlowRunCount = 0; mFgRunningSlowly = true; PRT_WARN1( "Control loop running consistently slowly (%f sec)\n", mLastLoopDuration ); } PRT_MSG1( 8, "Control loop ran slowly (%f sec)\n", mLastLoopDuration ); // the first time we often get outragously large values // which cause bad things to happen, so we limit this value // to the interval [0,5] mLastLoopDuration = limit(duration, 0.0, 5.0); } //gettimeofday( &tv, 0 ); //mLastSynchronizeTime = tv.tv_sec + tv.tv_usec * 1e-6; mLastSynchronizeTime = timeNow + sleepDuration; //printf("timeNow %f\n", timeNow); }
//----------------------------------------------------------------------------- CCharger::CCharger ( Stg::Model* mod ) : CDestination ( mod ) { if ( ! mStgModel->GetPropertyFloat ( "chargerate", &mChargeRate, 0.0 ) ) PRT_WARN1 ( "chargerate not specified for sink %s\n", mStgModel->Token() ); }
//----------------------------------------------------------------------------- void CChatterboxCtrl::updateData ( float dt ) { float distance; mTime += dt; mElapsedTime += dt; mVoltageLpf = mVoltageLpf + ( dt / TAU_VOLTAGE_LPF ) * ( mPowerPack->getVoltage() - mVoltageLpf ); // update position estimate estimateRobotPose(); //mDrivetrain->setEnabled(false); if (mWheelDrop->isAnyTriggered() ) { mState = FLYING; } switch (mState) { case START: if ( mDrivetrain->getOIMode() != CB_MODE_FULL ) { mDrivetrain->setDefaultOIMode ( CB_MODE_FULL ); } if (mTracker->isValid() ) { mOdometry->setPose(mTracker->getPose() ); mEstRobotPose = mTracker->getPose(); } if (mElapsedTime > 5.0 ) { if (mPuckLoad == 0) mState = GOTO_SOURCE; else mState = GOTO_SINK; } break; case GOTO_SOURCE: if (mFgStateChanged) mWaypointId = 0; if (mVoltageLpf < CHARGING_VOLTAGE) { mState = GOTO_CHARGER; } //if ( driveTo(mCurrentTaskPtr->getSourcePose(), distance ) == COMPLETED ) { if ( actionFollowWaypointVector(&(mCurrentTaskPtr->mSourceWaypointVector) ) == COMPLETED) { mTravelTime += mElapsedTime; mState = LOADING; } else if (mCurrentTaskPtr->getSourcePose().distance(mEstRobotPose) < SOURCE_PROXIMITY) { mState = WAIT_AT_SOURCE; } break; case WAIT_AT_SOURCE: if ( (mElapsedTime > MIN_SOURCE_WAIT_TIME) && (drand48() < GIVE_UP_PROBABILITY) ) { switchTask(); } if ( driveTo(mCurrentTaskPtr->getSourcePose(), 0.2, distance ) == COMPLETED ) { mTravelTime += mElapsedTime; mState = LOADING; } break; case GOTO_SINK: if (mFgStateChanged) mWaypointId = 0; //if ( driveTo(mCurrentTaskPtr->getSourcePose(), distance ) == COMPLETED ) { if ( actionFollowWaypointVector(&(mCurrentTaskPtr->mSinkWaypointVector) ) == COMPLETED) { //if ( driveTo(mCurrentTaskPtr->getSinkPose(), distance ) == COMPLETED ) { mTravelTime += mElapsedTime; mState = UNLOADING; } break; case GOTO_CHARGER: if (actionGotoCharger() == COMPLETED) mState = DOCKING; break; case DOCKING: if (actionDocking() == COMPLETED) break; case UNDOCKING: if (actionUndocking() == COMPLETED) break; case CHARGING: if (actionCharging() == COMPLETED) break; case LOADING: if (actionLoading() == COMPLETED) mState = GOTO_SINK; break; case UNLOADING: if (actionUnloading() == COMPLETED) { mState = GOTO_SOURCE; } break; case FLYING: mDrivetrain->stop(); if (not mWheelDrop->isAnyTriggered() ) { mState = START; } break; default: PRT_WARN1("Unknown FSM state %d ", mState); mState = START; break; } // switch if (mTracker->isValid() ) { mLights->setLight(0, GREEN); mLights->setLight(3, GREEN); } else { mLights->setLight(0, RED); mLights->setLight(3, RED); } mDataLogger->write( mTime ); if ( mPrevState != mState) { mPrevState = mState; mElapsedTime = 0; mFgStateChanged = true; rprintf("state %s, task %s\n", StateNames[mState].c_str(), mCurrentTaskPtr->getName().c_str()); mPowerPack->print(); } else { mFgStateChanged = false; } }
//----------------------------------------------------------------------------- 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 }