CPFA_loop_functions::CPFA_loop_functions() : RNG(argos::CRandom::CreateRNG("argos")), MaxSimTime(3600 * GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick()), ResourceDensityDelay(0), RandomSeed(GetSimulator().GetRandomSeed()), SimCounter(0), MaxSimCounter(1), VariableFoodPlacement(0), OutputData(0), DrawDensityRate(4), DrawIDs(1), DrawTrails(1), DrawTargetRays(1), FoodDistribution(2), FoodItemCount(256), NumberOfClusters(4), ClusterWidthX(8), ClusterLengthY(8), PowerRank(4), ProbabilityOfSwitchingToSearching(0.0), ProbabilityOfReturningToNest(0.0), UninformedSearchVariation(0.0), RateOfInformedSearchDecay(0.0), RateOfSiteFidelity(0.0), RateOfLayingPheromone(0.0), RateOfPheromoneDecay(0.0), FoodRadius(0.05), FoodRadiusSquared(0.0025), NestRadius(0.25), NestRadiusSquared(0.0625), NestElevation(0.01), SearchRadiusSquared((4.0 * FoodRadius) * (4.0 * FoodRadius)), // We are looking at a 4 by 4 square (3 targets + 2*1/2 target gaps) score(0), PrintFinalScore(0) {}
void CPFA_loop_functions::UpdatePheromoneList() { // Return if this is not a tick that lands on a 0.5 second interval if ((int)(GetSpace().GetSimulationClock()) % ((int)(GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick()) / 2) != 0) return; std::vector<Pheromone> new_p_list; argos::Real t = GetSpace().GetSimulationClock() / GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick(); //ofstream log_output_stream; //log_output_stream.open("time.txt", ios::app); //log_output_stream << t << ", " << GetSpace().GetSimulationClock() << ", " << GetSimulator().GetPhysicsEngine("default").GetInverseSimulationClockTick() << endl; //log_output_stream.close(); for(size_t i = 0; i < PheromoneList.size(); i++) { PheromoneList[i].Update(t); if(PheromoneList[i].IsActive() == true) { new_p_list.push_back(PheromoneList[i]); } } PheromoneList = new_p_list; }
void CFloorPowerFunctions::SetPovCamera() { m_Renderer = dynamic_cast<CQTOpenGLRender*>(&GetSimulator().GetVisualization()); m_Camera = &m_Renderer->GetMainWindow().GetOpenGLWidget().GetCamera(); m_CameraSettings = &m_Camera->GetActiveSettings(); m_SelectedEntity = dynamic_cast<CEFootBotEntity*>(m_Renderer->GetMainWindow().GetOpenGLWidget().GetSelectedEntity()); if(m_SelectedEntity != NULL){ // get robot position and orientation CVector3 pos_vec = m_SelectedEntity->GetEmbodiedEntity().GetOriginAnchor().Position; CQuaternion orientation = m_SelectedEntity->GetEmbodiedEntity().GetOriginAnchor().Orientation; // get robot angle CRadians cZAngle, cYAngle, cXAngle; orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle); // calculate camera direction vector double x = pos_vec.GetX() + cos(cZAngle.GetValue()); double y = pos_vec.GetY() + sin(cZAngle.GetValue()); // fixate X so you don't get tilt m_CameraSettings->Up.SetX(0); // set position and viewpoint target m_CameraSettings->Position.Set((double)(pos_vec.GetX()), (double)(pos_vec.GetY()), POV_HEIGHT); m_CameraSettings->Target.Set(x, y, POV_HEIGHT); } }
/***** * This function resets all iAnts and restarts the simulation based on initial * conditions set in the XML file. *****/ void DSA_loop_functions::Reset() { if(VariableSeed == 1) GetSimulator().SetRandomSeed(++RandomSeed); //GetSimulator().Reset(); GetSpace().Reset(); SimTime = 0; ResourceDensityDelay = 0; FoodList.clear(); //PheromoneList.clear(); //FidelityList.clear(); TargetRayList.clear(); SetFoodDistribution(); size_t STOP = 0; size_t robots = 0; for(size_t i = 0; i < iAnts.size(); i++) { iAnts[i]->Reset(); ReadFile(); iAnts[i]->SetStop(STOP); STOP += 10; if(robots <= N_robots) { iAnts[i]->GetPattern(robotPattern[robots]); robots++; } } }
void CPFA_loop_functions::Init(argos::TConfigurationNode &node) { argos::CDegrees USV_InDegrees; argos::TConfigurationNode CPFA_node = argos::GetNode(node, "CPFA"); argos::GetNodeAttribute(CPFA_node, "ProbabilityOfSwitchingToSearching", ProbabilityOfSwitchingToSearching); argos::GetNodeAttribute(CPFA_node, "ProbabilityOfReturningToNest", ProbabilityOfReturningToNest); argos::GetNodeAttribute(CPFA_node, "UninformedSearchVariation", USV_InDegrees); argos::GetNodeAttribute(CPFA_node, "RateOfInformedSearchDecay", RateOfInformedSearchDecay); argos::GetNodeAttribute(CPFA_node, "RateOfSiteFidelity", RateOfSiteFidelity); argos::GetNodeAttribute(CPFA_node, "RateOfLayingPheromone", RateOfLayingPheromone); argos::GetNodeAttribute(CPFA_node, "RateOfPheromoneDecay", RateOfPheromoneDecay); argos::GetNodeAttribute(CPFA_node, "PrintFinalScore", PrintFinalScore); UninformedSearchVariation = ToRadians(USV_InDegrees); /****************************************************************************************************************************/ argos::TConfigurationNode settings_node = argos::GetNode(node, "settings"); argos::GetNodeAttribute(settings_node, "MaxSimTimeInSeconds", MaxSimTime); MaxSimTime *= GetSimulator().GetPhysicsEngine("dyn2d").GetInverseSimulationClockTick(); argos::GetNodeAttribute(settings_node, "MaxSimCounter", MaxSimCounter); argos::GetNodeAttribute(settings_node, "VariableFoodPlacement", VariableFoodPlacement); argos::GetNodeAttribute(settings_node, "OutputData", OutputData); argos::GetNodeAttribute(settings_node, "DrawIDs", DrawIDs); argos::GetNodeAttribute(settings_node, "DrawTrails", DrawTrails); argos::GetNodeAttribute(settings_node, "DrawTargetRays", DrawTargetRays); argos::GetNodeAttribute(settings_node, "FoodDistribution", FoodDistribution); argos::GetNodeAttribute(settings_node, "FoodItemCount", FoodItemCount); argos::GetNodeAttribute(settings_node, "NumberOfClusters", NumberOfClusters); argos::GetNodeAttribute(settings_node, "ClusterWidthX", ClusterWidthX); argos::GetNodeAttribute(settings_node, "ClusterLengthY", ClusterLengthY); argos::GetNodeAttribute(settings_node, "PowerRank", PowerRank); argos::GetNodeAttribute(settings_node, "FoodRadius", FoodRadius); argos::GetNodeAttribute(settings_node, "NestElevation", NestElevation); FoodRadiusSquared = FoodRadius*FoodRadius; // calculate the forage range and compensate for the robot's radius of 0.085m argos::CVector3 ArenaSize = GetSpace().GetArenaSize(); argos::Real rangeX = (ArenaSize.GetX() / 2.0) - 0.085; argos::Real rangeY = (ArenaSize.GetY() / 2.0) - 0.085; ForageRangeX.Set(-rangeX, rangeX); ForageRangeY.Set(-rangeY, rangeY); // Send a pointer to this loop functions object to each controller. argos::CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot"); argos::CSpace::TMapPerType::iterator it; for(it = footbots.begin(); it != footbots.end(); it++) { argos::CFootBotEntity& footBot = *argos::any_cast<argos::CFootBotEntity*>(it->second); BaseController& c = dynamic_cast<BaseController&>(footBot.GetControllableEntity().GetController()); CPFA_controller& c2 = dynamic_cast<CPFA_controller&>(c); c2.SetLoopFunctions(this); } SetFoodDistribution(); }
bool CLUTConverter::SetLUT(const string &s, bool isInputCurrent) { _isInputCurrent = isInputCurrent; ifstream inputFile(string(GetSimulator()->GetPathPrefix() + s).c_str()); if (!inputFile) { return false; } _pLUT = new CLookUpTable(3, inputFile, false); return true; }
/***** * Required by ARGoS. This function initializes global variables using * values from the XML configuration file supplied when ARGoS is run. *****/ void DSA_loop_functions::Init(TConfigurationNode& node) { /* Temporary variables. */ CSimulator *simulator = &GetSimulator(); CPhysicsEngine *physicsEngine = &simulator->GetPhysicsEngine("default"); CVector3 ArenaSize = GetSpace().GetArenaSize(); CVector2 rangeX = CVector2(-ArenaSize.GetX()/2.0, ArenaSize.GetX()/2.0); CVector2 rangeY = CVector2(-ArenaSize.GetY()/2.0, ArenaSize.GetY()/2.0); CDegrees USV_InDegrees; /* Get each tag in the loop functions section of the XML file. */ TConfigurationNode simNode = GetNode(node, "simulation"); TConfigurationNode random = GetNode(node, "_0_FoodDistribution_Random"); TConfigurationNode cluster = GetNode(node, "_1_FoodDistribution_Cluster"); TConfigurationNode powerLaw = GetNode(node, "_2_FoodDistribution_PowerLaw"); GetNodeAttribute(simNode, "MaxSimCounter", MaxSimCounter); GetNodeAttribute(simNode, "VariableSeed", VariableSeed); GetNodeAttribute(simNode, "OutputData", OutputData); GetNodeAttribute(simNode, "NestPosition", NestPosition); GetNodeAttribute(simNode, "NestRadius", NestRadius); GetNodeAttribute(simNode, "FoodRadius", FoodRadius); GetNodeAttribute(simNode, "FoodDistribution", FoodDistribution); GetNodeAttribute(random, "FoodItemCount", FoodItemCount); GetNodeAttribute(cluster, "NumberOfClusters", NumberOfClusters); GetNodeAttribute(cluster, "ClusterWidthX", ClusterWidthX); GetNodeAttribute(cluster, "ClusterLengthY", ClusterLengthY); GetNodeAttribute(powerLaw, "PowerRank", PowerRank); /* Convert and calculate additional values. */ NestRadiusSquared = (NestRadius) * (NestRadius); FoodRadiusSquared = (FoodRadius + 0.04) * (FoodRadius + 0.04); ForageRangeX.Set(rangeX.GetX() + (2.0 * FoodRadius), rangeX.GetY() - (2.0 * FoodRadius)); ForageRangeY.Set(rangeY.GetX() + (2.0 * FoodRadius), rangeY.GetY() - (2.0 * FoodRadius)); RNG = CRandom::CreateRNG("argos"); /* Store the iAnts in a more friendly, human-readable structure. */ CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot"); CSpace::TMapPerType::iterator it; ReadFile(); size_t STOP = 0; size_t robots = 0; for(it = footbots.begin(); it != footbots.end(); it++) { CFootBotEntity& footBot = *any_cast<CFootBotEntity*>(it->second); DSA_controller& c = dynamic_cast<DSA_controller&>(footBot.GetControllableEntity().GetController()); DSAnts.push_back(&c); c.SetData(&data); c.SetStop(STOP); STOP += 10; /* adds 10 seconds extra pause for each robot */ if(robots <= N_robots) { c.GetPattern(robotPattern[robots]); robots++; //attempt to add mutiple colors for each robot // if(robots==2) // { // DrawTargetRays = 2; // } // else if(robots ==3) // { // DrawTargetRays = 3; // } } } /* Set up the food distribution based on the XML file. */ SetFoodDistribution(); }
argos::Real CPFA_loop_functions::getSimTimeInSeconds() { int ticks_per_second = GetSimulator().GetPhysicsEngine("Default").GetInverseSimulationClockTick(); float sim_time = GetSpace().GetSimulationClock(); return sim_time/ticks_per_second; }
void RbFirmataController::ProcessIO() { try { m_bIOThreadProcessing = true; std::cout << "Sending firmware version request\r\n"; //First lets flush the buffer. _port.flush(); //First reset firmata sendReset(); boost::this_thread::sleep(boost::posix_time::microseconds(10)); sendFirmwareVersionRequest(); //Loop through to do the innitial setup int iSendFirmwareCount=0; while(!(m_bStopIO || m_bSetupComplete)) { if(!_firmwareReceived) { update(); //if(iSendFirmwareCount <= 0 && !_firmwareReceived) //{ // //Then need to do this to init the pins, get the firmware version, and call setupArduino. // //Will stay in update loop looking for signal. When it arrives Setup will be called // //and we can start processing. // iSendFirmwareCount = 1000; //} //iSendFirmwareCount--; boost::this_thread::sleep(boost::posix_time::microseconds(300)); } } //Start the timer to measure motor send timing. m_lMotorSendStart = GetSimulator()->GetTimerTick(); //Now that setup has compled lets do our main loop while(!m_bStopIO) { if(m_bPauseIO || m_lpSim->Paused()) { m_bIOPaused = true; boost::this_thread::sleep(boost::posix_time::microseconds(1000)); } else { m_bIOPaused = false; //Update the firmata IO. update(); //Do not try and step IO until it has been setup correctly. StepIO(); if(_dynamixelMoveAdds > 0) { //Get the motor send time m_fltMotorSendTime = GetSimulator()->TimerDiff_m(m_lMotorSendStart, GetSimulator()->GetTimerTick()); //Execute any synch moves that were setup for this IO loop in StepIO //If none were setup it will ignore this call. sendDynamixelSynchMoveExecute(); //Star the timer again. m_lMotorSendStart = GetSimulator()->GetTimerTick(); } } } } catch(CStdErrorInfo oError) { m_bIOThreadProcessing = false; } catch(...) { m_bIOThreadProcessing = false; } m_bIOThreadProcessing = false; }