int Scheduler::getNext()
{
int min_time = Q;
int index = -1;
int task_time = -1;
for(int i=0; i<tasks.size(); i++)
{
if(tasks[i].state == RUNNING)
{
task_time = tasks[i].getTime( Q );
if(min_time >= task_time)
{
index =i;
min_time = task_time;
}
}
}
if( task_time != -1 && index == -1 )
index = goAround();
return index;
}
//gets user input and starts main control loop
void AI::start()
{
if( TESTING ) cout << "Getting coordinate inputs from user." << endl;
char buffer[256];
//get x coordinate
printf ("\n\nEnter X coord: ");
if(TESTING)
rc.target_x = 4000;
else
{
fgets (buffer, 256, stdin);
rc.target_x = atof(buffer);
}
//get y coordinate
printf ("\n\nEnter Y coord: ");
if(TESTING)
rc.target_y = 0;
else
{
fgets (buffer, 256, stdin);
rc.target_y = atof(buffer);
}
//turn toward destination
if( TESTING ) cout << "Turning towards destination" << endl;
TurnTowardsTarget();
//main loop, loops until destination is reached
while(true)
{
//checking if destination is reached, will exit program if so
if( TESTING ) cout << "Checking if destination has been reached" << endl;
DestinationCheck();
//manuever towards destination
if(dm.detectCloseFront())
{
//need to go around an obstacle
cout << "Obstacle detected in front" << endl;
if(!dm.detectSide(false))
{
//left side clear, go around left
cout << "Left side is clear, going around to the left" << endl;
goAround(false);
}
else
{
//left side obstructed, go around right
cout << "Left side is obstructed, going around to the right" << endl;
goAround(true);
}
//finished going around, turn back toward destination
cout << "Finished manuevering around obstacle, turning toward destination" << endl;
TurnTowardsTarget();
}
else
{
//front is clear, move foward
if( TESTING ) cout << "Front is clear, moving forward" << endl;
rc.moveForward(100);
}
}
if( TESTING ) cout << "Got outside of main loop" << endl;
}
//turns perpendicular to obstacle, drives until the obstacle is no longer in the way, then turns back
void AI::goAround(boolean right)
{
if( TESTING ) cout << "Beginning go around instruction" << endl;
//turn toward the direction passed in
rc.moveTurn(right);
//drive until obstacle no longer in the way
int i = 0;
while(true)
{
if( TESTING ) cout << "Driving until obstacle no longer in the way" << endl;
i++;
//check if destination was reached
DestinationCheck();
//check if obstacle in front
if(dm.detectCloseFront())
{
//check what side to try and go around
cout << "Obstacle detected in front, going around" << endl;
if(!dm.detectSide(!right))
{
//side clear, goaround that way
cout << "side clear, going around" << endl;
goAround(!right);
}
else
{
//side clear, goaround that way
cout << "Left side obstructed, going around right" << endl;
goAround(right);
}
//reset i so that we don't check for the side being clear before we move a decent distance
i = 0;
}
else
{
//move forward
if( TESTING ) cout << "Front is clear, moving forward" << endl;
rc.moveForward(100);
}
//if we've travelled a decent amount, start checking if side is clear
if(i > 2)
{
//check if side clear
cout << "Checking if side is clear" << endl;
if(!right && !dm.detectSide(true))
{
cout << "Right side clear, moving slightly further then turning back" << endl;
//move a bit then turn back toward where obstacle was blocking
rc.moveForward(140);
rc.moveTurn(true);
return;
}
else if( right && !dm.detectSide(false))
{
cout << "Left side clear, moving slightly further then turning back" << endl;
//move a bit then turn back toward where obstacle was blocking
rc.moveForward(140);
rc.moveTurn(false);
return;
}
}
}
if( TESTING ) cout << "Got outside of go around loop" << endl;
}
