Added path planning V3 (called main_driver_5)