Changed distance function in path_planning
Navigation module: computation from cm to dm to increase field race
Changed insertion of grid line, FIFO system