/***** * Helper function that reads vector <char> pattern * and sets the target's direction base on the * char at the current vector index. *****/ void iAnt_controller::GetTargets() { /* Finds the last direction of the pattern. */ char direction_last = tempPattern[tempPattern.size() - 1]; /* If the robot hit target and the patter size >0 then find the next direction. */ if(TargetHit() == true && tempPattern.size() > 0) { tempPattern.pop_back(); switch(direction_last) { case 'N': SetTargetN('N'); break; case 'S': SetTargetS('S'); break; case 'E': SetTargetE('E'); break; case 'W': SetTargetW('W'); break; default: motorActuator->SetLinearVelocity(0.0, 0.0); } } /* If the robot is down traversing the tempPattern, then return home */ else if(tempPattern.size() == 0) { ApproachTheTarget(data->NestPosition); Reset(); /* Otherwise we continue to approach the target. */ } else ApproachTheTarget(); }
/***** * 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); } } } }