/*****
* 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;
}
}
