Added Localization, updated path finder (navigation with avoidance),
dealt with different starting corners and team color.