//Uses A* search to find optimal sequence of actions to go from current location to desired location
//Once sequence is found, the actions are executed.
//Returns:
// ACT_GOING: still executing sequence of actions
// ACT_SUCCESS: finished executing sequence of actions
// ACT_ERROR: search provided no solution to the problem
// other: Flags to indicate something went wrong
ActionResult Brain::GoToLocation(byte end_x, byte end_y, int desired_direction /*= -1*/)
{
static bool is_searching = true;
static ActionList action_list;
//Perform the search and convert it to a usable list
if(is_searching)
{
byte_action_list byte_actions = SearchAlgorithm::AStarGoAToB(end_x, end_y, robot_state_, board_state_, desired_direction);
for(byte_action action : byte_actions)
{
SearchAlgorithm::PrintByteActionString(action);
}
action_list = ByteActionListConverter(byte_actions);
is_searching = false;
//If the action list is empty after search has been completed, no good!
//This means our search found no solution; return error
if(action_list.IsEmpty())
{
Serial.println(end_x);
Serial.println(end_y);
robot_state_.Print();
board_state_.Print();
is_searching = true; //reset search for next time
return ACT_ERROR;
}
}
//If the list is empty, we've completed our execution of the actions
if(action_list.IsEmpty())
{
is_searching = true; //reset search for next time
return ACT_SUCCESS;
}
//Get current action and execute it
Action* curr_action = action_list.GetCurrentAction();
// curr_action->Print();
ActionResult result = curr_action->Run();
//From the result of the action, return things to the calling function
switch(result)
{
case ACT_GOING: //Continue on this action
return ACT_GOING;
break;
case ACT_SUCCESS: //Good! Action completed, Move on to the next step
action_list.MoveToNextAction();
return ACT_GOING;
break;
default: //Error; return error flags and let calling function deal with it. Do not continue with these actions
is_searching = true; //reset search for next time
return result;
break;
}
}
