//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; } }