/*****
* 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);
}
}
}
void ofxSpriteRenderer::FocusCamera(ofVec2f position)
{
ofVec3f position3d(position.x, 0.0f, position.y);
m_Camera->setGlobalPosition(position3d + ofVec3f(0.0f, 1.0f, 1.0f));
m_Camera->lookAt(position3d);
m_CameraForce = true;
}
/*****
* 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);
}
}
}
}
void iAnt_controller::ControlStep() {
LOG << GetHeading() << endl << '\n';
/////////ADDED///////////////
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);
//[ + + ] move foward
//motorActuator->SetLinearVelocity(RobotForwardSpeed, RobotForwardSpeed);
//[ + - ] rotates to the right
//motorActuator->SetLinearVelocity(RobotForwardSpeed, -RobotForwardSpeed);
//[ + 0] spins left
//motorActuator->SetLinearVelocity(RobotForwardSpeed, 0.0);
//[ 0 +] spins right
//motorActuator->SetLinearVelocity(RobotForwardSpeed, 0.0);
east();
//west();
//north();
//south();
//char* pat = LinearSpiral();
//out << "pat" << *pat << '\n';
//free(pat);
//checkDistanceX();
// if(west() == true){
// motorActuator->SetLinearVelocity(stopMoving(checkDistanceX()), stopMoving(checkDistanceX()));
// //motorActuator->SetLinearVelocity(RobotForwardSpeed,RobotForwardSpeed);
// }
// if(west() == true) {
// motorActuator->SetLinearVelocity(RobotForwardSpeed,RobotForwardSpeed);
// }
// if(east() == true){
// motorActuator->SetLinearVelocity(RobotForwardSpeed, RobotForwardSpeed);
// }
//UNCONMMENT
bool stop=checkDistanceY();
cout << "stop: " << stop << '\n';
stopMoving(stop);
motorActuator->SetLinearVelocity(stopMoving(stop), stopMoving(stop));
if (stop)
{
east();
motorActuator->SetLinearVelocity(0.0,RobotForwardSpeed);
if(east() == true) {
motorActuator->SetLinearVelocity(RobotForwardSpeed,RobotForwardSpeed);
}
stop = false;
}
//motorActuator->SetLinearVelocity(stopMoving(stop), stopMoving(stop));
//north();
// if(north() == true) {
// //motorActuator->SetLinearVelocity(RobotForwardSpeed, RobotForwardSpeed);
// motorActuator->SetLinearVelocity(stopMoving(checkDistanceY()), stopMoving(checkDistanceY()));
// }
// if(south() == true) {
// checkDistanceY();
// motorActuator->SetLinearVelocity(RobotForwardSpeed, RobotForwardSpeed);
// }
}
