/***** * Primary control loop for this controller object. This function will execute * the CPFA logic using the CPFA enumeration flag once per frame. *****/ void iAnt_controller::ControlStep() { if(Stop()) { return; } if(isHoldingFood == false) { /* draws target rays every 2 seconds */ if((data->DrawTargetRays < 4) && (data->SimTime % (data->TicksPerSecond * 2)) == 0 ) { CVector3 position3d(GetPosition().GetX(), GetPosition().GetY(), 0.02); CVector3 target3d(GetTarget().GetX(), GetTarget().GetY(), 0.02); CRay3 targetRay(target3d, position3d); data->TargetRayList.push_back(targetRay); } } /* Checks if the robot found a food */ SetHoldingFood(); /* If it didn't continue in a sprial */ if(IsHoldingFood() == false) { GetTargets(); /* Initializes targets positions. */ } else { /* Check if it is near the nest then set isHoldingFood to false */ if((GetPosition() - data->NestPosition).SquareLength() < data->NestRadiusSquared) { isHoldingFood = false; } else { ApproachTheTarget(data->NestPosition); } } }
/***** * Primary control loop for this controller object. This function will execute * the CPFA logic using the CPFA enumeration flag once per frame. *****/ void DSA_controller::ControlStep() { if(IsHoldingFood() == false && DSA == SEARCHING) { /* draws target rays every 2 seconds */ if((loopFunctions.SimTime % (loopFunctions.TicksPerSecond)) == 0) { CVector3 position3d(GetPosition().GetX(), GetPosition().GetY(), 0.02); CVector3 target3d(GetTarget().GetX(), GetTarget().GetY(), 0.02); CRay3 targetRay(target3d, position3d); myTrail.push_back(targetRay); // loopFunctions.TargetRayList.push_back(myTrail); loopFunctions.TargetRayList.insert(loopFunctions.TargetRayList.end(), myTrail.begin(), myTrail.end()); } } if(Stop() == true) { return;//motorActuator->SetLinearVelocity(0.0, 0.0); } else { /* Checks if the robot found a food */ SetHoldingFood(); /* If it didn't continue in a sprial */ if(IsHoldingFood() == false) { GetTargets(); /* Initializes targets positions. */ } else { /* Check if it is near the nest then set isHoldingFood to false */ DSA = RETURNING; if((GetPosition() - loopFunctions.NestPosition).SquareLength() < loopFunctions.NestRadiusSquared) { isHoldingFood = false; } else { ApproachTheTarget(loopFunctions.NestPosition); } } } }
/***** * Check if the iAnt is finding food. This is defined as the iAnt being within * the distance tolerance of the position of a food item. If the iAnt has found * food then the appropriate boolean flags are triggered. *****/ void iAnt_controller::SetHoldingFood() { /* Is the iAnt already holding food? */ if(IsHoldingFood() == false) { vector <CVector2> newFoodList; size_t i = 0; /* No, the iAnt isn't holding food. Check if we have found food at our current position and update the food list if we have. */ for (i = 0; i < data->FoodList.size(); i++) { /* We found food! */ if ((GetPosition()-data->FoodList[i]).SquareLength() < data->FoodRadiusSquared) { isHoldingFood = true; //goingHome = true; } /* Else push the that current food onto the newFoodList. */ else { /* Return this unfound-food position to the list */ newFoodList.push_back(data->FoodList[i]); } } data->FoodList = newFoodList; } }