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();
}
