task main( ){ setSensorMode(COLOR, 8); delay(1000); setSensorMode(COLOR, 2); init(); isFinal(); while( hasAnyCityLeft() ) { if( current_deep == max_deep ){ isFinal(); backToParent(); continue; } turnToNextDirection(); verifyOrigin(); while( !goToSon() ){ turnAntiClockWise(); if( current_direction == parents_map[current_x][current_y] ){ backToParent(); continue; } } } }
void MotionManager::enable( float updateFrequency, SensorMode mode, bool showsCalibrationDisplay ) { auto impl = MotionManager::get()->mImpl; impl->setSensorMode( mode ); impl->setUpdateFrequency( updateFrequency ); impl->setShowsCalibrationView( showsCalibrationDisplay ); impl->startMotionUpdates(); }
Inertial::XsensInertialSensorConnection::XsensInertialSensorConnection( ACE_Reactor & _reactor, InertialSensorImpl & _i_impl ) : reactor_( &_reactor), inertial_event_( new EventHandler( inertial_poll_task_ ) ), inertial_poll_task_( new InertialSensorPollTask( *this, *inertial_event_, _i_impl ) ), parameters_(*InertialParameters::instance ( ) ), syncInertialSensorScan ( ), syncInertialSensorScanCond ( syncInertialSensorScan ) { std::cout << "Init Hardware! " << std::endl; std::cout << "outputmode " << parameters_.outputmode << " settings " << parameters_.outputsettings << endl; deviceName_ = parameters_.device; if ( parameters_.outputmode == OUTPUT_MODE_ORIENT ) outputMode_ = OUTPUTMODE_ORIENT; else if (parameters_.outputmode == OUTPUT_MODE_CALIB_ORIENT) { outputMode_ = OUTPUTMODE_CALIB_ORIENT; } else if (parameters_.outputmode == OUTPUT_MODE_CALIB) { outputMode_ = OUTPUTMODE_CALIB; } else { outputMode_ = OUTPUTMODE_ORIENT; } if ( parameters_.outputsettings == OUTPUT_SETTINGS_EULER ) outputSettings_ = OUTPUTSETTINGS_EULER; else if (parameters_.outputsettings == OUTPUT_SETTINGS_ACC_EULER) { outputSettings_ = OUTPUTSETTINGS_ACC_EULER; } else if (parameters_.outputsettings == OUTPUT_SETTINGS_ACC_GYR_EULER) { outputSettings_ = OUTPUTSETTINGS_ACC_GYR_EULER; } else if (parameters_.outputsettings == OUTPUT_SETTINGS_CALIBRATED) { outputSettings_ = OUTPUTSETTINGS_CALIBRATED; } else if (parameters_.outputsettings == OUTPUT_SETTINGS_ACC) { outputSettings_ = OUTPUTSETTINGS_ACC; } else { outputSettings_ = OUTPUTSETTINGS_EULER; } // Open and initialize serial port if (xbus.openPort(deviceName_.c_str()) != XBRV_OK) { printf("Cannot open COM port %s\n", deviceName_.c_str()); std::cout << "no device" << std::endl; // return XBRV_INPUTCANNOTBEOPENED; } if ( setSensorMode() == false ) std::cout << "schass" << std::endl; //return XBRV_UNEXPECTEDMSG; std::cout << "Init Hardware klappt! " << std::endl; inertial_poll_task_->open( 0 ); }
//---------------------------------------------------------------------------------------------------------------------- void SMCAgent::init() { m_radius = 0.02; if (SETTINGS->hasChild("Config/GA/Evolvable")) { const ci::XmlTree& xml = SETTINGS->getChild("Config/GA/Evolvable"); // Todo: move to respective sensor class!!! // Distance sensor if (xml.getChild("DistanceSensor").getValue<bool>(0)) { m_distanceSensor = new DistanceSensor(); m_distanceSensor->fromXml(xml.getChild("DistanceSensor")); setSensorMode(xml.getChild("DistanceSensor").getAttributeValue<std::string>("Mode", "Absolute")); } else { m_distanceSensor = NULL; } // Gradient sensor if (xml.getChild("GradientSensor").getValue<bool>(0)) { m_gradientSensor = new GradientSensor(); m_gradientSensor->fromXml(xml.getChild("GradientSensor")); } else { m_gradientSensor = NULL; } // Torus sensor if (xml.getChild("TorusSensor").getValue<bool>(0)) { m_torusSensor = new TorusSensor(); m_torusSensor->fromXml(xml.getChild("TorusSensor")); } else { m_torusSensor = NULL; } setMaxSpeed(xml.getChild("MaxSpeed").getValue<double>(1.0)); setMaxAngularSpeed(degreesToRadians(xml.getChild("MaxAngularSpeed").getValue<double>(180))); setMaxAngle(degreesToRadians(xml.getChild("MaxAngle").getValue<double>(90))); setMaxPosition(xml.getChild("MaxPosition").getValue<double>(0.5)); setAngleWraps(xml.getChild("AngleWraps").getValue<bool>(1)); setPositionWraps(xml.getChild("PositionWraps").getValue<bool>(1)); m_energyInitial = xml.getChild("Energy").getAttributeValue<float>("initial", 30.0); m_energySpeedTresh = xml.getChild("Energy").getAttributeValue<float>("threshForSpeed", -1); m_evMin = xml.getChild("Energy").getAttributeValue<float>("evMin", 0); m_evMax = xml.getChild("Energy").getAttributeValue<float>("evMax", 10); m_engReplFoodSens = xml.getChild("Energy").getAttributeValue<bool>("engReplFoodSens", false); getEnvironment().fromXml(xml.getChild("Environment")); } reset(); }