Ejemplo n.º 1
0
//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;
	}
}