Refactor and Eliminate the warning
Finish Exploration Algorithm
Refactor so that the robot only explores around when there exists
surrounding unexplored block
Debug the controller