Add new functions, follow line properly and distance from objects