Organized my code better. Worked more on multirobot navigation