void CFootBotForaging::ReturnToNest() {
/* As soon as you get to the nest, switch to 'resting' */
UpdateState();
/* Are we in the nest? */
if(m_sStateData.InNest) {
/* Have we looked for a place long enough? */
if(m_sStateData.TimeSearchingForPlaceInNest > m_sStateData.MinimumSearchForPlaceInNestTime) {
/* Yes, stop the wheels... */
m_pcWheels->SetLinearVelocity(0.0f, 0.0f);
/* Tell people about the last exploration attempt */
m_pcRABA->SetData(0, m_eLastExplorationResult);
/* ... and switch to state 'resting' */
m_pcLEDs->SetAllColors(CColor::RED);
m_sStateData.State = SStateData::STATE_RESTING;
m_sStateData.TimeSearchingForPlaceInNest = 0;
m_eLastExplorationResult = LAST_EXPLORATION_NONE;
return;
}
else {
/* No, keep looking */
++m_sStateData.TimeSearchingForPlaceInNest;
}
}
else {
/* Still outside the nest */
m_sStateData.TimeSearchingForPlaceInNest = 0;
}
/* Keep going */
bool bCollision;
SetWheelSpeedsFromVector(
m_sWheelTurningParams.MaxSpeed * DiffusionVector(bCollision) +
m_sWheelTurningParams.MaxSpeed * CalculateVectorToLight());
}
void CFootBotForagingNR::Explore() {
// This should be implemented differently: check whether we are on a
// food item, and if so, pick it up.
if (IsOverFoodItem() && !IsCarryingFood()) {
PickUpItem();
}
else {
bool collision;
CVector2 diffusion = DiffusionVector(collision);
SetWheelSpeedsFromVector(WheelTurningParams.MaxSpeed * diffusion);
}
}
void CFootBotFlocking::ControlStep() {
SetWheelSpeedsFromVector(VectorToLight() + FlockingVector());
}
void CFootBotForaging::Explore() {
/* We switch to 'return to nest' in two situations:
* 1. if we have a food item
* 2. if we have not found a food item for some time;
* in this case, the switch is probabilistic
*/
bool bReturnToNest(false);
/*
* Test the first condition: have we found a food item?
* NOTE: the food data is updated by the loop functions, so
* here we just need to read it
*/
if(m_sFoodData.HasFoodItem) {
/* Apply the food rule, decreasing ExploreToRestProb and increasing
* RestToExploreProb */
m_sStateData.ExploreToRestProb -= m_sStateData.FoodRuleExploreToRestDeltaProb;
m_sStateData.ProbRange.TruncValue(m_sStateData.ExploreToRestProb);
m_sStateData.RestToExploreProb += m_sStateData.FoodRuleRestToExploreDeltaProb;
m_sStateData.ProbRange.TruncValue(m_sStateData.RestToExploreProb);
/* Store the result of the expedition */
m_eLastExplorationResult = LAST_EXPLORATION_SUCCESSFUL;
/* Switch to 'return to nest' */
bReturnToNest = true;
}
/* Test the second condition: we probabilistically switch to 'return to
* nest' if we have been wandering for some time and found nothing */
else if(m_sStateData.TimeExploringUnsuccessfully > m_sStateData.MinimumUnsuccessfulExploreTime) {
if (m_pcRNG->Uniform(m_sStateData.ProbRange) < m_sStateData.ExploreToRestProb) {
/* Store the result of the expedition */
m_eLastExplorationResult = LAST_EXPLORATION_UNSUCCESSFUL;
/* Switch to 'return to nest' */
bReturnToNest = true;
}
else {
/* Apply the food rule, increasing ExploreToRestProb and
* decreasing RestToExploreProb */
m_sStateData.ExploreToRestProb += m_sStateData.FoodRuleExploreToRestDeltaProb;
m_sStateData.ProbRange.TruncValue(m_sStateData.ExploreToRestProb);
m_sStateData.RestToExploreProb -= m_sStateData.FoodRuleRestToExploreDeltaProb;
m_sStateData.ProbRange.TruncValue(m_sStateData.RestToExploreProb);
}
}
/* So, do we return to the nest now? */
if(bReturnToNest) {
/* Yes, we do! */
m_sStateData.TimeExploringUnsuccessfully = 0;
m_sStateData.TimeSearchingForPlaceInNest = 0;
m_pcLEDs->SetAllColors(CColor::BLUE);
m_sStateData.State = SStateData::STATE_RETURN_TO_NEST;
}
else {
/* No, perform the actual exploration */
++m_sStateData.TimeExploringUnsuccessfully;
UpdateState();
/* Get the diffusion vector to perform obstacle avoidance */
bool bCollision;
CVector2 cDiffusion = DiffusionVector(bCollision);
/* Apply the collision rule, if a collision avoidance happened */
if(bCollision) {
/* Collision avoidance happened, increase ExploreToRestProb and
* decrease RestToExploreProb */
m_sStateData.ExploreToRestProb += m_sStateData.CollisionRuleExploreToRestDeltaProb;
m_sStateData.ProbRange.TruncValue(m_sStateData.ExploreToRestProb);
m_sStateData.RestToExploreProb -= m_sStateData.CollisionRuleExploreToRestDeltaProb;
m_sStateData.ProbRange.TruncValue(m_sStateData.RestToExploreProb);
}
/*
* If we are in the nest, we combine antiphototaxis with obstacle
* avoidance
* Outside the nest, we just use the diffusion vector
*/
if(m_sStateData.InNest) {
/*
* The vector returned by CalculateVectorToLight() points to
* the light. Thus, the minus sign is because we want to go away
* from the light.
*/
SetWheelSpeedsFromVector(
m_sWheelTurningParams.MaxSpeed * cDiffusion -
m_sWheelTurningParams.MaxSpeed * 0.25f * CalculateVectorToLight());
}
else {
/* Use the diffusion vector only */
SetWheelSpeedsFromVector(m_sWheelTurningParams.MaxSpeed * cDiffusion);
}
}
}
