//----------------------------------------------------------------------------- CCBWallSensor::CCBWallSensor(CCBDriver* driver, std::string devName) : ARangeFinder(devName) { assert(driver); mCBDriver = driver; mNumSamples = 1; mRangeData = new tRangeData[mNumSamples]; mRelativeBeamPose = new CPose2d[mNumSamples]; // TODO: get a realistic value mFov = D2R( 20.0 ); mRelativeBeamPose[0] = CPose2d(0.2, 0.2, D2R( -45.0) ); mMinRange = 0.0; mMaxRange = 4.0; setEnabled ( true ); }
//----------------------------------------------------------------------------- void ALocalizer2d::setToZero() { mPose = CPose2d( 0.0, 0.0, 0.0); }
//----------------------------------------------------------------------------- CChatterboxCtrl::CChatterboxCtrl ( ARobot* robot ) : ARobotCtrl ( robot ) { CTask* task; ADrivetrain2dof* drivetrain; // Initialize robot mTime = 0.0; mState = START; mAvoidCount = 0; mElapsedTime = 0.0; mPuckLoad = 0; mFgPoseEstValid = true; mTrackerHeadingTime = INFINITY; mTrackerPositionTime = INFINITY; mTravelTime = 0.0; mLoadingTime = 0.0; mUnloadingTime = 0.0; mStateName = StateNames[mState]; // Get robot devices mRobot->findDevice ( mPowerPack, "CB:powerpack" ); mRobot->findDevice ( drivetrain, "CB:drivetrain" ); mRobot->findDevice ( mTextDisplay, "CB:textdisplay" ); mRobot->findDevice ( mBumper, "CB:bumper" ); mRobot->findDevice ( mButton, "CB:button" ); mRobot->findDevice ( mLights, "CB:lights" ); mRobot->findDevice ( mWheelDrop, "CB:wheeldrop" ); mRobot->findDevice ( mLowSideDriver, "CB:lowsidedriver" ); mRobot->findDevice ( mFrontFiducial, "CB:front_fiducial" ); mRobot->findDevice ( mTopFiducial, "CB:top_fiducial" ); mRobot->findDevice ( mPhoto, "CB:photosensor" ); mRobot->findDevice ( mCliffSensor, "CB:cliff" ); mRobot->findDevice ( mIr, "CB:ir" ); mDrivetrain = ( CCBDrivetrain2dof* ) drivetrain; mTracker = new CAutolabTracker("Tracker", mRobot->getName(), "192.168.1.116", 6379); //mTracker = new CAutolabTracker("Tracker", "cb18", // "192.168.1.116", 6379); mRedisClient = CRedisClient::getInstance("192.168.1.116", 6379); mRobot->addDevice(mTracker); // Configure devices mVoltageLpf = mPowerPack->getVoltage(); mLights->setBlink( DOT, true, 1.0 ); mDrivetrain->setTranslationalAccelerationLimit( CLimit( -INFINITY, 0.5) ); mDrivetrain->setRotationalAccelerationLimit( CLimit(-INFINITY, INFINITY) ); mDrivetrain->setDefaultOIMode( CB_MODE_FULL ); mOdometry = mDrivetrain->getOdometry(); mDataLogger = CDataLogger::getInstance( "chatterbox.log", OVERWRITE ); mDataLogger->setInterval( 0.1 ); mOdometry->startLogging(""); mTracker->startLogging(""); mEstRobotPose.setName("EstRobotPose"); mDataLogger->addVar(&mEstRobotPose, "EstRobotPose"); mDrivetrain->setEnabled(true); //********************************************************* // Create Blue task task = new CTask("blue", BLUE, CPose2d(4.5, 2.5, 0.0), CPose2d(5.0, 9.2, 0.0) ); task->mSourceWaypointVector.push_back(CPose2d(5.5, 5.0, 0.0)); task->mSourceWaypointVector.push_back(CPose2d(5.5, 2.5, 0.0)); task->mSourceWaypointVector.push_back(CPose2d(4.5, 2.5, 0.0)); // source task->mSinkWaypointVector.push_back(CPose2d(4.0, 5.0, 0.0)); task->mSinkWaypointVector.push_back(CPose2d(4.0, 9.0, 0.0)); task->mSinkWaypointVector.push_back(CPose2d(5.0, 9.2, 0.0)); // sink mTaskVector.push_back(task); //********************************************************* // Create Purple task task = new CTask( "purple", CRgbColor(255,0,255), CPose2d(1.2, 2.0, 0.0), CPose2d(2.0, 8.0, 0.0) ); task->mSourceWaypointVector.push_back(CPose2d(3.0, 5.0, 0.0)); task->mSourceWaypointVector.push_back(CPose2d(3.0, 2.0, 0.0)); task->mSourceWaypointVector.push_back(CPose2d(1.2, 2.0, 0.0)); // source task->mSinkWaypointVector.push_back(CPose2d(1.0, 5.0, 0.0)); task->mSinkWaypointVector.push_back(CPose2d(1.0, 8.0, 0.0)); task->mSinkWaypointVector.push_back(CPose2d(2.0, 8.0, 0.0)); // sink mTaskVector.push_back(task); mCurrentTaskPtr = NULL; switchTask(); rprintf("Power: "); mPowerPack->print(); }
//----------------------------------------------------------------------------- ALocalizer2d::ALocalizer2d ( std::string devName ) : ADevice ( devName ) { printf("localizer2d zhao\n"); mCoordinateSystemOffset = CPose2d( 0.0, 0.0, 0.0 ); }